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); }
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; }
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; }
public void update(double input) { if (hasBeenUpdated) { output = (AngleTools.computeAngleDifferenceMinusPiToPi(input, previous.getDoubleValue())) / dt; previous.set(input); } else { reset(input); } hasBeenUpdated = true; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testOneOverSplusA() { double a = 3.0; double[] numerator = new double[] {1.0}; double[] denominator = new double[] {1.0, a}; TransferFunction transferFunction = new TransferFunction(numerator, denominator); double omega = 1.0; assertEquals(1.0 / Math.sqrt(omega * omega + a * a), transferFunction.getMagnitude(omega), 1e-7); assertEquals(0.0, AngleTools.computeAngleDifferenceMinusPiToPi(-Math.atan2(omega, a), transferFunction.getPhase(omega)), 1e-7); omega = 5.0; assertEquals(1.0 / Math.sqrt(omega * omega + a * a), transferFunction.getMagnitude(omega), 1e-7); assertEquals(0.0, AngleTools.computeAngleDifferenceMinusPiToPi(-Math.atan2(omega, a), transferFunction.getPhase(omega)), 1e-7); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeAngleDifferenceMinusPiToPi() { Random random = new Random(123456); double angleA = Math.PI; double angleB = Math.PI / 2; double expectedReturn = Math.PI / 2; double actualReturn = AngleTools.computeAngleDifferenceMinusPiToPi(angleA, angleB); assertEquals(expectedReturn, actualReturn, Double.MIN_VALUE); for (int i = 0; i < 25; i++) { angleA = RandomNumbers.nextDouble(random, -128.0, 128.0); angleB = RandomNumbers.nextDouble(random, -128.0, 128.0); double ret = AngleTools.computeAngleDifferenceMinusPiToPi(angleA, angleB); assertTrue(ret <= Math.PI); assertTrue(ret >= -Math.PI); } }
private void verify(TransferFunction transferFunction, double omega, double expectedMagnitude, double expectedPhase) { assertEquals(expectedMagnitude, transferFunction.getMagnitude(omega), 1e-7); assertEquals(0.0, AngleTools.computeAngleDifferenceMinusPiToPi(expectedPhase, transferFunction.getPhase(omega)), 1e-7); }
private void verify(TransferFunction transferFunction, double[] omega, double[] expectedMagnitude, double[] expectedPhase) { double[] magnitude = transferFunction.getMagnitude(omega); double[] phase = transferFunction.getPhase(omega); for (int i = 0; i < omega.length; i++) { assertEquals(expectedMagnitude[i], magnitude[i], 1e-7); assertEquals(0.0, AngleTools.computeAngleDifferenceMinusPiToPi(expectedPhase[i], phase[i]), 1e-7); } }
private void estimateYawDriftPerFoot() { if (numberOfFeetTrusted.getIntegerValue() != numberOfFeet) { for (int i = 0; i < numberOfFeet; i++) estimatedYawDriftPerFoot.get(allFeet.get(i)).set(Double.NaN); return; } for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); referenceAverageFootPosition.getFrameTupleIncludingFrame(averagePosition); referenceFootPositions.get(foot).getFrameTupleIncludingFrame(footPosition); referenceAverageToFootPosition.setToZero(worldFrame); referenceAverageToFootPosition.sub(footPosition, averagePosition); currentAverageFootPosition.getFrameTupleIncludingFrame(averagePosition); currentFootPositions.get(foot).getFrameTupleIncludingFrame(footPosition); currentAverageToFootPosition.setToZero(worldFrame); currentAverageToFootPosition.sub(footPosition, averagePosition); double refenceAngle = Math.atan2(referenceAverageToFootPosition.getY(), referenceAverageToFootPosition.getX()); double currentAngle = Math.atan2(currentAverageToFootPosition.getY(), currentAverageToFootPosition.getX()); estimatedYawDriftPerFoot.get(foot).set(AngleTools.computeAngleDifferenceMinusPiToPi(currentAngle, refenceAngle)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testOneOverS2() { double[] numerator = new double[] {1.0}; double[] denominator = new double[] {1.0, 0.0, 0.0}; TransferFunction transferFunction = new TransferFunction(numerator, denominator); assertEquals(1.0, transferFunction.getMagnitude(1.0), epsilon); assertEquals(0.0, AngleTools.computeAngleDifferenceMinusPiToPi(Math.PI, transferFunction.getPhase(1.0)), 1e-7); assertEquals(1.0 / 25.0, transferFunction.getMagnitude(5.0), 1e-7); assertEquals(0.0, AngleTools.computeAngleDifferenceMinusPiToPi(Math.PI, transferFunction.getPhase(5.0)), 1e-7); }
public void updateForAngles(double currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); lastPosition.set(currentPosition); set(0.0); } double difference = AngleTools.computeAngleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue()); updateUsingDifference(difference); lastPosition.set(currentPosition); }
@Override protected double computeHeuristics(FootstepNode node, FootstepNode goalNode) { Point2D goalPoint = goalNode.getOrComputeMidFootPoint(parameters.getIdealFootstepWidth()); Point2D nodeMidFootPoint = node.getOrComputeMidFootPoint(parameters.getIdealFootstepWidth()); double euclideanDistance = nodeMidFootPoint.distance(goalPoint); double yaw = AngleTools.computeAngleDifferenceMinusPiToPi(node.getYaw(), goalNode.getYaw()); double minSteps = euclideanDistance / parameters.getMaximumStepReach(); return euclideanDistance + costParameters.getYawWeight() * Math.abs(yaw) + costParameters.getCostPerStep() * minSteps; } }
public void updateForAngles(double currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); lastPosition.set(currentPosition); set(0.0); } double difference = AngleTools.computeAngleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue()); updateUsingDifference(difference); lastPosition.set(currentPosition); }
@Override public double compute(FootstepNode startNode, FootstepNode endNode) { Point2D startPoint = startNode.getOrComputeMidFootPoint(parameters.getIdealFootstepWidth()); Point2D endPoint = endNode.getOrComputeMidFootPoint(parameters.getIdealFootstepWidth()); double euclideanDistance = startPoint.distance(endPoint); double yaw = AngleTools.computeAngleDifferenceMinusPiToPi(startNode.getYaw(), endNode.getYaw()); double distanceCost = 0.5 * (costParameters.getForwardWeight() + costParameters.getLateralWeight()); return distanceCost * euclideanDistance + costParameters.getYawWeight() * Math.abs(yaw) + costParameters.getCostPerStep(); } }
@Override public double compute(FootstepNode startNode, FootstepNode endNode) { Point2D startPoint = startNode.getOrComputeMidFootPoint(parameters.getIdealFootstepWidth()); Point2D endPoint = endNode.getOrComputeMidFootPoint(parameters.getIdealFootstepWidth()); startNodePose.setPosition(startPoint.getX(), startPoint.getY(), 0.0); startNodePose.setOrientationYawPitchRoll(startNode.getYaw(), 0.0, 0.0); startNodeFrame.setPoseAndUpdate(startNodePose); endNodePosition.setIncludingFrame(ReferenceFrame.getWorldFrame(), endPoint, 0.0); endNodePosition.changeFrame(startNodeFrame); double cost = costParameters.getForwardWeight() * Math.pow(endNodePosition.getX(), 2.0); cost += costParameters.getLateralWeight() * Math.pow(endNodePosition.getY(), 2.0); double yaw = AngleTools.computeAngleDifferenceMinusPiToPi(startNode.getYaw(), endNode.getYaw()); cost += costParameters.getYawWeight() * Math.abs(yaw) + costParameters.getCostPerStep(); return cost; } }
public double computeForAngles(double currentPosition, double desiredPosition, double currentRate, double desiredRate) { // System.out.println("PGain: " + proportionalGain.getDoubleValue() + "DGain: " + derivativeGain.getDoubleValue()); this.positionError.set(applyDeadband(AngleTools.computeAngleDifferenceMinusPiToPi(desiredPosition, currentPosition))); rateError.set(desiredRate - currentRate); actionP.set(getProportionalGain() * positionError.getDoubleValue()); actionD.set(getDerivativeGain() * rateError.getDoubleValue()); return actionP.getDoubleValue() + actionD.getDoubleValue(); }
public double computeForAngles(double currentPosition, double desiredPosition, double currentRate, double desiredRate) { // System.out.println("PGain: " + proportionalGain.getDoubleValue() + "DGain: " + derivativeGain.getDoubleValue()); this.positionError.set(applyDeadband(AngleTools.computeAngleDifferenceMinusPiToPi(desiredPosition, currentPosition))); rateError.set(desiredRate - currentRate); actionP.set(proportionalGain.getDoubleValue() * positionError.getDoubleValue()); actionD.set(derivativeGain.getDoubleValue() * rateError.getDoubleValue()); return actionP.getDoubleValue() + actionD.getDoubleValue(); }
@Override protected double computeHeuristics(FootstepNode node, FootstepNode goalNode) { Point2D midFootPoint = node.getOrComputeMidFootPoint(parameters.getIdealFootstepWidth()); Pose2D closestPointOnPath = new Pose2D(); double alpha = bodyPath.getClosestPoint(midFootPoint, closestPointOnPath); alpha = MathTools.clamp(alpha, 0.0, goalAlpha); bodyPath.getPointAlongPath(alpha, closestPointOnPath); double distanceToPath = closestPointOnPath.getPosition().distance(midFootPoint); double pathLength = bodyPath.computePathLength(alpha) - bodyPath.computePathLength(goalAlpha); double remainingDistance = pathLength + pathViolationWeight * distanceToPath; double yaw = pathViolationWeight * AngleTools.computeAngleDifferenceMinusPiToPi(node.getYaw(), closestPointOnPath.getYaw()); double minSteps = remainingDistance / parameters.getMaximumStepReach(); return remainingDistance + costParameters.getYawWeight() * Math.abs(yaw) + costParameters.getCostPerStep() * minSteps; }