public void get(Quat4d quaternionToPack) { putYoValuesIntoFrameOrientation(); frameOrientation.getQuaternion(quaternionToPack); }
public void queueFrameOrientation(FrameOrientation frameOrientation) { if (referenceFrame != null) referenceFrame.checkReferenceFrameMatch(frameOrientation); frameOrientation.getQuaternion(tempQuaternion); queueQuaternion(tempQuaternion); }
public void setOrientation(FrameOrientation orientation) { checkReferenceFrameMatch(orientation); geometryObject.setOrientation(orientation.getQuaternion()); }
public void setOrientation(FrameOrientation frameOrientation) { checkReferenceFrameMatch(frameOrientation); pose.setOrientation(frameOrientation.getQuaternion()); }
public void getOrientation(FrameOrientation orientationToPack) { checkReferenceFrameMatch(orientationToPack); geometryObject.getOrientation(orientationToPack.getQuaternion()); }
public void getOrientation(FrameOrientation orientationToPack) { checkReferenceFrameMatch(orientationToPack); geometryObject.getOrientation(orientationToPack.getQuaternion()); }
public void getOrientationIncludingFrame(FrameOrientation orientationToPack) { orientationToPack.setToZero(getReferenceFrame()); geometryObject.getOrientation(orientationToPack.getQuaternion()); }
public void set(FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration) { desiredOrientation.checkReferenceFrameMatch(worldFrame); desiredAngularVelocity.checkReferenceFrameMatch(worldFrame); feedForwardAngularAcceleration.checkReferenceFrameMatch(worldFrame); desiredOrientation.getQuaternion(desiredOrientationInWorld); desiredAngularVelocity.get(desiredAngularVelocityInWorld); feedForwardAngularAcceleration.get(feedForwardAngularAccelerationInWorld); }
private FootstepDataMessage createRelativeFootStep(PoseReferenceFrame frame, RobotSide side, Point3d location, Quat4d orientation) { FramePose pose = offsetPointFromFrameInWorldFrame(frame, location, orientation); FootstepDataMessage message = new FootstepDataMessage(side, pose.getFramePointCopy().getPoint(), pose.getFrameOrientationCopy().getQuaternion()); return message; }
public void changeFrameAndSet(FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration) { desiredOrientation.changeFrame(worldFrame); desiredAngularVelocity.changeFrame(worldFrame); feedForwardAngularAcceleration.changeFrame(worldFrame); desiredOrientation.getQuaternion(desiredOrientationInWorld); desiredAngularVelocity.get(desiredAngularVelocityInWorld); feedForwardAngularAcceleration.get(feedForwardAngularAccelerationInWorld); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { getQuaternion(quaternion); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); frameOrientation.setIncludingFrame(currentReferenceFrame, quaternion); frameOrientation.changeFrame(desiredFrame); frameOrientation.getQuaternion(quaternion); set(quaternion); }
public void changeFrameAndSet(FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration) { desiredOrientation.changeFrame(worldFrame); desiredAngularVelocity.changeFrame(worldFrame); feedForwardAngularAcceleration.changeFrame(worldFrame); desiredOrientation.getQuaternion(desiredOrientationInWorld); desiredAngularVelocity.get(desiredAngularVelocityInWorld); feedForwardAngularAcceleration.get(feedForwardAngularAccelerationInWorld); }
@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 setIncludingFrame(double time, FrameOrientation orientation, FrameVector angularVelocity) { orientation.checkReferenceFrameMatch(angularVelocity); setToZero(orientation.getReferenceFrame()); geometryObject.set(time, orientation.getQuaternion(), angularVelocity.getVector()); }
public void setControlFrameFixedInEndEffector(FramePoint position, FrameOrientation orientation) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); position.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); orientation.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); position.get(controlFrameOriginInEndEffectorFrame); orientation.getQuaternion(controlFrameOrientationInEndEffectorFrame); }
public void changeFrameAndSetControlFrameFixedInEndEffector(FramePoint position, FrameOrientation orientation) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); position.changeFrame(endEffector.getBodyFixedFrame()); orientation.changeFrame(endEffector.getBodyFixedFrame()); position.get(controlFrameOriginInEndEffectorFrame); orientation.getQuaternion(controlFrameOrientationInEndEffectorFrame); }
public void correctState(DenseMatrix64F correction) { orientationPort.getData().getQuaternion(quaternion); MatrixTools.extractTuple3dFromEJMLVector(tempRotationVector, correction, 0); RotationTools.convertRotationVectorToAxisAngle(tempRotationVector, tempAxisAngle); quaternionDelta.set(tempAxisAngle); quaternion.mul(quaternionDelta); orientation.set(quaternion); orientationPort.setData(orientation); } }
public void set(double time, FramePoint position, FrameOrientation orientation, FrameVector linearVelocity, FrameVector angularVelocity) { checkReferenceFrameMatch(position); checkReferenceFrameMatch(orientation); checkReferenceFrameMatch(linearVelocity); checkReferenceFrameMatch(angularVelocity); geometryObject.set(time, position.getPoint(), orientation.getQuaternion(), linearVelocity.getVector(), angularVelocity.getVector()); }
public void set(FramePoint position, FrameOrientation orientation, FrameVector linearVelocity, FrameVector angularVelocity) { checkReferenceFrameMatch(position); checkReferenceFrameMatch(orientation); checkReferenceFrameMatch(linearVelocity); checkReferenceFrameMatch(angularVelocity); geometryObject.set(position.getPoint(), orientation.getQuaternion(), linearVelocity.getVector(), angularVelocity.getVector()); }
public void computeChestTrajectoryMessage() { checkIfDataHasBeenSet(); ReferenceFrame chestFrame = fullRobotModelToUseForConversion.getChest().getBodyFixedFrame(); Quat4d desiredQuaternion = new Quat4d(); FrameOrientation desiredOrientation = new FrameOrientation(chestFrame); desiredOrientation.changeFrame(worldFrame); desiredOrientation.getQuaternion(desiredQuaternion); ChestTrajectoryMessage chestTrajectoryMessage = new ChestTrajectoryMessage(trajectoryTime, desiredQuaternion); output.setChestTrajectoryMessage(chestTrajectoryMessage); }