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(); }
@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(); }
yPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPosition.getY(), initialVelocity.getY(), 0.0, finalPosition.getY(), finalVelocity.getY(), 0.0); zPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPosition.getZ(), initialVelocity.getZ(), 0.0, finalPosition.getZ(), finalVelocity.getZ(), 0.0);