public ConstantPoseTrajectoryGenerator(String namePrefix, boolean allowMultipleFrames, ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry) { this.allowMultipleFrames = allowMultipleFrames; YoVariableRegistry registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames(namePrefix + "ConstantPosition", registry, referenceFrame); position = yoFramePointInMultipleFrames; YoFrameQuaternionInMultipleFrames yoFrameQuaternionInMultiplesFrames = new YoFrameQuaternionInMultipleFrames(namePrefix + "ConstantOrientation", registry, referenceFrame); orientation = yoFrameQuaternionInMultiplesFrames; multipleFramesHolders = new ArrayList<YoMultipleFramesHolder>(); multipleFramesHolders.add(yoFramePointInMultipleFrames); multipleFramesHolders.add(yoFrameQuaternionInMultiplesFrames); }
/** * 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); set(0.0, 0.0, 0.0); return previousReferenceFrame; }
@Override public String toString() { String ret = ""; List<ReferenceFrame> referenceFrames = new ArrayList<ReferenceFrame>(); multipleFramesHelper.getRegisteredReferenceFrames(referenceFrames); for (int i = 0; i < referenceFrames.size(); i++) { if (i > 0) ret += "\n"; ret += toStringForASingleReferenceFrame(referenceFrames.get(i)); } return ret; }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(quaternion); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); frameOrientation.setIncludingFrame(currentReferenceFrame, quaternion); frameOrientation.changeFrame(desiredFrame); frameOrientation.getQuaternion(quaternion); set(quaternion); }
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); }
currentAcceleration.setToZero(); currentOrientation.set(finalOrientation); currentAngularVelocity.setToZero(); currentAngularAcceleration.setToZero(); currentAcceleration.subAndScale(alphaAcc, finalPosition, initialPosition); currentOrientation.interpolate(initialOrientation, finalOrientation, quinticParameterPolynomial.getPosition()); orientationInterpolationCalculator.computeAngularVelocity(currentAngularVelocity, initialOrientation, finalOrientation, alphaAngVel); orientationInterpolationCalculator.computeAngularAcceleration(currentAngularAcceleration, initialOrientation, finalOrientation, alphaAngAcc); currentOrientation.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(currentOrientationForViz.getReferenceFrame()); currentOrientationForViz.set(tempOrientation);
public void getOrientation(FrameOrientation orientationToPack) { currentOrientation.getFrameOrientationIncludingFrame(orientationToPack); }
interpolationFrameForViz.set(copyOfInitialOrientation); trajectoryFrame = initialOrientation.getReferenceFrame(); finalVelocity.getZ(), 0.0); initialOrientation.getFrameOrientationIncludingFrame(copyOfInitialOrientation); finalOrientation.getFrameOrientationIncludingFrame(copyOfFinalOrientation); initialAngularVelocity.getFrameTupleIncludingFrame(copyOfInitialAngularVelocity); finalAngularVelocity.getFrameTupleIncludingFrame(copyOfFinalAngularVelocity);
@Override public void setToNaN(ReferenceFrame desiredReferenceFrame) { setToNaN(); multipleFramesHelper.switchCurrentReferenceFrame(desiredReferenceFrame); } }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(currentPosition.getReferenceFrame()); framePoseToPack.setPosition(currentPosition.getFrameTuple()); currentOrientation.get(temp); framePoseToPack.setOrientation(temp); }
public void getOrientation(FrameOrientation orientationToPack) { currentOrientation.getFrameOrientationIncludingFrame(orientationToPack); }
@Override public void setToNaN(ReferenceFrame desiredReferenceFrame) { setToNaN(); multipleFramesHelper.switchCurrentReferenceFrame(desiredReferenceFrame); } }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(currentPosition.getReferenceFrame()); framePoseToPack.setPosition(currentPosition.getFrameTuple()); currentOrientation.get(temp); framePoseToPack.setOrientation(temp); }
public ConstantPoseTrajectoryGenerator(String namePrefix, boolean allowMultipleFrames, ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry) { this.allowMultipleFrames = allowMultipleFrames; YoVariableRegistry registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames(namePrefix + "ConstantPosition", registry, referenceFrame); position = yoFramePointInMultipleFrames; YoFrameQuaternionInMultipleFrames yoFrameQuaternionInMultiplesFrames = new YoFrameQuaternionInMultipleFrames(namePrefix + "ConstantOrientation", registry, referenceFrame); orientation = yoFrameQuaternionInMultiplesFrames; multipleFramesHolders = new ArrayList<YoMultipleFramesHolder>(); multipleFramesHolders.add(yoFramePointInMultipleFrames); multipleFramesHolders.add(yoFrameQuaternionInMultiplesFrames); }
public void setInitialPose(FramePoint initialPosition, FrameOrientation initialOrientation) { this.initialPosition.set(initialPosition); this.initialOrientation.set(initialOrientation); initialOrientationForViz.setAndMatchFrame(initialOrientation); }
@Override public String toString() { String ret = ""; List<ReferenceFrame> referenceFrames = new ArrayList<ReferenceFrame>(); multipleFramesHelper.getRegisteredReferenceFrames(referenceFrames); for (int i = 0; i < referenceFrames.size(); i++) { if (i > 0) ret += "\n"; ret += toStringForASingleReferenceFrame(referenceFrames.get(i)); } return ret; }
public String toStringForASingleReferenceFrame(ReferenceFrame referenceFrame) { getFrameOrientationIncludingFrame(frameOrientation); frameOrientation.changeFrame(referenceFrame); return frameOrientation.toStringAsQuaternion(); }
YoFrameQuaternionInMultipleFrames initialOrientation = new YoFrameQuaternionInMultipleFrames(initialOrientationName, registry, referenceFrame); YoFrameQuaternionInMultipleFrames finalOrientation = new YoFrameQuaternionInMultipleFrames(finalOrientationName, registry, referenceFrame); YoFrameQuaternionInMultipleFrames currentOrientation = new YoFrameQuaternionInMultipleFrames(currentOrientationName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentAngularVelocity = new YoFrameVectorInMultipleFrames(currentAngularVelocityName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentAngularAcceleration = new YoFrameVectorInMultipleFrames(currentAngularAccelerationName, registry, referenceFrame);
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 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; } }