public void compute(double time) { tangentialPlane.update(); changeFrame(tangentialPlane, false); this.currentTime.set(time); double tIntermediate = leaveTime.getDoubleValue(); xyPolynomial.compute(MathTools.clipToMinMax(time, tIntermediate, trajectoryTime.getDoubleValue())); boolean shouldBeZero = currentTime.getDoubleValue() >= tIntermediate || currentTime.getDoubleValue() < 0.0; double alphaDot = shouldBeZero ? 0.0 : xyPolynomial.getVelocity(); double alphaDDot = shouldBeZero ? 0.0 : xyPolynomial.getAcceleration(); currentPosition.interpolate(initialPosition, finalPosition, xyPolynomial.getPosition()); currentVelocity.subAndScale(alphaDot, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaDDot, finalPosition, initialPosition); zPolynomial.compute(MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue())); shouldBeZero = isDone() || currentTime.getDoubleValue() < 0.0; alphaDot = shouldBeZero ? 0.0 : zPolynomial.getVelocity(); alphaDDot = shouldBeZero ? 0.0 : zPolynomial.getAcceleration(); currentPosition.setZ(zPolynomial.getPosition()); currentVelocity.setZ(zPolynomial.getVelocity()); currentAcceleration.setZ(zPolynomial.getAcceleration()); changeFrame(currentTrajectoryFrame, false); }
@Override public void compute(double time) { currentTime.set(time); double t1 = leaveTime.getDoubleValue(); double t2 = trajectoryTime.getDoubleValue() - approachTime.getDoubleValue(); double tf = trajectoryTime.getDoubleValue(); xyPolynomial.compute(MathTools.clipToMinMax(time, t1, t2)); currentDistortionPose.interpolate(initialDistortionPose, finalDistortionPose, xyPolynomial.getPosition()); distortedPlanePose.setAndMatchFrame(currentDistortionPose); distortedPlane.update(); changeFrame(distortedPlane, false); boolean shouldBeZero = currentTime.getDoubleValue() >= t2 || currentTime.getDoubleValue() < t1; double alphaDot = shouldBeZero ? 0.0 : xyPolynomial.getVelocity(); double alphaDDot = shouldBeZero ? 0.0 : xyPolynomial.getAcceleration(); currentPosition.interpolate(initialPosition, finalPosition, xyPolynomial.getPosition()); currentVelocity.subAndScale(alphaDot, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaDDot, finalPosition, initialPosition); zPolynomial.compute(MathTools.clipToMinMax(time, 0.0, tf)); currentPosition.setZ(zPolynomial.getPosition()); currentVelocity.setZ(zPolynomial.getVelocity()); currentAcceleration.setZ(zPolynomial.getAcceleration()); changeFrame(currentTrajectoryFrame, false); }
@Override public void compute(double time) { tangentialPlane.update(); changeFrame(tangentialPlane, false); currentTime.set(time); double tIntermediate = trajectoryTime.getDoubleValue() - approachTime.getDoubleValue(); time = MathTools.clipToMinMax(time, 0.0, tIntermediate); xyPolynomial.compute(MathTools.clipToMinMax(time, 0.0, tIntermediate)); boolean shouldBeZero = currentTime.getDoubleValue() >= tIntermediate || currentTime.getDoubleValue() < 0.0; double alphaDot = shouldBeZero ? 0.0 : xyPolynomial.getVelocity(); double alphaDDot = shouldBeZero ? 0.0 : xyPolynomial.getAcceleration(); currentPosition.interpolate(initialPosition, finalPosition, xyPolynomial.getPosition()); currentVelocity.subAndScale(alphaDot, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaDDot, finalPosition, initialPosition); time = MathTools.clipToMinMax(currentTime.getDoubleValue(), 0.0, trajectoryTime.getDoubleValue()); zPolynomial.compute(time); shouldBeZero = isDone() || currentTime.getDoubleValue() < 0.0; alphaDot = shouldBeZero ? 0.0 : zPolynomial.getVelocity(); alphaDDot = shouldBeZero ? 0.0 : zPolynomial.getAcceleration(); currentPosition.setZ(zPolynomial.getPosition()); currentVelocity.setZ(zPolynomial.getVelocity()); currentAcceleration.setZ(zPolynomial.getAcceleration()); changeFrame(currentTrajectoryFrame, false); }