public void compute(double x) { polynomial.compute(x); }
public void compute(double time) { this.currentTime.set(time); polynomial.compute(time); }
public void compute(double time) { this.currentTime.set(time); polynomial.compute(time); }
public void compute(double time) { this.currentTime.set(time); time = MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue()); polynomial.compute(time); }
public void compute(double time) { this.currentTime.set(time); time = MathTools.clamp(time, 0.0, trajectoryTime.getDoubleValue()); polynomial.compute(time); }
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); }
@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 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()); }
private void updatePrivilegedConfiguration() { double timeInTrajectory = MathTools.clipToMinMax(getTimeInCurrentState(), 0.0, durationForStanceLegStraightening.getDoubleValue()); kneePrivilegedConfigurationTrajectory.compute(timeInTrajectory); straightLegsPrivilegedConfigurationCommand.clear(); straightLegsPrivilegedConfigurationCommand.addJoint(kneePitch, kneePrivilegedConfigurationTrajectory.getPosition()); straightLegsPrivilegedConfigurationCommand.applyPrivilegedConfigurationToSubChain(pelvis, contactableFoot.getRigidBody()); }
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()); } } }
@Override public void compute(double time) { currentTime.set(time); time = MathTools.clamp(time, 0.0, trajectoryTime.getDoubleValue()); if (currentTime.getDoubleValue() - currentTimeOffset.getDoubleValue() > subTrajectoryTime.getDoubleValue()) { currentTimeOffset.add(subTrajectoryTime.getDoubleValue()); currentPolynomialIndex.increment(); currentPolynomialIndex.set(Math.min(currentPolynomialIndex.getIntegerValue(), currentNumberOfWaypoints.getIntegerValue())); } time -= currentTimeOffset.getDoubleValue(); time = MathTools.clamp(time, 0.0, subTrajectoryTime.getDoubleValue()); findCurrentPolynomial().compute(time); }
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()); }
@Override public void compute(double time) { currentTime.set(time); time = MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue()); if (currentTime.getDoubleValue() - currentTimeOffset.getDoubleValue() > subTrajectoryTime.getDoubleValue()) { currentTimeOffset.add(subTrajectoryTime.getDoubleValue()); currentPolynomialIndex.increment(); currentPolynomialIndex.set(Math.min(currentPolynomialIndex.getIntegerValue(), currentNumberOfWaypoints.getIntegerValue())); } time -= currentTimeOffset.getDoubleValue(); time = MathTools.clipToMinMax(time, 0.0, subTrajectoryTime.getDoubleValue()); findCurrentPolynomial().compute(time); }
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); }