public void setLinear(double t0, double tFinal, double z0, double zf) { setTime(t0, tFinal); polynomial.setLinear(t0, tFinal, z0, zf); }
public void setLinear(double t0, double tFinal, Point3DReadOnly z0, Point3DReadOnly zf) { for (int index = 0; index < 3; index++) getYoPolynomial(index).setLinear(t0, tFinal, z0.getElement(index), zf.getElement(index)); }
public void setLinear(double t0, double tf, FramePoint p0, FramePoint pf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 2); for (Direction direction : Direction.values) { polynomials.get(direction).setLinear(t0, tf, p0.get(direction), pf.get(direction)); } setYoVariables(t0, tf); }
public void setLinearUsingInitialPositionAndVelocity(double t0, double tf, FramePoint p0, FrameVector pd0) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 2); for (Direction direction : Direction.values) { polynomials.get(direction).setLinear(t0, p0.get(direction), pd0.get(direction)); } setYoVariables(t0, tf); }
public void setLinearUsingFinalPositionAndVelocity(double t0, double tf, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 2); for (Direction direction : Direction.values) { polynomials.get(direction).setLinear(tf, pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); polynomial.setLinear(0.0, initialPositionProvider.getValue(), velocityProvider.getValue()); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); polynomial.setLinear(0.0, initialPositionProvider.getValue(), velocityProvider.getValue()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testLinearDerivativePointAutomated() { //linear polynomial: y(x) = a0 + a1*x YoVariableRegistry registry = new YoVariableRegistry(namePrefix); int numberOfCoefficients = 2; YoPolynomial linear = new YoPolynomial(namePrefix + "Linear", numberOfCoefficients, registry); double x0 = 1.0, xf = 2.0; double y0 = 0.5, yf = 1.5; linear.setLinear(x0, xf, y0, yf); double x = 2.0/3.0 * (xf - x0); compareDerivativesPoint(linear, x); }
public void setLinearUsingInitialPositionAndVelocity(double t0, double tf, FramePoint3D p0, FrameVector3D pd0) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 2); p0.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setLinear(t0, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setLinearUsingFinalPositionAndVelocity(double t0, double tf, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 2); pf.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setLinear(tf, pf.getElement(axis.ordinal()), pdf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setLinear(double t0, double tf, FramePoint3D p0, FramePoint3D pf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 2); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setLinear(t0, tf, p0.getElement(axis.ordinal()), pf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
/** * Desired joint angles and velocities come from reading the joints, this method can override them those position and velocity values. * @param currentDesiredPosition Sets the desired joint position. * @param currentDesiredVelocity Sets the desired joint velocity. */ @Override public void initialize(double initialPosition, double initialVelocity) { currentTime.set(0.0); currentTimeOffset.set(0.0); currentPolynomialIndex.set(0); trajectoryTime.set(trajectoryTimeProvider.getValue()); subTrajectoryTime.set(trajectoryTime.getDoubleValue() / (currentNumberOfWaypoints.getIntegerValue() + 1)); startMotionPolynomial.setCubicThreeInitialConditionsFinalPosition(0.0, subTrajectoryTime.getDoubleValue(), initialPosition, initialVelocity, 0.0, intermediatePositions.get(0).getDoubleValue()); startMotionPolynomial.compute(subTrajectoryTime.getDoubleValue()); for (int i = 0; i < currentNumberOfWaypoints.getIntegerValue() - 1; i++) { YoPolynomial connectingPolynomial = connectingPolynomials.get(i); double z0 = intermediatePositions.get(i).getDoubleValue(); double zFinal = intermediatePositions.get(i + 1).getDoubleValue(); connectingPolynomial.setLinear(0.0, subTrajectoryTime.getDoubleValue(), z0, zFinal); connectingPolynomial.compute(subTrajectoryTime.getDoubleValue()); } double z0 = intermediatePositions.get(currentNumberOfWaypoints.getIntegerValue() - 1).getDoubleValue(); double zFinal = finalPosition.getDoubleValue(); finalizeMotionPolynomial.setCubicInitialPositionThreeFinalConditions(0.0, subTrajectoryTime.getDoubleValue(), z0, zFinal, 0.0, 0.0); }
@Override protected void setPolynomial() { if (Precision.equals(0.0, trajectoryTime.getDoubleValue())) { polynomial.setLinear(0.0, initialPositionProvider.getValue(), initialVelocityProvider.getValue()); } else { polynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPositionProvider.getValue(), initialVelocityProvider.getValue(), 0.0, finalPositionProvider.getValue(), finalVelocityProvider.getValue(), 0.0); } } }
@Override protected void setPolynomial() { if (Precision.equals(0.0, trajectoryTime.getDoubleValue())) { polynomial.setLinear(0.0, initialPositionProvider.getValue(), initialVelocityProvider.getValue()); } else { polynomial.setCubic(0.0, trajectoryTime.getDoubleValue(), initialPositionProvider.getValue(), initialVelocityProvider.getValue(), finalPositionProvider.getValue(), finalVelocityProvider.getValue()); } } }
/** * Desired joint angles and velocities come from reading the joints, this method can override them those position and velocity values. * @param currentDesiredPosition Sets the desired joint position. * @param currentDesiredVelocity Sets the desired joint velocity. */ @Override public void initialize(double initialPosition, double initialVelocity) { currentTime.set(0.0); currentTimeOffset.set(0.0); currentPolynomialIndex.set(0); trajectoryTime.set(trajectoryTimeProvider.getValue()); subTrajectoryTime.set(trajectoryTime.getDoubleValue() / (currentNumberOfWaypoints.getIntegerValue() + 1)); startMotionPolynomial.setCubicThreeInitialConditionsFinalPosition(0.0, subTrajectoryTime.getDoubleValue(), initialPosition, initialVelocity, 0.0, intermediatePositions.get(0).getDoubleValue()); startMotionPolynomial.compute(subTrajectoryTime.getDoubleValue()); for (int i = 0; i < currentNumberOfWaypoints.getIntegerValue() - 1; i++) { YoPolynomial connectingPolynomial = connectingPolynomials.get(i); double z0 = intermediatePositions.get(i).getDoubleValue(); double zFinal = intermediatePositions.get(i + 1).getDoubleValue(); connectingPolynomial.setLinear(0.0, subTrajectoryTime.getDoubleValue(), z0, zFinal); connectingPolynomial.compute(subTrajectoryTime.getDoubleValue()); } double z0 = intermediatePositions.get(currentNumberOfWaypoints.getIntegerValue() - 1).getDoubleValue(); double zFinal = finalPosition.getDoubleValue(); finalizeMotionPolynomial.setCubicInitialPositionThreeFinalConditions(0.0, subTrajectoryTime.getDoubleValue(), z0, zFinal, 0.0, 0.0); }
public void initialize() { currentTime.set(0.0); yoInitialPosition.getFrameTupleIncludingFrame(initialPosition); initialPosition.changeFrame(steeringWheelFrame); finalOrientation.setIncludingFrame(initialOrientation); finalOrientation.changeFrame(trajectoryFrame); yoFinalOrientation.set(finalOrientation); double x = initialPosition.getX(); double y = initialPosition.getY(); initialZ.set(initialPosition.getZ()); initialSteeringAngle.set(Math.atan2(y, x)); double initialToFinalAngle = computeAngleDifferenceMinusPiToPi(finalSteeringAngle.getDoubleValue(), initialSteeringAngle.getDoubleValue()); double finalSteeringAngleForShortestPath = initialSteeringAngle.getDoubleValue() + initialToFinalAngle; trajectoryTime.set(MathTools.clipToMinMax(Math.abs(initialToFinalAngle) / desiredSteeringSpeed.getDoubleValue(), 0.25, Double.POSITIVE_INFINITY)); steeringAnglePolynomial.setLinear(0.0, trajectoryTime.getDoubleValue(), initialSteeringAngle.getDoubleValue(), finalSteeringAngleForShortestPath); double xFinal = steeringWheelRadius.getDoubleValue() * Math.cos(finalSteeringAngle.getDoubleValue()); double yFinal = steeringWheelRadius.getDoubleValue() * Math.sin(finalSteeringAngle.getDoubleValue()); double zFinal = initialZ.getDoubleValue(); finalPosition.setIncludingFrame(steeringWheelFrame, xFinal, yFinal, zFinal); yoFinalPosition.setAndMatchFrame(finalPosition); currentAngleTrackingError.set(0.0); currentControlledFrameRelativeAngle.set(initialSteeringAngle.getDoubleValue()); updateTangentialCircleFrame(); if (visualize) visualizeTrajectory(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testLinearDerivativePointManual() { //linear polynomial: y(x) = a0 + a1*x YoVariableRegistry registry = new YoVariableRegistry(namePrefix); int numberOfCoefficients = 2; YoPolynomial linear = new YoPolynomial(namePrefix + "Linear", numberOfCoefficients, registry); double x0 = 1.0, xf = 2.0; double y0 = 0.5, yf = 1.5; linear.setLinear(x0, xf, y0, yf); double x = 2.0/3.0 * (xf - x0); double a0 = linear.getCoefficient(0); double a1 = linear.getCoefficient(1); double yLinear = linear.getDerivative(0, x); double yManual = a0 + a1*x; assertEquals(yLinear, yManual, EPSILON); double dyLinear = linear.getDerivative(1, x); double dyManual = a1; assertEquals(dyLinear, dyManual, EPSILON); double ddyLinear = linear.getDerivative(2, x); double ddyManual = 0.0; assertEquals(ddyLinear, ddyManual, EPSILON); }