public void setSexticUsingWaypoint(double t0, double tIntermediate, double tFinal, double z0, double zd0, double zdd0, double zIntermediate, double zf, double zdf, double zddf) { setTime(t0, tFinal); polynomial.setSexticUsingWaypoint(t0, tIntermediate, tFinal, z0, zd0, zdd0, zIntermediate, zf, zdf, zddf); }
public void setSexticUsingWaypoint(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FramePoint p1, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 7); for (Direction direction : Direction.values) { polynomials.get(direction).setSexticUsingWaypoint(t0, t1, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), p1.get(direction), pf.get(direction), pdf.get(direction), pddf.get(direction)); } setYoVariables(t0, tf); }
public void setSexticUsingWaypoint(double t0, double tIntermediate, double tFinal, Point3DReadOnly z0, Vector3DReadOnly zd0, Vector3DReadOnly zdd0, Point3DReadOnly zIntermediate, Point3DReadOnly zf, Vector3DReadOnly zdf, Vector3DReadOnly zddf) { for (int index = 0; index < 3; index++) getYoPolynomial(index).setSexticUsingWaypoint(t0, tIntermediate, tFinal, z0.getElement(index), zd0.getElement(index), zdd0.getElement(index), zIntermediate.getElement(index), zf.getElement(index), zdf.getElement(index), zddf.getElement(index)); }
public void setSexticUsingWaypoint(double t0, double t1, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pdd0, FramePoint3D p1, FramePoint3D pf, FrameVector3D pdf, FrameVector3D pddf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 7); p0.checkReferenceFrameMatch(referenceFrame); p1.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdd0.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); pddf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setSexticUsingWaypoint(t0, t1, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pdd0.getElement(axis.ordinal()), p1.getElement( axis.ordinal()), pf.getElement(axis.ordinal()), pdf.getElement(axis.ordinal()), pddf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
@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(); }