private void setupPolynomialSpline(double time, double duration) { yoPolynomial.setCubic(time, time + duration, 0.0, 0.0, 1.0, 0.0); }
private void setupPolynomialSpline(double time, double duration) { yoPolynomial.setCubic(time, time + duration, 0.0, 0.0, 1.0, 0.0); }
private void setupPolynomialSpline(double time, double duration) { yoPolynomial.setCubic(time, time + duration, 0.0, 0.0, 1.0, 0.0); }
public void setCubic(double t0, double tFinal, double z0, double zFinal) { setTime(t0, tFinal); polynomial.setCubic(t0, tFinal, z0, zFinal); }
public void setCubic(double t0, double tFinal, double z0, double zd0, double zFinal, double zdFinal) { setTime(t0, tFinal); polynomial.setCubic(t0, tFinal, z0, zd0, zFinal, zdFinal); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testDerivativeVersionsCubic() { //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); int numTrials = 9; for(int i = 0; i < numTrials; i++) { double scaleX0 = 1.0 / Math.random(), scaleXf = 1.0 / Math.random(); double scaleY0 = 1.0 / Math.random(), scaleYf = 1.0 / Math.random(); double scaleDY0 = 1.0 / Math.random(), scaleDYf = 1.0 / Math.random(); double x0 = Math.signum(Math.random()) * Math.random() * scaleX0, xf = x0 + Math.random() * scaleXf; double y0 = Math.signum(Math.random()) * Math.random() * scaleY0, yf = Math.signum(Math.random()) * Math.random() * scaleYf; double dy0 = Math.signum(Math.random()) * Math.random() * scaleDY0, dyf = Math.signum(Math.random()) * Math.random() * scaleDYf; cubic.setCubic(x0, xf, y0, dy0, yf, dyf); double x = Math.random() * (xf - x0); compareDerivativeVersions(cubic, x); } }
public void setCubic(double t0, double tFinal, Point3DReadOnly z0, Point3DReadOnly zFinal) { for (int index = 0; index < 3; index++) getYoPolynomial(index).setCubic(t0, tFinal, z0.getElement(index), zFinal.getElement(index)); }
public void setCubic(double t0, double tf, FramePoint p0, FrameVector pd0, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 4); for (Direction direction : Direction.values) { polynomials.get(direction).setCubic(t0, tf, p0.get(direction), pd0.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
private void initializeTrajectory() { for (int i = 0; i < allFingerJoints.size(); i++) { OneDegreeOfFreedomJoint fingerJoint = allFingerJoints.get(i); initialDesiredAngles.get(fingerJoint).set(desiredAngles.get(fingerJoint).getDoubleValue()); } startTrajectoryTime.set(yoTime.getDoubleValue()); endTrajectoryTime.set(startTrajectoryTime.getDoubleValue() + trajectoryTime.getDoubleValue()); if (hasTrajectoryTimeChanged.getBooleanValue()) { yoPolynomial.setCubic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 1.0, 0.0); hasTrajectoryTimeChanged.set(false); } }
private void initializeTrajectory() { for (int i = 0; i < allFingerJoints.size(); i++) { OneDegreeOfFreedomJoint fingerJoint = allFingerJoints.get(i); initialDesiredAngles.get(fingerJoint).set(desiredAngles.get(fingerJoint).getDoubleValue()); } startTrajectoryTime.set(yoTime.getDoubleValue()); endTrajectoryTime.set(startTrajectoryTime.getDoubleValue() + trajectoryTime.getDoubleValue()); if (hasTrajectoryTimeChanged.getBooleanValue()) { yoPolynomial.setCubic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 1.0, 0.0); hasTrajectoryTimeChanged.set(false); } }
protected void setPolynomial() { polynomial.setCubic(0.0, trajectoryTime.getDoubleValue(), initialPositionProvider.getValue(), initialVelocityProvider.getValue(), finalPositionProvider.getValue(), finalVelocityProvider.getValue()); }
public void setCubic(double t0, double tFinal, Point3DReadOnly z0, Vector3DReadOnly zd0, Point3DReadOnly zFinal, Vector3DReadOnly zdFinal) { for (int index = 0; index < 3; index++) getYoPolynomial(index).setCubic(t0, tFinal, z0.getElement(index), zd0.getElement(index), zFinal.getElement(index), zdFinal.getElement(index)); }
@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); }
@Override public void doTransitionIntoAction() { super.doTransitionIntoAction(); FrameVector fullyConstrainedNormalContactVector = footControlHelper.getFullyConstrainedNormalContactVector(); momentumBasedController.setPlaneContactStateNormalContactVector(contactableFoot, fullyConstrainedNormalContactVector); for (int i = 0; i < dofs; i++) isDirectionFeedbackControlled[i] = false; footBarelyLoaded.set(false); copOnEdge.set(false); updateHoldPositionSetpoints(); double currentKneeAngle = kneePitch.getQ(); kneePrivilegedConfigurationTrajectory.setCubic(0.0, durationForStanceLegStraightening.getDoubleValue(), currentKneeAngle, 0.0, straightKneeAngle.getDoubleValue(), 0.0); }
public void setCubic(double t0, double tf, FramePoint3D p0, FrameVector3D pd0, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 4); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setCubic(t0, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pf.getElement(axis.ordinal()), pdf.getElement( axis.ordinal())); } setYoVariables(t0, tf); }
@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()); } } }
@Override public void onEntry() { for (int i = 0; i < jointsData.size(); i++) { OneDoFJointBasics joint = jointsData.get(i).getLeft(); TrajectoryData trajectoryData = jointsData.get(i).getRight(); YoDouble initialPosition = trajectoryData.getInitialPosition(); YoPolynomial trajectory = trajectoryData.getTrajectory(); double startAngle = jointTorqueOffsetEstimatorController.getJointCalibrationPosition(joint); double startVelocity = 0.0; double finalAngle = initialPosition.getDoubleValue(); double finalVelocity = 0.0; trajectory.setCubic(0.0, timeToMoveForCalibration.getDoubleValue(), startAngle, startVelocity, finalAngle, finalVelocity); } }
@Override public void onEntry() { for (int i = 0; i < jointsData.size(); i++) { OneDoFJointBasics joint = jointsData.get(i).getLeft(); TrajectoryData trajectoryData = jointsData.get(i).getRight(); YoDouble initialPosition = trajectoryData.getInitialPosition(); YoPolynomial trajectory = trajectoryData.getTrajectory(); JointDesiredOutputReadOnly jointDesiredOutput = highLevelControlOutput.getJointDesiredOutput(joint); double startAngle = jointDesiredOutput != null && jointDesiredOutput.hasDesiredPosition() ? jointDesiredOutput.getDesiredPosition() : joint.getQ(); double startVelocity = 0.0; double finalAngle = jointTorqueOffsetEstimatorController.getJointCalibrationPosition(joint); double finalVelocity = 0.0; initialPosition.set(startAngle); trajectory.setCubic(0.0, timeToMoveForCalibration.getDoubleValue(), startAngle, startVelocity, finalAngle, finalVelocity); } jointTorqueOffsetEstimatorController.initialize(); }
@Override public void initialize() { tangentialPlane.update(); currentTrajectoryFrame = initialPosition.getReferenceFrame(); changeFrame(tangentialPlane, false); currentTime.set(0.0); MathTools.checkIfInRange(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY); double tIntermediate = trajectoryTime.getDoubleValue() - approachTime.getDoubleValue(); xyPolynomial.setCubic(0.0, tIntermediate, 0.0, 0.0, 1.0, 0.0); double z0 = initialPosition.getZ(); double zIntermediate = finalPosition.getZ() + approachDistance.getDoubleValue(); double zf = finalPosition.getZ(); zPolynomial.setSexticUsingWaypoint(0.0, tIntermediate, trajectoryTime.getDoubleValue(), z0, 0.0, 0.0, zIntermediate, zf, 0.0, 0.0); currentPosition.set(initialPosition); currentVelocity.setToZero(); currentAcceleration.setToZero(); changeFrame(currentTrajectoryFrame, false); if (visualize) visualizeTrajectory(); }
public void initialize() { tangentialPlane.update(); currentTrajectoryFrame = initialPosition.getReferenceFrame(); changeFrame(tangentialPlane, false); currentTime.set(0.0); MathTools.checkIfInRange(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY); double tIntermediate = leaveTime.getDoubleValue(); xyPolynomial.setCubic(tIntermediate, trajectoryTime.getDoubleValue(), 0.0, 0.0, 1.0, 0.0); double z0 = initialPosition.getZ(); double zIntermediate = initialPosition.getZ() + leaveDistance.getDoubleValue(); double zf = finalPosition.getZ(); zPolynomial.setSexticUsingWaypoint(0.0, tIntermediate, trajectoryTime.getDoubleValue(), z0, 0.0, 0.0, zIntermediate, zf, 0.0, 0.0); currentPosition.set(initialPosition); currentVelocity.setToZero(); currentAcceleration.setToZero(); changeFrame(currentTrajectoryFrame, false); if (visualize) visualizeTrajectory(); }