public void getOrientation(FrameOrientation orientationToPack) { orientationToPack.setToZero(endEffectorFrame); } }
public FrameOrientation(ReferenceFrame referenceFrame) { super(referenceFrame, new TransformableQuat4d()); this.quaternion = this.getGeometryObject(); setToZero(referenceFrame); }
public void setToZero() { frameOrientation.setToZero(getReferenceFrame()); getYoValuesFromFrameOrientation(); }
public void getOrientationIncludingFrame(FrameOrientation orientationToPack) { orientationToPack.setToZero(getReferenceFrame()); geometryObject.getOrientation(orientationToPack.getQuaternion()); }
public void getOrientationIncludingFrame(FrameOrientation orientationToPack) { orientationToPack.setToZero(getReferenceFrame()); geometryObject.getOrientation(orientationToPack.getQuaternion()); }
/** * Sets the orientation of this to the origin of the passed in ReferenceFrame. * * @param referenceFrame */ public void setFromReferenceFrame(ReferenceFrame referenceFrame, boolean notifyListeners) { frameOrientation.setToZero(referenceFrame); frameOrientation.changeFrame(getReferenceFrame()); getYoValuesFromFrameOrientation(notifyListeners); }
public void getOrientationIncludingFrame(FrameOrientation orientationToPack) { orientationToPack.setToZero(getReferenceFrame()); geometryObject.getOrientation(orientationToPack.getQuaternion()); }
public void getOrientationIncludingFrame(FrameOrientation orientationToPack) { orientationToPack.setToZero(getReferenceFrame()); geometryObject.getOrientation(orientationToPack.getQuaternion()); }
private void putYoValuesIntoFrameOrientation() { tempFrameOrientation.setToZero(getReferenceFrame()); tempFrameOrientation.setYawPitchRoll(yaw.getDoubleValue(), pitch.getDoubleValue(), roll.getDoubleValue()); }
public void getFrameOrientationIncludingFrame(FrameOrientation orientationToPack) { orientationToPack.setToZero(getReferenceFrame()); orientationToPack.setYawPitchRoll(yaw.getDoubleValue(), pitch.getDoubleValue(), roll.getDoubleValue()); }
public void interpolate(YoFrameOrientation orientationOne, YoFrameOrientation orientationTwo, double alpha) { orientationOne.putYoValuesIntoFrameOrientation(); orientationTwo.putYoValuesIntoFrameOrientation(); tempFrameOrientation.setToZero(getReferenceFrame()); tempFrameOrientation.interpolate(orientationOne.tempFrameOrientation, orientationTwo.tempFrameOrientation, alpha); this.set(tempFrameOrientation); }
public void initialize() { if (hasBeenInitialized.getBooleanValue()) return; hasBeenInitialized.set(true); initialOrientation.setToZero(chestFrame); taskspaceChestControlState.holdOrientation(initialOrientation); requestState(taskspaceChestControlState.getStateEnum()); }
public void resetOrientationOffset() { tempOrientation.setToZero(desiredPelvisFrame); tempAngularVelocity.setToZero(desiredPelvisFrame); orientationOffsetTrajectoryGenerator.clear(); orientationOffsetTrajectoryGenerator.switchTrajectoryFrame(desiredPelvisFrame); orientationOffsetTrajectoryGenerator.appendWaypoint(0.0, tempOrientation, tempAngularVelocity); orientationOffsetTrajectoryGenerator.initialize(); }
public void holdCurrentOrientation() { desiredOrientation.setToZero(headFrame); desiredOrientation.changeFrame(chestFrame); desiredAngularVelocity.setToZero(chestFrame); trajectoryGenerator.clear(); trajectoryGenerator.switchTrajectoryFrame(chestFrame); trajectoryGenerator.appendWaypoint(0.0, desiredOrientation, desiredAngularVelocity); }
public void initialize() { if (hasBeenInitialized.getBooleanValue()) return; hasBeenInitialized.set(true); initialOrientation.setToZero(headFrame); taskspaceHeadControlState.handleGoHome(defaultTrajectoryTime, initialOrientation); requestedState.set(taskspaceHeadControlState.getStateEnum()); }
public void setInitialPoseToMatchReferenceFrame(ReferenceFrame referenceFrame) { initialPosition.setToZero(referenceFrame); initialOrientation.setToZero(referenceFrame); initialPosition.changeFrame(trajectoryFrame); initialOrientation.changeFrame(trajectoryFrame); yoInitialPosition.set(initialPosition); yoInitialOrientation.set(initialOrientation); }
/** Go instantly to zero, no smooth interpolation. */ public void setToZeroInMidFeetZUpFrame() { tempOrientation.setToZero(midFeetZUpFrame); tempOrientation.changeFrame(worldFrame); initialPelvisOrientation.set(tempOrientation); finalPelvisOrientation.set(tempOrientation); desiredPelvisOrientation.set(tempOrientation); initialize(midFeetZUpFrame); }
public void setToHoldCurrent(ReferenceFrame trajectoryFrame) { tempOrientation.setToZero(pelvisFrame); tempOrientation.changeFrame(worldFrame); initialPelvisOrientation.set(tempOrientation); finalPelvisOrientation.set(tempOrientation); desiredPelvisOrientation.set(tempOrientation); resetOrientationOffset(); initialize(trajectoryFrame); }
/** 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); }