How to use

AngleTools

in

us.ihmc.robotics.geometry

public static double roundToGivenPrecisionForAngle(double angleValue, double precisionFactor)
{
 double centeredAngleValue = trimAngleMinusPiToPi(angleValue + 0.5 * precisionFactor);
 long longValue = (long) (centeredAngleValue / precisionFactor);
 double roundedValue = ((double) longValue) * precisionFactor;
 return trimAngleMinusPiToPi(roundedValue);
}

public static double interpolateAngle(double angleA, double angleB, double alpha)
{
 // A + alpha * (B-A)/2
 double average = angleA + alpha * AngleTools.computeAngleDifferenceMinusPiToPi(angleB, angleA);
 return trimAngleMinusPiToPi(average);
}

private static List<OverheadPath> calculatePaths(FramePose2d startPose, FramePoint2d endPoint, double headingOffset, double noTranslationTolerance)
{
 startPose.checkReferenceFrameMatch(endPoint);
 double heading = AngleTools.calculateHeading(startPose, endPoint, headingOffset, noTranslationTolerance);
 FrameOrientation2d intermediateOrientation = new FrameOrientation2d(startPose.getReferenceFrame(), heading);
 TurningOverheadPath turningPath = new TurningOverheadPath(startPose, intermediateOrientation);
 FramePose2d intermediatePose = turningPath.getPoseAtS(1.0);
 StraightLineOverheadPath straightPath = new StraightLineOverheadPath(intermediatePose, endPoint);
 List<OverheadPath> paths = new ArrayList<OverheadPath>();
 paths.add(turningPath);
 paths.add(straightPath);
 return paths;
}

int ret = -1;
double difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 0.0);
difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI / 2.0);
difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI);
difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 3.0 * Math.PI / 2.0);

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);

interiorAnglesAtZeroConfiguration[0] = Math.PI - AngleTools.angleMinusPiToPi(vectorABProjected, vectorDAProjected);
interiorAnglesAtZeroConfiguration[1] = Math.PI - AngleTools.angleMinusPiToPi(vectorBCProjected, vectorABProjected);
interiorAnglesAtZeroConfiguration[2] = Math.PI - AngleTools.angleMinusPiToPi(vectorCDProjected, vectorBCProjected);
interiorAnglesAtZeroConfiguration[3] = Math.PI;
interiorAnglesAtZeroConfiguration[0] = Math.PI - AngleTools.angleMinusPiToPi(vectorDAProjected, vectorABProjected);
interiorAnglesAtZeroConfiguration[1] = Math.PI - AngleTools.angleMinusPiToPi(vectorABProjected, vectorBCProjected);
interiorAnglesAtZeroConfiguration[2] = Math.PI - AngleTools.angleMinusPiToPi(vectorBCProjected, vectorCDProjected);
interiorAnglesAtZeroConfiguration[3] = Math.PI;

int ret = -1;
double difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 0.0);
difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI / 2.0);
difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI);
difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 3.0 * Math.PI / 2.0);

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;
}

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testAngleMinusPiToPi()
{
 Vector2D vectorA, vectorB;
 double expected, actual;
 vectorA = new Vector2D(0.0, 1.0);
 vectorB = new Vector2D(1.0, 0.0);
 expected = -0.5 * Math.PI;
 actual = AngleTools.angleMinusPiToPi(vectorA, vectorB);
 assertEquals(expected, actual, 1e-12);
 vectorA = new Vector2D(0.0, 1.0);
 vectorB = new Vector2D(-1.0, 0.0);
 expected = 0.5 * Math.PI;
 actual = AngleTools.angleMinusPiToPi(vectorA, vectorB);
 assertEquals(expected, actual, 1e-12);
 vectorA = new Vector2D(1.0, 1.0);
 vectorB = new Vector2D(-1.0, 0.0);
 expected = 0.75 * Math.PI;
 actual = AngleTools.angleMinusPiToPi(vectorA, vectorB);
 assertEquals(expected, actual, 1e-12);
 vectorA = new Vector2D(0.0, 1.0);
 vectorB = new Vector2D(0.0, 0.0);
 expected = AngleTools.angleMinusPiToPi(vectorA, vectorB);
 assertTrue(Double.isNaN(expected));
}

public static double roundToGivenPrecisionForAngle(double angleValue, double precisionFactor)
{
 double centeredAngleValue = AngleTools.trimAngleMinusPiToPi(angleValue + 0.5 * precisionFactor);
 long longValue = (long) (centeredAngleValue / precisionFactor);
 double roundedValue = ((double) longValue) * precisionFactor;
 return AngleTools.trimAngleMinusPiToPi(roundedValue);
}

