public void setQuinticWithZeroTerminalAcceleration(double t0, double tFinal, double z0, double zd0, double zFinal, double zdFinal) { setQuintic(t0, tFinal, z0, zd0, 0.0, zFinal, zdFinal, 0.0); }
public void setQuintic(double t0, double tFinal, double z0, double zd0, double zdd0, double zf, double zdf, double zddf) { setTime(t0, tFinal); polynomial.setQuintic(t0, tFinal, z0, zd0, zdd0, zf, zdf, zddf); }
public void setQuinticWithZeroTerminalAcceleration(double t0, double tFinal, Point3DReadOnly z0, Vector3DReadOnly zd0, Point3DReadOnly zFinal, Vector3DReadOnly zdFinal) { for (int index = 0; index < 3; index++) getYoPolynomial(index).setQuintic(t0, tFinal, z0.getElement(index), zd0.getElement(index), 0.0, zFinal.getElement(index), zdFinal.getElement(index), 0.0); }
protected void setPolynomial() { this.polynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPositionProvider.getValue(), initialVelocityProvider.getValue(), 0.0, finalPositionProvider.getValue(), finalVelocityProvider.getValue(), 0.0); } }
public void initialize() { MathTools.checkIfInRange(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY); quinticParameterPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); reset(); if (visualize) visualizeTrajectory(); }
public void setQuintic(double t0, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 6); for (Direction direction : Direction.values) { polynomials.get(direction).setQuintic(t0, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), pf.get(direction), pdf.get(direction), pddf.get(direction)); } setYoVariables(t0, tf); }
@Override public void initialize() { currentTime.set(0.0); trajectoryTime.set(trajectoryTimeProvider.getValue()); parameterPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); updateInitialPosition(); updateFinalPosition(); }
@Override public void initialize() { currentTime.set(0.0); trajectoryTime.set(trajectoryTimeProvider.getValue()); parameterPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); updateInitialPosition(); updateFinalPosition(); }
public void setQuintic(double t0, double tFinal, Point3DReadOnly z0, Vector3DReadOnly zd0, Vector3DReadOnly zdd0, Point3DReadOnly zf, Vector3DReadOnly zdf, Vector3DReadOnly zddf) { for (int index = 0; index < 3; index++) getYoPolynomial(index).setQuintic(t0, tFinal, z0.getElement(index), zd0.getElement(index), zdd0.getElement(index), zf.getElement(index), zdf.getElement(index), zddf.getElement(index)); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); initialPositionProvider.getPosition(tempFramePoint); tempFramePoint.changeFrame(referenceFrame); double y = tempFramePoint.getY(); double x = tempFramePoint.getX(); z.set(tempFramePoint.getZ()); radius.set(Math.sqrt(x * x + y * y)); double initialAngle = Math.atan2(y, x); double finalAngle = initialAngle + desiredRotationAngleProvider.getValue(); anglePolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialAngle, 0.0, 0.0, finalAngle, 0.0, 0.0); }
@Override public void initialize() { currentTime.set(0.0); MathTools.checkIntervalContains(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY); parameterPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); currentOrientation.set(initialOrientation); currentAngularVelocity.setToZero(); currentAngularAcceleration.setToZero(); }
@Override public void initialize() { currentTime.set(0.0); MathTools.checkIfInRange(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY); parameterPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); currentOrientation.set(initialOrientation); currentAngularVelocity.setToZero(); currentAngularAcceleration.setToZero(); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); initialPositionProvider.getPosition(tempFramePoint); tempFramePoint.changeFrame(referenceFrame); double y = tempFramePoint.getY(); double x = tempFramePoint.getX(); z.set(tempFramePoint.getZ()); radius.set(Math.sqrt(x * x + y * y)); double initialAngle = Math.atan2(y, x); double finalAngle = initialAngle + desiredRotationAngleProvider.getValue(); anglePolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialAngle, 0.0, 0.0, finalAngle, 0.0, 0.0); }
private void computeInitialConstraintPolynomial(double initialTime, double blendDuration) { initialBlendTimeOffset.set(initialTime); initialBlendStartTime.set(0.0); initialBlendEndTime.set(blendDuration); for (int i = 0; i < 3; i++) { double startTime = initialBlendStartTime.getDoubleValue(); double endTime = initialBlendEndTime.getDoubleValue(); double positionError = initialConstraintPositionError.getElement(i); double velocityError = initialConstraintVelocityError.getElement(i); initialConstraintPolynomial[i].setQuintic(startTime, endTime, positionError, velocityError, 0, 0, 0, 0); } }
private void computeFinalConstraintPolynomial(double finalTime, double blendDuration) { finalBlendTimeOffset.set(finalTime); finalBlendStartTime.set(-blendDuration); finalBlendEndTime.set(0.0); for (int i = 0; i < 3; i++) { double startTime = finalBlendStartTime.getDoubleValue(); double endTime = finalBlendEndTime.getDoubleValue(); double positionError = finalConstraintPositionError.getElement(i); double velocityError = finalConstraintVelocityError.getElement(i); finalConstraintPolynomial[i].setQuintic(startTime, endTime, 0, 0, 0, positionError, velocityError, 0); } }
/** * Desired joint angles and velocities come from reading the joints, this method can override them those position and velocity values. * @param currentDesiredPosition Sets the desired joint position. * @param currentDesiredVelocity Sets the desired joint velocity. */ @Override public void initialize(double initialPosition, double initialVelocity) { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); this.polynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPosition, initialVelocity, 0.0, finalPosition.getDoubleValue(), 0.0, 0.0); currentPosition.set(initialPosition); currentVelocity.set(initialVelocity); currentAcceleration.set(0.0); }
/** * Desired joint angles and velocities come from reading the joints, this method can override them those position and velocity values. * @param currentDesiredPosition Sets the desired joint position. * @param currentDesiredVelocity Sets the desired joint velocity. */ @Override public void initialize(double initialPosition, double initialVelocity) { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); this.polynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPosition, initialVelocity, 0.0, finalPosition.getDoubleValue(), 0.0, 0.0); currentPosition.set(initialPosition); currentVelocity.set(initialVelocity); currentAcceleration.set(0.0); }
public void initialize() { double trajectoryTime = trajectoryTimeProvider.getValue(); MathTools.checkIntervalContains(trajectoryTime, 0.0, Double.POSITIVE_INFINITY); this.trajectoryTime.set(trajectoryTime); currentTime.set(0.0); parameterPolynomial.setQuintic(0.0, trajectoryTime, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); updateInitialOrientation(); updateFinalOrientation(); desiredOrientation.set(initialOrientation); desiredAngularVelocity.setToZero(); desiredAngularAcceleration.setToZero(); }
public void initialize() { double trajectoryTime = trajectoryTimeProvider.getValue(); MathTools.checkIfInRange(trajectoryTime, 0.0, Double.POSITIVE_INFINITY); this.trajectoryTime.set(trajectoryTime); currentTime.set(0.0); parameterPolynomial.setQuintic(0.0, trajectoryTime, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); updateInitialOrientation(); updateFinalOrientation(); desiredOrientation.set(initialOrientation); desiredAngularVelocity.setToZero(); desiredAngularAcceleration.setToZero(); }
@Override protected void setPolynomial() { if (Precision.equals(0.0, trajectoryTime.getDoubleValue())) { polynomial.setLinear(0.0, initialPositionProvider.getValue(), initialVelocityProvider.getValue()); } else { polynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPositionProvider.getValue(), initialVelocityProvider.getValue(), 0.0, finalPositionProvider.getValue(), finalVelocityProvider.getValue(), 0.0); } } }