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