@Override public void getOrientation(FrameOrientation orientationToPack) { currentOrientation.getFrameOrientationIncludingFrame(orientationToPack); }
@Override public void getOrientation(FrameOrientation orientationToPack) { currentOrientation.getFrameOrientationIncludingFrame(orientationToPack); }
@Override public void getOrientation(FrameOrientation orientationToPack) { currentOrientation.getFrameOrientationIncludingFrame(orientationToPack); }
public void getOrientation(FrameOrientation orientationToPack) { orientation.getFrameOrientationIncludingFrame(orientationToPack); }
public void setFinalOrientation(YoFrameQuaternion finalOrientation) { finalOrientation.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); this.finalOrientation.set(tempOrientation); }
public void setInitialOrientation(YoFrameQuaternion initialOrientation) { initialOrientation.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); this.initialOrientation.set(tempOrientation); }
public void setFinalOrientation(YoFrameQuaternion finalOrientation) { finalOrientation.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); this.finalOrientation.set(tempOrientation); }
public void set(YoFrameQuaternion yoFrameQuaternion) { checkReferenceFrameMatch(yoFrameQuaternion); yoFrameQuaternion.getFrameOrientationIncludingFrame(frameOrientation); getYoValuesFromFrameOrientation(); }
@Override public FrameOrientation getData() { yoFrameQuaternion.getFrameOrientationIncludingFrame(super.getData()); return super.getData(); }
public void setInitialOrientation(YoFrameQuaternion initialOrientation) { initialOrientation.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); this.initialOrientation.set(tempOrientation); }
@Override public void getPose(FramePose framePoseToPack) { yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(currentPosition); yoCurrentOrientation.getFrameOrientationIncludingFrame(currentOrientation); framePoseToPack.setPoseIncludingFrame(currentPosition, currentOrientation); }
@Override public void getPose(FramePose framePoseToPack) { yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(currentPosition); yoCurrentOrientation.getFrameOrientationIncludingFrame(currentOrientation); framePoseToPack.setPoseIncludingFrame(currentPosition, currentOrientation); }
public void setToHoldCurrentDesired(ReferenceFrame desiredTrajectoryFrame) { desiredPelvisOrientation.getFrameOrientationIncludingFrame(tempOrientation); initialPelvisOrientation.set(tempOrientation); finalPelvisOrientation.set(tempOrientation); initialize(desiredTrajectoryFrame); }
public void getFramePose(FramePose framePoseToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); framePoseToPack.setPosition(tempFramePoint); framePoseToPack.setOrientation(tempFrameOrientation); }
public void set(YoFrameQuaternion orientation) { orientation.checkReferenceFrameMatch(getReferenceFrame()); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); set(tempFrameOrientation); }
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); }
/** 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 handleGoHome(double trajectoryTime, FrameOrientation initialOrientation) { initialOrientation.changeFrame(chestFrame); homeOrientation.getFrameOrientationIncludingFrame(desiredOrientation); desiredAngularVelocity.setToZero(chestFrame); trajectoryGenerator.clear(); trajectoryGenerator.switchTrajectoryFrame(chestFrame); trajectoryGenerator.appendWaypoint(0.0, initialOrientation, desiredAngularVelocity); trajectoryGenerator.appendWaypoint(trajectoryTime, desiredOrientation, desiredAngularVelocity); }
public void getPose(RigidBodyTransform rigidBodyTransformToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.getTransform3D(rigidBodyTransformToPack); rigidBodyTransformToPack.setTranslation(tempFramePoint.getX(), tempFramePoint.getY(), tempFramePoint.getZ()); }
public void prepareForLocomotion() { desiredPelvisOrientationWithOffset.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(initialPelvisOrientation.getReferenceFrame()); initialPelvisOrientation.set(tempOrientation); finalPelvisOrientation.set(tempOrientation); desiredPelvisOrientation.set(tempOrientation); resetOrientationOffset(); initialize(worldFrame); }