/** * Change the current reference frame and set to zero the coordinates (different from changeFrame() ). * @return the previous current reference frame. */ @Override public ReferenceFrame switchCurrentReferenceFrame(ReferenceFrame referenceFrame) { ReferenceFrame previousReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(referenceFrame); setToZero(); return previousReferenceFrame; }
/** * Change the current reference frame and set to zero the coordinates (different from changeFrame() ). * @return the previous current reference frame. */ @Override public ReferenceFrame switchCurrentReferenceFrame(ReferenceFrame referenceFrame) { ReferenceFrame previousReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(referenceFrame); setToZero(); return previousReferenceFrame; }
public void setInitialPoseWithoutInitialVelocity(FramePose initialPose) { initialPose.getPoseIncludingFrame(tempPosition, tempOrientation); this.initialPosition.set(tempPosition); this.initialVelocity.setToZero(); ; this.initialOrientation.set(tempOrientation); this.initialAngularVelocity.setToZero(); }
public void setFinalPoseWithoutFinalVelocity(FramePoint finalPosition, FrameOrientation finalOrientation) { this.finalPosition.set(finalPosition); this.finalOrientation.set(finalOrientation); finalPositionForViz.setAndMatchFrame(finalPosition); finalOrientationForViz.setAndMatchFrame(finalOrientation); this.finalVelocity.setToZero(); this.finalAngularVelocity.setToZero(); }
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(); }