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); }
@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; }
YoFrameVectorInMultipleFrames currentVelocity = new YoFrameVectorInMultipleFrames(currentVelocityName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentAcceleration = new YoFrameVectorInMultipleFrames(currentAccelerationName, registry, referenceFrame);
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 YoFrameVector buildUpdatedYoFrameVectorForVisualizationOnly() { if (yoFrameVectorInWorld == null) { final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); if (!isReferenceFrameRegistered(worldFrame)) registerReferenceFrame(worldFrame); yoFrameVectorInWorld = new YoFrameVector(namePrefix, worldFrame.getName(), worldFrame, registry); attachVariableChangedListener(new VariableChangedListener() { private final FrameVector localFrameVector = new FrameVector(); private final YoFrameVector vector = yoFrameVectorInWorld; @Override public void variableChanged(YoVariable<?> v) { getFrameTupleIncludingFrame(localFrameVector); vector.setAndMatchFrame(localFrameVector); } }); } return yoFrameVectorInWorld; }
@Override public void getVelocity(FrameVector velocityToPack) { currentVelocity.getFrameTupleIncludingFrame(velocityToPack); }
initialDirection = new YoFrameVectorInMultipleFrames(namePrefix + "InitialDirection", registry, referenceFrame, tangentialPlane); currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame, tangentialPlane); currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame, tangentialPlane); currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame, tangentialPlane); final YoGraphicPosition finalPositionViz = new YoGraphicPosition(namePrefix + "FinalPosition", finalPosition, 0.02, YoAppearance.Red()); final YoGraphicVector initialDirectionViz = new YoGraphicVector(namePrefix + "InitialDirection", initialPosition.buildUpdatedYoFramePointForVisualizationOnly(), initialDirection.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.BlueViolet()); yoGraphicsList = new YoGraphicsList(namePrefix + "FinalApproachTraj");
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); }
@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); }
xPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPosition.getX(), initialVelocity.getX(), 0.0, finalPosition.getX(), finalVelocity.getX(), 0.0); yPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPosition.getY(), initialVelocity.getY(), 0.0, finalPosition.getY(), finalVelocity.getY(), 0.0); zPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPosition.getZ(), initialVelocity.getZ(), 0.0, finalPosition.getZ(), finalVelocity.getZ(), 0.0); initialAngularVelocity.getFrameTupleIncludingFrame(copyOfInitialAngularVelocity); finalAngularVelocity.getFrameTupleIncludingFrame(copyOfFinalAngularVelocity);
currentVelocity.setToZero(); currentAcceleration.setToZero(); currentAngularVelocity.setToZero(); currentAngularAcceleration.setToZero(); currentVelocity.subAndScale(alphaVel, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaAcc, finalPosition, initialPosition);
@Override public void setToNaN(ReferenceFrame desiredReferenceFrame) { setToNaN(); multipleFramesHelper.switchCurrentReferenceFrame(desiredReferenceFrame); } }
YoFrameVectorInMultipleFrames initialVelocity = new YoFrameVectorInMultipleFrames(initialVelocityName, registry, referenceFrame); YoFrameVectorInMultipleFrames finalVelocity = new YoFrameVectorInMultipleFrames(finalVelocityName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentVelocity = new YoFrameVectorInMultipleFrames(currentVelocityName, registry, referenceFrame); YoFrameVectorInMultipleFrames currentAcceleration = new YoFrameVectorInMultipleFrames(currentAccelerationName, registry, referenceFrame);
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 YoFrameVector3D buildUpdatedYoFrameVectorForVisualizationOnly() { if (yoFrameVectorInWorld == null) { final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); if (!isReferenceFrameRegistered(worldFrame)) registerReferenceFrame(worldFrame); yoFrameVectorInWorld = new YoFrameVector3D(namePrefix, worldFrame.getName(), worldFrame, registry); attachVariableChangedListener(new VariableChangedListener() { private final FrameVector3D localFrameVector = new FrameVector3D(); private final YoFrameVector3D vector = yoFrameVectorInWorld; @Override public void notifyOfVariableChange(YoVariable<?> v) { localFrameVector.setIncludingFrame(YoFrameVectorInMultipleFrames.this); vector.setMatchingFrame(localFrameVector); } }); } return yoFrameVectorInWorld; }
@Override public void getAcceleration(FrameVector accelerationToPack) { currentAcceleration.getFrameTupleIncludingFrame(accelerationToPack); }
initialDirection = new YoFrameVectorInMultipleFrames(namePrefix + "InitialDirection", registry, referenceFrame, distortedPlane); finalDirection = new YoFrameVectorInMultipleFrames(namePrefix + "FinalDirection", registry, referenceFrame, distortedPlane); currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame, distortedPlane); currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame, distortedPlane); currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame, distortedPlane); final YoGraphicPosition finalPositionViz = new YoGraphicPosition(namePrefix + "FinalPosition", finalPosition, 0.02, YoAppearance.Red()); final YoGraphicVector initialDirectionViz = new YoGraphicVector(namePrefix + "InitialDirection", initialPosition.buildUpdatedYoFramePointForVisualizationOnly(), initialDirection.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.BlueViolet()); final YoGraphicVector finalDirectionViz = new YoGraphicVector(namePrefix + "FinalDirection", finalPosition.buildUpdatedYoFramePointForVisualizationOnly(), finalDirection.buildUpdatedYoFrameVectorForVisualizationOnly(), 0.2, YoAppearance.Red()); distortedPlanePose = new YoFramePose(namePrefix + "DistortedPlane", ReferenceFrame.getWorldFrame(), registry);
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); }