public void getOrientationInWorldFrame(Quat4d orientation) { FrameOrientation ankleOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), orientation); ankleReferenceFrame.getOrientationIncludingFrame(ankleOrientation); ankleOrientation.changeFrame(ReferenceFrame.getWorldFrame()); ankleOrientation.getQuaternion(orientation); }
@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 String toString() { FrameOrientation frameOrientation = new FrameOrientation(ankleReferenceFrame); frameOrientation.changeFrame(ReferenceFrame.getWorldFrame()); double[] ypr = frameOrientation.getYawPitchRoll(); String yawPitchRoll = "YawPitchRoll = " + Arrays.toString(ypr); return "id: " + id + " - pose: " + ankleReferenceFrame + " - trustHeight = " + trustHeight + "\n\tYawPitchRoll= {" + yawPitchRoll + "}"; } }
public void mul(FrameOrientation frameOrientation) { checkReferenceFrameMatch(frameOrientation); mul(frameOrientation.quaternion); }
private void computeEstimationLinkToWorldTransform(RigidBodyTransform estimationLinkToWorldToPack, FrameOrientation estimationLinkOrientation) { // R_{estimation}^{w} estimationLinkOrientation.changeFrame(worldFrame); estimationLinkOrientation.getTransform3D(estimationLinkToWorldToPack); }
public double dot(FrameOrientation frameOrientation) { checkReferenceFrameMatch(frameOrientation); return dot(frameOrientation.quaternion); }
return new double[lowerArmJointsClone.get(robotSide).length]; FrameOrientation temporaryDesiredHandOrientation = new FrameOrientation(); temporaryDesiredHandOrientation.setIncludingFrame(desiredHandOrientation); temporaryDesiredHandOrientation.checkReferenceFrameMatch(lowerArmsFrames.get(robotSide)); double qx = -temporaryDesiredHandOrientation.getQx(); double qy = temporaryDesiredHandOrientation.getQy(); double qz = -temporaryDesiredHandOrientation.getQz(); double qs = temporaryDesiredHandOrientation.getQs(); temporaryDesiredHandOrientation.set(qx, qy, qz, qs); temporaryDesiredHandOrientation.getTransform3D(desiredTransformForLowerArm); boolean success = inverseKinematicsForLowerArms.get(robotSide).solve(desiredTransformForLowerArm);
public FrameVector computeDesiredAngularVelocity(FrameOrientation desiredOrientation, ReferenceFrame controlFrame, MutableDouble errorMagnitude) { FrameVector ret = new FrameVector(); errorFrameOrientation.setIncludingFrame(desiredOrientation); errorFrameOrientation.changeFrame(controlFrame); errorFrameOrientation.getAxisAngle(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(errorAxisAngle.getAngle()); if (errorMagnitude != null) errorMagnitude.setValue(errorRotation.length()); errorRotation.scale(1.0 / updateDT); ret.setIncludingFrame(controlFrame, errorRotation); return ret; }
return new double[upperArmJointsClone.get(robotSide).length]; FrameOrientation temporaryDesiredUpperArmOrientation = new FrameOrientation(); temporaryDesiredUpperArmOrientation.setIncludingFrame(desiredUpperArmOrientation); temporaryDesiredUpperArmOrientation.checkReferenceFrameMatch(fullRobotModel.getChest().getBodyFixedFrame()); double[] yawPitchRoll = temporaryDesiredUpperArmOrientation.getYawPitchRoll(); yawPitchRoll[0] = robotSide.negateIfRightSide(yawPitchRoll[0]); yawPitchRoll[2] = robotSide.negateIfRightSide(yawPitchRoll[2]); temporaryDesiredUpperArmOrientation.setYawPitchRoll(yawPitchRoll); temporaryDesiredUpperArmOrientation.getTransform3D(desiredTransformForUpperArm); desiredTransformForUpperArm.multiply(desiredTransformForUpperArm, armZeroJointAngleConfigurationOffsets.get(robotSide)); boolean success = inverseKinematicsForUpperArms.get(robotSide).solve(desiredTransformForUpperArm);
public void setInitialOrientation(FrameOrientation initialOrientation) { tempOrientation.setIncludingFrame(initialOrientation); tempOrientation.changeFrame(trajectoryFrame); this.initialOrientation.set(tempOrientation); }
public void initialize() { time.set(0.0); FrameOrientation orientationToPack = new FrameOrientation(ReferenceFrame.getWorldFrame()); orientationProvider.getOrientation(orientationToPack); orientationToPack.changeFrame(orientation.getReferenceFrame()); orientation.set(orientationToPack); }
edgeVector.setXYIncludingFrame(edgeVector2d); edgeVector.normalize(); desiredOrientationCopy.setIncludingFrame(desiredOrientation); desiredOrientationCopy.changeFrame(footPolygon.getReferenceFrame()); desiredOrientationCopy.getAxisAngle(desiredAxisAngle); desiredRotationVector.set(desiredAxisAngle.getX(), desiredAxisAngle.getY(), desiredAxisAngle.getZ()); desiredRotationVector.scale(desiredAxisAngle.getAngle()); desiredRotationVector.scale(1.0 / angle); desiredAxisAngle.set(desiredRotationVector, angle); desiredOrientationCopy.set(desiredAxisAngle); desiredOrientationCopy.changeFrame(worldFrame); desiredHoldOrientation.set(desiredOrientationCopy); desiredHoldOrientation.getFrameOrientationIncludingFrame(desiredOrientation);
public void setAndMatchFrame(FrameOrientation orientation, boolean notifyListeners) { tempFrameOrientation.setIncludingFrame(orientation); tempFrameOrientation.changeFrame(getReferenceFrame()); tempFrameOrientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
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(); }
/** * Computes the angular velocity for an interpolation between two orientations using the SLERP method. * @param startOrientation the starting orientation * @param endOrientation the final orientation * @param alphaDot the interpolation rate * @return the angular velocity of the interpolated frame, w.r.t. the startOrientation, expressed in the frame in which the orientations were expressed */ public void computeAngularVelocity(FrameVector angularVelocityToPack, FrameOrientation startOrientation, FrameOrientation endOrientation, double alphaDot) { startOrientation.checkReferenceFrameMatch(endOrientation); ReferenceFrame frame = startOrientation.getReferenceFrame(); startOrientation.getQuaternion(startRotationQuaternion); endOrientation.getQuaternion(endRotationQuaternion); computeAngularVelocity(angularVelocity, startRotationQuaternion, endRotationQuaternion, alphaDot); angularVelocityToPack.setIncludingFrame(frame, angularVelocity); }
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); }
public String toString() { String ret = ""; FrameOrientation frameOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), this.orientation); double[] ypr = frameOrientation.getYawPitchRoll(); ret = location.toString(); ret += ", YawPitchRoll = " + Arrays.toString(ypr) + "\n"; return ret; }
/** * 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 getFrameOrientationIncludingFrame(FrameOrientation frameOrientationToPack) { putYoValuesIntoFrameOrientation(); frameOrientationToPack.setIncludingFrame(frameOrientation); }