public YoQuaternionProvider(String namePrefix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { orientation = new YoFrameQuaternion(namePrefix + "Orientation", referenceFrame, registry); }
@Override public void getOrientation(Quat4d orientationToPack) { orientation.get(orientationToPack); }
@Override public void setOrientation(Quat4d orientation) { this.orientation.set(orientation); }
public void set(YoFrameQuaternion yoFrameQuaternion) { checkReferenceFrameMatch(yoFrameQuaternion); yoFrameQuaternion.getFrameOrientationIncludingFrame(frameOrientation); getYoValuesFromFrameOrientation(); }
public void setToHoldCurrentDesired(ReferenceFrame desiredTrajectoryFrame) { desiredPelvisOrientation.getFrameOrientationIncludingFrame(tempOrientation); initialPelvisOrientation.set(tempOrientation); finalPelvisOrientation.set(tempOrientation); initialize(desiredTrajectoryFrame); }
public void prepareForLocomotion() { desiredPelvisOrientationWithOffset.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(initialPelvisOrientation.getReferenceFrame()); initialPelvisOrientation.set(tempOrientation); finalPelvisOrientation.set(tempOrientation); desiredPelvisOrientation.set(tempOrientation); resetOrientationOffset(); initialize(worldFrame); }
public TaskspaceHeadControlState(RigidBody chest, RigidBody head, double[] homeYawPitchRoll, YoOrientationPIDGainsInterface gains, YoVariableRegistry parentRegistry) { super(HeadControlMode.TASKSPACE); this.gains = gains; feedbackControlCommand.set(chest, head); headFrame = head.getBodyFixedFrame(); chestFrame = chest.getBodyFixedFrame(); trajectoryGenerator = new MultipleWaypointsOrientationTrajectoryGenerator("head", true, worldFrame, registry); trajectoryGenerator.registerNewTrajectoryFrame(chestFrame); homeOrientation = new YoFrameQuaternion("headHomeOrientation", chestFrame, registry); homeOrientation.set(homeYawPitchRoll); parentRegistry.addChild(registry); }
@Override public void compute() { if (!isEnabled()) return; yoDesiredOrientation.getFrameOrientationIncludingFrame(tempOrientation); yoDesiredAngularVelocity.getFrameTupleIncludingFrame(tempAngularVelocity); yoFeedForwardAngularAcceleration.getFrameTupleIncludingFrame(feedForwardAngularAcceleration); accelerationControlModule.compute(desiredAngularAcceleration, tempOrientation, tempAngularVelocity, feedForwardAngularAcceleration, base); yoDesiredAngularAcceleration.setAndMatchFrame(desiredAngularAcceleration); tempOrientation.setToZero(endEffectorFrame); yoCurrentOrientation.setAndMatchFrame(tempOrientation); accelerationControlModule.getEndEffectorCurrentAngularVelocity(tempAngularVelocity); yoCurrentAngularVelocity.setAndMatchFrame(tempAngularVelocity); yoDesiredOrientation.get(tempAxisAngle); yoDesiredRotationVector.set(tempAxisAngle.getX(), tempAxisAngle.getY(), tempAxisAngle.getZ()); yoDesiredRotationVector.scale(tempAxisAngle.getAngle()); yoCurrentOrientation.get(tempAxisAngle); yoCurrentRotationVector.set(tempAxisAngle.getX(), tempAxisAngle.getY(), tempAxisAngle.getZ()); yoCurrentRotationVector.scale(tempAxisAngle.getAngle()); output.setAngularAcceleration(endEffectorFrame, baseFrame, desiredAngularAcceleration); }
private void updateOrientationVisualization() { desiredSpatialAcceleration.getAngularPart(desiredAngularAcceleration); yoDesiredAngularAcceleration.setAndMatchFrame(desiredAngularAcceleration); tempOrientation.setToZero(accelerationControlModule.getTrackingFrame()); yoCurrentOrientation.setAndMatchFrame(tempOrientation); accelerationControlModule.getEndEffectorCurrentAngularVelocity(tempAngularVelocity); yoCurrentAngularVelocity.setAndMatchFrame(tempAngularVelocity); yoDesiredOrientation.get(tempAxisAngle); yoDesiredRotationVector.set(tempAxisAngle.getX(), tempAxisAngle.getY(), tempAxisAngle.getZ()); yoDesiredRotationVector.scale(tempAxisAngle.getAngle()); yoCurrentOrientation.get(tempAxisAngle); yoCurrentRotationVector.set(tempAxisAngle.getX(), tempAxisAngle.getY(), tempAxisAngle.getZ()); yoCurrentRotationVector.scale(tempAxisAngle.getAngle()); }
YoFrameQuaternion yoDesiredHandOrientation = yoDesiredHandOrientations.get(robotSide); if (yoDesiredHandPosition.containsNaN() || yoDesiredHandOrientation.containsNaN()) Quat4d desiredHandOrientation = new Quat4d(); yoDesiredHandPosition.get(desiredHandPosition); yoDesiredHandOrientation.get(desiredHandOrientation); HandTrajectoryMessage temporaryHandTrajectoryMessage = new HandTrajectoryMessage(robotSide, 0.0, desiredHandPosition, desiredHandOrientation); handTrajectoryMessage.put(robotSide, temporaryHandTrajectoryMessage); if (yoDesiredChestOrientation.containsNaN()) yoDesiredChestQuaternion.get(desiredChestOrientation); chestTrajectoryMessage = new ChestTrajectoryMessage(0.0, desiredChestOrientation); if (yoDesiredPelvisOrientation.containsNaN()) yoDesiredPelvisQuaternion.get(desiredPelvisOrientation); pelvisOrientationTrajectoryMessage = new PelvisOrientationTrajectoryMessage(0.0, desiredPelvisOrientation);
@Override public void initializeForFrozenState() { rotationFrozenOffset.setIdentity(); // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); double initialYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); rootJointYawOffsetFromFrozenState.set(initialYaw); rotationFrozenOffset.rotZ(initialYaw); yoRootJointFrameQuaternion.setToZero(); yoRootJointFrameOrientation.setToZero(); yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
public void setPose(FramePose pose) { pose.getPositionIncludingFrame(tempPoint); tempPoint.changeFrame(this.position.getReferenceFrame()); this.position.set(tempPoint); pose.getOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(this.orientation.getReferenceFrame()); this.orientation.set(tempOrientation); } }
@Override public void getOrientation(FrameOrientation orientationToPack) { currentOrientation.getFrameOrientationIncludingFrame(orientationToPack); }
@Override public void setOrientationToZero() { orientation.setToZero(); }
@Override public void setOrientationToNaN() { orientation.setToNaN(); }
public void setDesiredChestOrientation(FrameOrientation desiredChestOrientation) { yoDesiredChestOrientation.setAndMatchFrame(desiredChestOrientation); }
public void set(YoFrameQuaternion orientation) { orientation.checkReferenceFrameMatch(getReferenceFrame()); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); set(tempFrameOrientation); }
@Override public boolean containsNaN() { return position.containsNaN() || orientation.containsNaN(); }
public void setInitialOrientation(YoFrameQuaternion initialOrientation) { initialOrientation.getFrameOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(trajectoryFrame); this.initialOrientation.set(tempOrientation); }