public void setInitialPoseWithInitialVelocity(FramePoint initialPosition, FrameVector initialVelocity, FrameOrientation initialOrientation, FrameVector initialAngularVelocity) { this.initialPosition.set(initialPosition); this.initialVelocity.set(initialVelocity); this.initialOrientation.set(initialOrientation); this.initialAngularVelocity.set(initialAngularVelocity); }
public void setInitialPoseWithInitialVelocity(FramePose initialPose, FrameVector initialVelocity, FrameVector initialAngularVelocity) { initialPose.getPoseIncludingFrame(tempPosition, tempOrientation); this.initialPosition.set(tempPosition); this.initialVelocity.set(initialVelocity); this.initialOrientation.set(tempOrientation); this.initialAngularVelocity.set(initialAngularVelocity); }
public void setIncludingFrame(FrameTuple3DReadOnly frameTuple) { multipleFramesHelper.switchCurrentReferenceFrame(frameTuple.getReferenceFrame()); set(frameTuple); }
public void setIncludingFrame(FrameTuple<?, ?> frameTuple) { multipleFramesHelper.switchCurrentReferenceFrame(frameTuple.getReferenceFrame()); set(frameTuple); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { vector.set(this); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); frameVector.setIncludingFrame(currentReferenceFrame, vector); frameVector.changeFrame(desiredFrame); vector.set(frameVector); set(vector); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(vector); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); frameVector.setIncludingFrame(currentReferenceFrame, vector); frameVector.changeFrame(desiredFrame); frameVector.get(vector); set(vector); }
public void setInitialClearance(FrameVector initialDirection, double leaveDistance) { this.initialDirection.set(initialDirection); this.initialDirection.normalize(); this.initialDirection.get(tempVector); GeometryTools.getAxisAngleFromZUpToVector(tempVector, axisAngleToWorld); rotationPlane.setIncludingFrame(this.initialDirection.getReferenceFrame(), axisAngleToWorld); this.leaveDistance.set(leaveDistance); }
public void setFinalApproach(FrameVector finalDirection, double approachDistance) { this.finalDirection.set(finalDirection); this.finalDirection.normalize(); this.finalDirection.get(tempVector); tempVector.negate(); GeometryTools.getAxisAngleFromZUpToVector(tempVector, axisAngleToWorld); rotationPlane.setIncludingFrame(this.finalDirection.getReferenceFrame(), axisAngleToWorld); this.approachDistance.set(approachDistance); }
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 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); }
currentVelocity.set(xPolynomial.getVelocity(), yPolynomial.getVelocity(), zPolynomial.getVelocity()); currentAcceleration.set(xPolynomial.getAcceleration(), yPolynomial.getAcceleration(), zPolynomial.getAcceleration()); currentAngularVelocity.set(tempCurrentAngularVelocity); currentVelocity.set(finalVelocity); currentAcceleration.set(0.0, 0.0, 0.0); currentAngularVelocity.set(finalAngularVelocity); currentAcceleration.set(0.0, 0.0, 0.0);