public static double computeAngleAverage(double angleA, double angleB)
{
 // average = A + (B-A)/2 = (A+B)/2
 double average = angleA + 0.5 * AngleTools.computeAngleDifferenceMinusPiToPi(angleB, angleA);
 return trimAngleMinusPiToPi(average);
}

private void assertFootstepInAngleRange(String testDescription, Footstep footstep, double startYaw, double endYaw)
{
 FramePose3D footPose = new FramePose3D();
 footstep.getPose(footPose);
 FrameOrientation2D footOrientation = new FrameOrientation2D(footPose.getOrientation());
 footOrientation.changeFrame(WORLD_FRAME);
 double footYaw = footOrientation.getYaw();
 double angleBetweenStartAndEnd = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(endYaw, startYaw));
 double angleBetweenStartAndFoot = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(footYaw, startYaw));
 double angleBetweenFootAndEnd = Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(endYaw, footYaw));
 //if start->foot + foot-> end > start->end, then the foot angle is outside the range between the two
 boolean outsideFan = (angleBetweenStartAndFoot + angleBetweenFootAndEnd) > angleBetweenStartAndEnd + 1e-14;
}

private static List<OverheadPath> calculatePaths(FramePose2D startPose, FramePoint2D endPoint, double headingOffset, double noTranslationTolerance)
{
 startPose.checkReferenceFrameMatch(endPoint);
 double heading = AngleTools.calculateHeading(startPose, endPoint, headingOffset, noTranslationTolerance);
 FrameOrientation2D intermediateOrientation = new FrameOrientation2D(startPose.getReferenceFrame(), heading);
 TurningOverheadPath turningPath = new TurningOverheadPath(startPose, intermediateOrientation);
 FramePose2D intermediatePose = turningPath.getPoseAtS(1.0);
 StraightLineOverheadPath straightPath = new StraightLineOverheadPath(intermediatePose, endPoint);
 List<OverheadPath> paths = new ArrayList<OverheadPath>();
 paths.add(turningPath);
 paths.add(straightPath);
 return paths;
}

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);
}

interiorAnglesAtZeroConfiguration[0] = Math.PI - AngleTools.angleMinusPiToPi(tempVectorAB, tempVectorDA);
interiorAnglesAtZeroConfiguration[1] = Math.PI - AngleTools.angleMinusPiToPi(tempVectorBC, tempVectorAB);
interiorAnglesAtZeroConfiguration[2] = Math.PI - AngleTools.angleMinusPiToPi(tempVectorCD, tempVectorBC);
interiorAnglesAtZeroConfiguration[3] = Math.PI;
interiorAnglesAtZeroConfiguration[0] = Math.PI - AngleTools.angleMinusPiToPi(tempVectorDA, tempVectorAB);
interiorAnglesAtZeroConfiguration[1] = Math.PI - AngleTools.angleMinusPiToPi(tempVectorAB, tempVectorBC);
interiorAnglesAtZeroConfiguration[2] = Math.PI - AngleTools.angleMinusPiToPi(tempVectorBC, tempVectorCD);
interiorAnglesAtZeroConfiguration[3] = Math.PI;

public double indexToAngle(int index)
{
 double doubleIndex = (double) index;
 double doubleSectors = (double) sectors;
 double sectorsFraction = doubleIndex / doubleSectors;
 return AngleTools.trimAngleMinusPiToPi(sectorsFraction * Math.PI * 2.0);
}

public void extrapolate(FrameOrientation2d orientationOne, FrameOrientation2d orientationTwo, double alpha)
{
 orientationOne.checkReferenceFrameMatch(orientationTwo);
 double deltaYaw = AngleTools.computeAngleDifferenceMinusPiToPi(orientationTwo.yaw, orientationOne.yaw);
 this.yaw = AngleTools.trimAngleMinusPiToPi(orientationOne.yaw + alpha * deltaYaw);
 this.referenceFrame = orientationOne.getReferenceFrame();
}

public void update(double input)
{
 if (hasBeenUpdated)
 {
   output = (AngleTools.computeAngleDifferenceMinusPiToPi(input, previous.getDoubleValue())) / dt;
   previous.set(input);
 }
 else
 {
   reset(input);
 }
 hasBeenUpdated = true;
}

private void addTurnInPlaceToFacePoint(ArrayList<FramePose2D> footstepList, FramePose2D robotPose, FramePoint2D goalPoint)
{
 double turningAngle = AngleTools.calculateHeading(robotPose, goalPoint, -robotPose.getYaw(), 0.0);
 FramePoint2D pointToTurnAbout = new FramePoint2D(robotPose.getPosition());
 addTurnInPlace(footstepList, turningAngle, pointToTurnAbout);
}

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);
}