/** * Sets the coefficients to zero. Initial and final time are retained. */ public void setZero() { polynomial.setConstant(0.0); }
public void setConstant(double t0, double tFinal, double z) { setTime(t0, tFinal); polynomial.setConstant(z); }
public void clearFinalConstraint() { for (int i = 0; i < 3; i++) { finalConstraintPositionError.setToZero(); finalConstraintVelocityError.setToZero(); finalConstraintPolynomial[i].setConstant(0.0); } }
public void setConstant(Point3DReadOnly z) { for (int index = 0; index < 3; index++) getYoPolynomial(index).setConstant(z.getElement(index)); }
public void clearInitialConstraint() { for (int i = 0; i < 3; i++) { initialConstraintPositionError.setToZero(); initialConstraintVelocityError.setToZero(); initialConstraintPolynomial[i].setConstant(0.0); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConstant() { Random random = new Random(1635L); YoPolynomial spline = new YoPolynomial("test", 5, registry); double z = random.nextDouble(); spline.setConstant(z); double[] coefficients = spline.getCoefficients(); double epsilon = 1e-9; assertEquals(z, coefficients[0], epsilon); for (int i = 1; i < coefficients.length; i++) { assertEquals(0.0, coefficients[i], epsilon); } }
@Override public void initialize() { optimizer.compute(maxIterations.getIntegerValue()); for (int i = 0; i < segments.getIntegerValue()-1; i++) waypointTimes.get(i).set(optimizer.getWaypointTime(i)); for (int dimension = 0; dimension < dimensions; dimension++) { optimizer.getPolynomialCoefficients(coefficients, dimension); Direction axis; if (dimension == 0) axis = Direction.X; else if (dimension == 1) axis = Direction.Z; else axis = null; for (int i = 0; i < segments.getIntegerValue(); i++) { coefficients.get(i).toArray(tempCoeffs); trajectories.get(axis).get(i).setDirectlyReverse(tempCoeffs); } } for (int i = 0; i < segments.getIntegerValue(); i++) { trajectories.get(Direction.Y).get(i).setConstant(0.0); } visualize(); }