/** * Before Initializing the isSolvable() method should be called to confirm that the limit conditions are within the bounds */ @Override public void initialize() { piInteger.set(0); currentTime.set(0.0); trajectoryTimeScale.set(1.0 / trajectoryTime.getDoubleValue()); if (initialOrientation.dot(finalOrientation) < 0.0) finalOrientation.negate(); updateControlQuaternions(); currentOrientation.set(initialOrientation); currentAngularVelocity.set(initialAngularVelocity); currentAngularAcceleration.setToZero(); }
@Override public void initialize() { currentTime.set(0.0); initialAngularVelocityMagnitude.set(initialAngularVelocity.length()); finalAngularVelocityMagnitude.set(finalAngularVelocity.length()); maxAngularVelocityMagnitudeAtLimits.set(PI / trajectoryTime.getDoubleValue()); initialDriftSaturated.set(initialAngularVelocityMagnitude.getDoubleValue() > maxAngularVelocityMagnitudeAtLimits.getDoubleValue()); finalDriftSaturated.set(finalAngularVelocityMagnitude.getDoubleValue() > maxAngularVelocityMagnitudeAtLimits.getDoubleValue()); parameterPolynomial.setQuintic(0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); if (initialOrientation.dot(finalOrientation) < 0.0) finalOrientation.negate(); currentOrientation.set(initialOrientation); currentAngularVelocity.set(initialAngularVelocity); currentAngularAcceleration.setToZero(); }