@Override public double getX() { return xPolynomial.getPosition(); }
@Override public double getY() { return yPolynomial.getPosition(); }
@Override public double getZ() { return zPolynomial.getPosition(); } };
public double getValue() { return polynomial.getPosition(); }
public double getAngleFromXAxis() { return anglePolynomial.getPosition(); }
public double getPosition() { return polynomial.getPosition(); }
public double getValue() { return polynomial.getPosition(); }
public double getValue() { return polynomial.getPosition(); }
public double getValue() { return polynomial.getPosition(); }
public double getAngleFromXAxis() { return anglePolynomial.getPosition(); }
@Override public double getValue() { return isDone() ? finalPosition.getDoubleValue() : findCurrentPolynomial().getPosition(); }
@Override public double getValue() { return isDone() ? finalPosition.getDoubleValue() : findCurrentPolynomial().getPosition(); }
@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()); } } }
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); }