public String toStringForASingleReferenceFrame(ReferenceFrame referenceFrame) { getFrameOrientationIncludingFrame(frameOrientation); frameOrientation.changeFrame(referenceFrame); return frameOrientation.toStringAsQuaternion(); }
public String toStringForASingleReferenceFrame(ReferenceFrame referenceFrame) { getFrameOrientationIncludingFrame(frameOrientation); frameOrientation.changeFrame(referenceFrame); return frameOrientation.toStringAsQuaternion(); }
public void setInitialOrientation(YoFrameQuaternion initialOrientation) { initialOrientation.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); this.initialOrientation.set(tempOrientation); }
public void setFinalOrientation(FramePose finalPose) { finalPose.getOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); this.finalOrientation.set(tempOrientation); }
public String toString() { FrameOrientation frameOrientation = new FrameOrientation(poseReferenceFrame); frameOrientation.changeFrame(ReferenceFrame.getWorldFrame()); double[] ypr = frameOrientation.getYawPitchRoll(); String yawPitchRoll = "YawPitchRoll = " + Arrays.toString(ypr); return "id: " + id + " - pose: " + poseReferenceFrame + "\n\tYawPitchRoll= {" + yawPitchRoll + "}"; }
private void updateInitialOrientation() { initialOrientationProvider.getOrientation(tempInitialOrientation); tempInitialOrientation.changeFrame(initialOrientation.getReferenceFrame()); initialOrientation.set(tempInitialOrientation); initialOrientation.checkQuaternionIsUnitMagnitude(); }
public void convertToFramePose(FramePoint endEffectorPositionIn, FrameOrientation endEffectorOrientationIn, FramePose poseToPack) { endEffectorPosition.setIncludingFrame(endEffectorPositionIn); endEffectorPosition.changeFrame(endEffectorFrame); endEffectorOrientation.setIncludingFrame(endEffectorOrientationIn); endEffectorOrientation.changeFrame(endEffectorFrame); poseToPack.setPoseIncludingFrame(endEffectorPosition, endEffectorOrientation); }
private void updateFinalOrientation() { finalOrientationProvider.getOrientation(tempFinalOrientation); tempFinalOrientation.changeFrame(finalOrientation.getReferenceFrame()); finalOrientation.set(tempFinalOrientation); finalOrientation.checkQuaternionIsUnitMagnitude(); }
public static Quat4d getTransformedQuat(Quat4d quat4d, RigidBodyTransform transform3D) { ReferenceFrame ending = ReferenceFrame.constructARootFrame("ending", false, true, true); ReferenceFrame starting = ReferenceFrame.constructFrameWithUnchangingTransformToParent("starting", ending, transform3D, false, true, true); FrameOrientation start = new FrameOrientation(starting, quat4d); start.changeFrame(ending); return start.getQuaternionCopy(); }
public void setInitialPose(FramePose initialPose) { initialPose.getPoseIncludingFrame(initialPosition, initialOrientation); initialPosition.changeFrame(trajectoryFrame); initialOrientation.changeFrame(trajectoryFrame); yoInitialPosition.set(initialPosition); yoInitialOrientation.set(initialOrientation); }
@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); }
/** Go instantly to zero, no smooth interpolation. */ public void setToZeroInSupportFoot(RobotSide supportSide) { ReferenceFrame supportAnkleZUp = ankleZUpFrames.get(supportSide); tempOrientation.setToZero(supportAnkleZUp); tempOrientation.changeFrame(worldFrame); initialPelvisOrientation.set(tempOrientation); finalPelvisOrientation.set(tempOrientation); desiredPelvisOrientation.set(tempOrientation); initialize(supportAnkleZUp); }
/** Move towards zero smoothly within the given swing time */ public void moveToZeroInSupportFoot(RobotSide supportSide) { desiredPelvisOrientation.getFrameOrientationIncludingFrame(tempOrientation); initialPelvisOrientation.set(tempOrientation); ReferenceFrame supportAnkleZUp = ankleZUpFrames.get(supportSide); tempOrientation.setToZero(supportAnkleZUp); tempOrientation.changeFrame(worldFrame); finalPelvisOrientation.set(tempOrientation); initialize(supportAnkleZUp); }
public void initialize() { time.set(0.0); FrameOrientation orientationToPack = new FrameOrientation(ReferenceFrame.getWorldFrame()); orientationProvider.getOrientation(orientationToPack); orientationToPack.changeFrame(orientation.getReferenceFrame()); orientation.set(orientationToPack); }
public void setAndMatchFrame(YoFrameOrientation yoFrameOrientation, boolean notifyListeners) { yoFrameOrientation.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.changeFrame(getReferenceFrame()); tempFrameOrientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
public void setAndMatchFrame(YoFrameQuaternion yoFrameQuaternion, boolean notifyListeners) { yoFrameQuaternion.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.changeFrame(getReferenceFrame()); tempFrameOrientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
public void setToHoldCurrent(ReferenceFrame trajectoryFrame) { tempOrientation.setToZero(pelvisFrame); tempOrientation.changeFrame(worldFrame); initialPelvisOrientation.set(tempOrientation); finalPelvisOrientation.set(tempOrientation); desiredPelvisOrientation.set(tempOrientation); resetOrientationOffset(); initialize(trajectoryFrame); }
public void setAndMatchFrame(FramePose framePose, boolean notifyListeners) { framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); tempFramePoint.changeFrame(getReferenceFrame()); tempFrameOrientation.changeFrame(getReferenceFrame()); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
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); }
private FramePose offsetPointFromChestInWorldFrame(double x, double y, double z, double yaw, double pitch, double roll) { FramePoint point1 = new FramePoint(referenceFrames.getChestFrame(), x, y, z); point1.changeFrame(ReferenceFrame.getWorldFrame()); FrameOrientation orient = new FrameOrientation(referenceFrames.getChestFrame(), yaw, pitch, roll); orient.changeFrame(ReferenceFrame.getWorldFrame()); FramePose pose = new FramePose(point1, orient); return pose; }