@Override public double getZ() { return zPolynomial.getAcceleration(); } };
public double getAcceleration() { return polynomial.getAcceleration(); } }
@Override public double getX() { return xPolynomial.getAcceleration(); }
public double getAcceleration() { return polynomial.getAcceleration(); } }
@Override public double getY() { return yPolynomial.getAcceleration(); }
public double getAcceleration() { return polynomial.getAcceleration(); }
@Override public double getAcceleration() { return isDone() ? 0.0 : findCurrentPolynomial().getAcceleration(); }
@Override public double getAcceleration() { return isDone() ? 0.0 : findCurrentPolynomial().getAcceleration(); }
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 testGetAcceleration() { Random random = new Random(1675L); int order = 5; YoPolynomial spline = new YoPolynomial("test", order, registry); double[] coefficients = getRandomCoefficients(order, random); spline.setDirectly(coefficients); double x = random.nextDouble(); double dx = 1e-9; spline.compute(x); double d2ydx2 = spline.getAcceleration(); double dydxx = spline.getVelocity(); spline.compute(x + dx); double dydxxPlusdx = spline.getVelocity(); double d2ydx2Numerical = (dydxxPlusdx - dydxx) / dx; assertEquals(d2ydx2Numerical, d2ydx2, 1e-6); }
@Override public void compute(double time) { this.currentTime.set(time); time = MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue()); xPolynomial.compute(time); yPolynomial.compute(time); zPolynomial.compute(time); if (time < trajectoryTime.getDoubleValue()) { currentPosition.set(xPolynomial.getPosition(), yPolynomial.getPosition(), zPolynomial.getPosition()); currentVelocity.set(xPolynomial.getVelocity(), yPolynomial.getVelocity(), zPolynomial.getVelocity()); currentAcceleration.set(xPolynomial.getAcceleration(), yPolynomial.getAcceleration(), zPolynomial.getAcceleration()); } else { currentPosition.set(finalPosition); currentVelocity.set(finalVelocity); currentAcceleration.set(0.0, 0.0, 0.0); } }
@Override public void compute(double dt) { this.currentTime.add(dt); double time = MathTools.clamp(currentTime.getDoubleValue(), 0.0, trajectoryTime.getDoubleValue()); xPolynomial.compute(time); yPolynomial.compute(time); zPolynomial.compute(time); if (time < trajectoryTime.getDoubleValue()) { currentPosition.set(xPolynomial.getPosition(), yPolynomial.getPosition(), zPolynomial.getPosition()); currentVelocity.set(xPolynomial.getVelocity(), yPolynomial.getVelocity(), zPolynomial.getVelocity()); currentAcceleration.set(xPolynomial.getAcceleration(), yPolynomial.getAcceleration(), zPolynomial.getAcceleration()); } else { currentPosition.set(finalPosition); currentVelocity.set(finalVelocity); currentAcceleration.set(0.0, 0.0, 0.0); } }
@Override public void compute(double dt) { this.currentTime.add(dt); double time = MathTools.clipToMinMax(currentTime.getDoubleValue(), 0.0, trajectoryTime.getDoubleValue()); xPolynomial.compute(time); yPolynomial.compute(time); zPolynomial.compute(time); if (time < trajectoryTime.getDoubleValue()) { currentPosition.set(xPolynomial.getPosition(), yPolynomial.getPosition(), zPolynomial.getPosition()); currentVelocity.set(xPolynomial.getVelocity(), yPolynomial.getVelocity(), zPolynomial.getVelocity()); currentAcceleration.set(xPolynomial.getAcceleration(), yPolynomial.getAcceleration(), zPolynomial.getAcceleration()); } else { currentPosition.set(finalPosition); currentVelocity.set(finalVelocity); currentAcceleration.set(0.0, 0.0, 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()); }
private void computeInitialConstraintOffset(double time) { double startTime = initialBlendStartTime.getDoubleValue(); double endTime = initialBlendEndTime.getDoubleValue(); time = MathTools.clamp(time - initialBlendTimeOffset.getValue(), startTime, endTime); for (int i = 0; i < 3; i++) { initialConstraintPolynomial[i].compute(time); initialConstraintPositionOffset.setElement(i, initialConstraintPolynomial[i].getPosition()); initialConstraintVelocityOffset.setElement(i, initialConstraintPolynomial[i].getVelocity()); initialConstraintAccelerationOffset.setElement(i, initialConstraintPolynomial[i].getAcceleration()); } }
private void computeFinalConstraintOffset(double time) { double startTime = finalBlendStartTime.getDoubleValue(); double endTime = finalBlendEndTime.getDoubleValue(); time = MathTools.clamp(time - finalBlendTimeOffset.getValue(), startTime, endTime); for (int i = 0; i < 3; i++) { finalConstraintPolynomial[i].compute(time); finalConstraintPositionOffset.setElement(i, finalConstraintPolynomial[i].getPosition()); finalConstraintVelocityOffset.setElement(i, finalConstraintPolynomial[i].getVelocity()); finalConstraintAccelerationOffset.setElement(i, finalConstraintPolynomial[i].getAcceleration()); } } }
public void compute(double time) { timeIntoStep.set(time); nominalTrajectoryGenerator.compute(time); nominalTrajectoryGenerator.getLinearData(nominalTrajectoryPosition, nominalTrajectoryVelocity, nominalTrajectoryAcceleration); xPolynomial.compute(time); yPolynomial.compute(time); desiredPosition.setX(xPolynomial.getPosition()); desiredPosition.setY(yPolynomial.getPosition()); desiredPosition.setZ(nominalTrajectoryPosition.getZ()); desiredVelocity.setX(xPolynomial.getVelocity()); desiredVelocity.setY(yPolynomial.getVelocity()); desiredVelocity.setZ(nominalTrajectoryVelocity.getZ()); desiredAcceleration.setX(xPolynomial.getAcceleration()); desiredAcceleration.setY(yPolynomial.getAcceleration()); desiredAcceleration.setZ(nominalTrajectoryAcceleration.getZ()); }
public void compute(double time) { if (continuouslyUpdateFinalOrientation.getBooleanValue()) updateFinalOrientation(); this.currentTime.set(time); time = MathTools.clamp(time, 0.0, trajectoryTime.getDoubleValue()); parameterPolynomial.compute(time); double parameter = isDone() ? 1.0 : parameterPolynomial.getPosition(); desiredOrientation.interpolate(initialOrientation, finalOrientation, parameter); double parameterd = isDone() ? 0.0 : parameterPolynomial.getVelocity(); orientationInterpolationCalculator.computeAngularVelocity(desiredAngularVelocity, initialOrientation, finalOrientation, parameterd); double parameterdd = isDone() ? 0.0 : parameterPolynomial.getAcceleration(); orientationInterpolationCalculator.computeAngularAcceleration(desiredAngularAcceleration, initialOrientation, finalOrientation, parameterdd); }
public void compute(double time) { if (continuouslyUpdateFinalOrientation.getBooleanValue()) updateFinalOrientation(); this.currentTime.set(time); time = MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue()); parameterPolynomial.compute(time); double parameter = isDone() ? 1.0 : parameterPolynomial.getPosition(); desiredOrientation.interpolate(initialOrientation, finalOrientation, parameter); double parameterd = isDone() ? 0.0 : parameterPolynomial.getVelocity(); orientationInterpolationCalculator.computeAngularVelocity(desiredAngularVelocity, initialOrientation, finalOrientation, parameterd); double parameterdd = isDone() ? 0.0 : parameterPolynomial.getAcceleration(); orientationInterpolationCalculator.computeAngularAcceleration(desiredAngularAcceleration, initialOrientation, finalOrientation, parameterdd); }
@Override public void compute(double time) { if (continuouslyUpdateFinalPosition.getBooleanValue()) { updateFinalPosition(); } this.currentTime.set(time); time = MathTools.clamp(time, 0.0, trajectoryTime.getDoubleValue()); parameterPolynomial.compute(time); differenceVector.sub(finalPosition, initialPosition); double parameter = isDone() ? 1.0 : parameterPolynomial.getPosition(); double parameterd = isDone() ? 0.0 : parameterPolynomial.getVelocity(); double parameterdd = isDone() ? 0.0 : parameterPolynomial.getAcceleration(); currentPosition.interpolate(initialPosition, finalPosition, parameter); currentVelocity.set(differenceVector); currentVelocity.scale(parameterd); currentAcceleration.set(differenceVector); currentAcceleration.scale(parameterdd); }