@Override public String toString() { String ret = ""; ReferenceFrame currentFrame = initialPosition.getReferenceFrame(); ret += "Current time: " + currentTime.getDoubleValue() + ", trajectory time: " + trajectoryTime.getDoubleValue(); ret += "\nCurrent position: " + currentPosition.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent velocity: " + currentVelocity.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent acceleration: " + currentAcceleration.toStringForASingleReferenceFrame(currentFrame); return ret; } }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(currentPosition.getReferenceFrame()); framePoseToPack.setPosition(currentPosition.getFrameTuple()); currentOrientation.get(temp); framePoseToPack.setOrientation(temp); }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(currentPosition.getReferenceFrame()); framePoseToPack.setPosition(currentPosition.getFrameTuple()); currentOrientation.get(temp); framePoseToPack.setOrientation(temp); }
public String toString() { String ret = ""; ReferenceFrame currentFrame = initialPosition.getReferenceFrame(); ret += "Current time: " + currentTime.getDoubleValue() + ", trajectory time: " + trajectoryTime.getDoubleValue(); ret += "\nCurrent position: " + currentPosition.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent velocity: " + currentVelocity.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent acceleration: " + currentAcceleration.toStringForASingleReferenceFrame(currentFrame); return ret; } }
@Override public String toString() { String ret = ""; ReferenceFrame currentFrame = initialPosition.getReferenceFrame(); ret += "Current time: " + currentTime.getDoubleValue() + ", trajectory time: " + trajectoryTime.getDoubleValue(); ret += "\nCurrent position: " + currentPosition.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent velocity: " + currentVelocity.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent acceleration: " + currentAcceleration.toStringForASingleReferenceFrame(currentFrame); return ret; } }
public void setInitialPose(FramePose initialPose) { initialPose.getPoseIncludingFrame(tempPosition, tempOrientation); tempPosition.changeFrame(initialPosition.getReferenceFrame()); initialPosition.set(tempPosition); tempOrientation.changeFrame(initialOrientation.getReferenceFrame()); initialOrientation.set(tempOrientation); initialOrientationForViz.setAndMatchFrame(tempOrientation); }
public String toString() { String ret = ""; ReferenceFrame currentFrame = initialPosition.getReferenceFrame(); ret += "Current time: " + currentTime.getDoubleValue() + ", trajectory time: " + trajectoryTime.getDoubleValue(); ret += "\nCurrent position: " + currentPosition.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent velocity: " + currentVelocity.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent acceleration: " + currentAcceleration.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent orientation: " + currentOrientation.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent angular velocity: " + currentAngularVelocity.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent angular acceleration: " + currentAngularAcceleration.toStringForASingleReferenceFrame(currentFrame); return ret; } }
public void setInitialLeadOut(FramePoint initialPosition, FrameVector initialDirection, double leaveDistance) { this.initialPosition.set(initialPosition); this.initialDirection.set(initialDirection); this.initialDirection.normalize(); this.initialDirection.get(tempVector); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); initialDistortionPose.setToZero(this.initialPosition.getReferenceFrame()); initialDistortionPose.setPosition(initialPosition); initialDistortionPose.setOrientation(tempAxisAngle); this.leaveDistance.set(leaveDistance); }
public String toString() { String ret = ""; ReferenceFrame currentFrame = initialPosition.getReferenceFrame(); ret += "Current time: " + currentTime.getDoubleValue() + ", trajectory time: " + trajectoryTime.getDoubleValue(); ret += "\nCurrent position: " + currentPosition.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent velocity: " + currentVelocity.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent acceleration: " + currentAcceleration.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent orientation: " + currentOrientation.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent angular velocity: " + currentAngularVelocity.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent angular acceleration: " + currentAngularAcceleration.toStringForASingleReferenceFrame(currentFrame); return ret; } }
public void setFinalLeadIn(FramePoint finalPosition, FrameVector finalDirection, double approachDistance) { this.finalPosition.set(finalPosition); this.finalDirection.set(finalDirection); this.finalDirection.normalize(); this.finalDirection.get(tempVector); tempVector.negate(); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); finalDistortionPose.setToZero(this.finalPosition.getReferenceFrame()); finalDistortionPose.setPosition(finalPosition); finalDistortionPose.setOrientation(tempAxisAngle); this.approachDistance.set(approachDistance); }
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(); }
@Override public void initialize() currentTrajectoryFrame = initialPosition.getReferenceFrame();