angleB = 2 * Math.PI + 0.3; expected = (Math.PI + 0.3) / 2.0; actual = AngleTools.computeAngleAverage(angleA, angleB); assertEquals(expected, actual, 1e-12); angleA = 2 * Math.PI + 0.3; expected = (Math.PI + 0.3) / 2.0; actual = AngleTools.computeAngleAverage(angleA, angleB); assertEquals(expected, actual, 1e-12); angleB = Math.PI; expected = (Math.PI + 0.3) / 2.0; actual = AngleTools.computeAngleAverage(angleA, angleB); assertEquals(expected, actual, 1e-12); angleB = -Math.PI + 0.1; expected = Math.PI - 0.1; actual = AngleTools.computeAngleAverage(angleA, angleB); assertEquals(expected, actual, 1e-12); angleA = -Math.PI + 0.1; expected = Math.PI - 0.1; actual = AngleTools.computeAngleAverage(angleA, angleB); assertEquals(expected, actual, 1e-12);
private boolean findMidStanceFrame(FootstepNode node, FootstepNode previousNode) { Point3D midPoint = getMidPoint(node, previousNode); if (midPoint == null) return false; double angleAverage = AngleTools.computeAngleAverage(node.getYaw(), previousNode.getYaw()); midPose.setPosition(midPoint); midPose.setOrientationYawPitchRoll(angleAverage, 0.0, 0.0); midStanceFrame.setPoseAndUpdate(midPose); bodyCollisionFrame.update(); return true; }
public void moveToAverageInSupportFoot(RobotSide supportSide) { desiredPelvisOrientation.getFrameOrientationIncludingFrame(tempOrientation); initialPelvisOrientation.set(tempOrientation); ReferenceFrame otherAnkleZUpFrame = ankleZUpFrames.get(supportSide.getOppositeSide()); ReferenceFrame supportAnkleZUpFrame = ankleZUpFrames.get(supportSide); tempOrientation.setToZero(otherAnkleZUpFrame); tempOrientation.changeFrame(worldFrame); double yawOtherFoot = tempOrientation.getYaw(); tempOrientation.setToZero(supportAnkleZUpFrame); tempOrientation.changeFrame(worldFrame); double yawSupportFoot = tempOrientation.getYaw(); double finalDesiredPelvisYawAngle = AngleTools.computeAngleAverage(yawOtherFoot, yawSupportFoot); finalPelvisOrientation.set(finalDesiredPelvisYawAngle, 0.0, 0.0); initialize(supportAnkleZUpFrame); }
public void setWithUpcomingFootstep(Footstep upcomingFootstep) { RobotSide upcomingFootstepSide = upcomingFootstep.getRobotSide(); desiredPelvisOrientation.getFrameOrientationIncludingFrame(tempOrientation); initialPelvisOrientation.set(tempOrientation); upcomingFootstep.getOrientationIncludingFrame(upcomingFootstepOrientation); upcomingFootstepOrientation.changeFrame(worldFrame); tempOrientation.setToZero(ankleZUpFrames.get(upcomingFootstepSide.getOppositeSide())); tempOrientation.changeFrame(worldFrame); double finalDesiredPelvisYawAngle = AngleTools.computeAngleAverage(upcomingFootstepOrientation.getYaw(), tempOrientation.getYaw()); upcomingFootstep.getPositionIncludingFrame(upcomingFootstepLocation); upcomingFootstepLocation.changeFrame(ankleZUpFrames.get(upcomingFootstepSide.getOppositeSide())); double desiredSwingPelvisYawAngle = 0.0; if (Math.abs(upcomingFootstepLocation.getX()) > 0.1) { desiredSwingPelvisYawAngle = Math.atan2(upcomingFootstepLocation.getY(), upcomingFootstepLocation.getX()); desiredSwingPelvisYawAngle -= upcomingFootstepSide.negateIfRightSide(Math.PI / 2.0); } swingPelvisYaw.set(desiredSwingPelvisYawAngle); finalPelvisOrientation.set(finalDesiredPelvisYawAngle + swingPelvisYawScale.getDoubleValue() * desiredSwingPelvisYawAngle, 0.0, 0.0); initialize(worldFrame); }
double[] randomAngles = RandomNumbers.nextDoubleArray(random, 100, Math.PI); AxisAngle expectedAverageAxisAngle = new AxisAngle(randomRotationAxis, AngleTools.computeAngleAverage(randomAngles)); Quaternion expectedAverageQuat = new Quaternion(); expectedAverageQuat.set(expectedAverageAxisAngle);