private void setupPolynomialSpline(double time, double duration) { yoPolynomial.setCubic(time, time + duration, 0.0, 0.0, 1.0, 0.0); }
@Override public double getY() { return yPolynomial.getPosition(); }
public void compute(double t) { for (Direction direction : Direction.values) { polynomials.get(direction).compute(t); } for (Direction direction : Direction.values) { position.set(direction, polynomials.get(direction).getPosition()); } for (Direction direction : Direction.values) { velocity.set(direction, polynomials.get(direction).getVelocity()); } for (Direction direction : Direction.values) { acceleration.set(direction, polynomials.get(direction).getAcceleration()); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetPosition() { Random random = new Random(165L); int order = 5; YoPolynomial spline = new YoPolynomial("test", order, registry); double[] coefficients = getRandomCoefficients(order, random); spline.setDirectly(coefficients); double x = random.nextDouble(); spline.compute(x); double y = spline.getPosition(); double yCheck = coefficients[0] + coefficients[1] * x + coefficients[2] * x * x + coefficients[3] * x * x * x + coefficients[4] * x * x * x * x; assertEquals(yCheck, y, 1e-12); }
public FootstepPlanningResult planWaypoints() { waypoints.clear(); double yaw = bodyStartPose.getYaw(); xPoly.setQuadratic(0.0, 1.0, bodyStartPose.getX(), Math.cos(yaw) * 0.2, bodyGoalPose.getX()); yPoly.setQuadratic(0.0, 1.0, bodyStartPose.getY(), Math.sin(yaw) * 0.2, bodyGoalPose.getY()); zPoly.setQuadratic(0.0, 1.0, bodyStartPose.getZ(), Math.sin(yaw) * 0.2, bodyGoalPose.getZ()); for (int i = 0; i < numberOfPoints; i++) { double percent = i / (double) (numberOfPoints - 1); xPoly.compute(percent); yPoly.compute(percent); zPoly.compute(percent); Point3D point = new Point3D(xPoly.getPosition(), yPoly.getPosition(), zPoly.getPosition()); waypoints.add(point); } yoResult.set(FootstepPlanningResult.OPTIMAL_SOLUTION); return yoResult.getEnumValue(); }
public SplinePathPlanner(FootstepPlannerParameters parameters, YoVariableRegistry parentRegistry) { super(parameters, parentRegistry); xPoly = new YoPolynomial("xPoly", 4, parentRegistry); yPoly = new YoPolynomial("yPoly", 4, parentRegistry); zPoly = new YoPolynomial("zPoly", 4, parentRegistry); }
public void computePositionsForVis(double time) { nominalTrajectoryGenerator.compute(time); xPolynomial.compute(time); yPolynomial.compute(time); nominalTrajectoryGenerator.getPosition(nominalTrajectoryPosition); nominalTrajectoryGenerator.getVelocity(nominalTrajectoryVelocity); nominalTrajectoryGenerator.getAcceleration(nominalTrajectoryAcceleration); desiredPosition.setX(xPolynomial.getPosition()); desiredPosition.setY(yPolynomial.getPosition()); desiredPosition.setZ(nominalTrajectoryPosition.getZ()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testCubicDerivativePointAutomated() { //cubic polynomial: y(x) = a0 + a1*x + a2*x^2 + a3*x^3 YoVariableRegistry registry = new YoVariableRegistry(namePrefix); int numberOfCoefficients = 4; YoPolynomial cubic = new YoPolynomial(namePrefix + "Cubic", numberOfCoefficients, registry); double x0 = 1.0, xf = 2.0; double y0 = 0.5, yf = 1.5; double dy0 = -0.5, dyf = 2.0; cubic.setCubic(x0, xf, y0, dy0, yf, dyf); double x = 2.0/3.0 * (xf - x0); compareDerivativesPoint(cubic, x); }
@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); }
/** * 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 public double getX() { return xPolynomial.getVelocity(); }
@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()); } } }
@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); }
@Override public double getZ() { return zPolynomial.getAcceleration(); } };
public void setLinear(double t0, double tFinal, double z0, double zf) { setTime(t0, tFinal); polynomial.setLinear(t0, tFinal, z0, zf); }
public void setQuadratic(double t0, double tFinal, double z0, double zd0, double zFinal) { setTime(t0, tFinal); polynomial.setQuadratic(t0, tFinal, z0, zd0, zFinal); }
@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); } } }
public void compute(double time) { this.currentTime.set(time); time = MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue()); polynomial.compute(time); currentValue.set(polynomial.getPosition()); currentVelocity.set(polynomial.getVelocity()); currentAcceleration.set(polynomial.getAcceleration()); }
xPoly.setQuadratic(0.0, 1.0, 0.0, 0.2, x.getDoubleValue()); yPoly.setQuadratic(0.0, 1.0, 0.0, 0.0, y.getDoubleValue()); for (int i = 0; i < numberOfPoints; i++) xPoly.compute(percent); yPoly.compute(percent); Point3D point2d = new Point3D(xPoly.getPosition(), yPoly.getPosition(), 0.0); waypoints.add(point2d);