public double dot(FrameOrientation frameOrientation) { checkReferenceFrameMatch(frameOrientation); return dot(frameOrientation.quaternion); }
public void mul(FrameOrientation frameOrientation) { checkReferenceFrameMatch(frameOrientation); mul(frameOrientation.quaternion); }
public void setOrientationFromOneToTwo(FrameOrientation orientationOne, FrameOrientation orientationTwo) { orientationOne.checkReferenceFrameMatch(orientationTwo); this.checkReferenceFrameMatch(orientationOne); this.quaternion.conjugate(orientationTwo.quaternion); this.quaternion.mul(orientationOne.quaternion); }
public void setOrientationAndUpdate(FrameOrientation frameOrientation) { frameOrientation.checkReferenceFrameMatch(parentFrame); originPose.setOrientation(frameOrientation); this.update(); }
public void interpolate(FrameOrientation orientationOne, FrameOrientation orientationTwo, double alpha) { orientationOne.checkReferenceFrameMatch(orientationTwo); alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); quaternion.interpolate(orientationOne.quaternion, orientationTwo.quaternion, alpha); referenceFrame = orientationOne.getReferenceFrame(); }
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 void setIncludingFrame(FrameOrientation orientation, FrameVector angularVelocity) { orientation.checkReferenceFrameMatch(angularVelocity); setToZero(orientation.getReferenceFrame()); geometryObject.set(orientation.getQuaternion(), angularVelocity.getVector()); }
public void set(FrameOrientation orientation, boolean notifyListeners) { orientation.checkReferenceFrameMatch(getReferenceFrame()); orientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
public void setIncludingFrame(double time, FrameOrientation orientation, FrameVector angularVelocity) { orientation.checkReferenceFrameMatch(angularVelocity); setToZero(orientation.getReferenceFrame()); geometryObject.set(time, orientation.getQuaternion(), angularVelocity.getVector()); }
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 void setControlFrameFixedInEndEffector(FramePoint position, FrameOrientation orientation) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); position.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); orientation.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); position.get(controlFrameOriginInEndEffectorFrame); orientation.getQuaternion(controlFrameOrientationInEndEffectorFrame); }
/** * 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 setSoleFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); orientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setSoleFrame(new FramePose(ReferenceFrame.getWorldFrame(), position.getPoint(), orientation.getQuaternionCopy())); }
private void estimateYawDriftAndCorrectOrientation(FrameOrientation firstFrameOrientation, FrameOrientation secondFrameOrientation, FrameOrientation fusedFrameOrientation) firstFrameOrientation.checkReferenceFrameMatch(fusedMeasurementFrame); secondFrameOrientation.checkReferenceFrameMatch(fusedMeasurementFrame); fusedFrameOrientation.checkReferenceFrameMatch(fusedMeasurementFrame);
public void initializeEstimatorToActual(Tuple3d initialCoMPosition, Quat4d initialEstimationLinkOrientation) { // Setting the initial CoM Position here. FramePoint estimatedCoMPosition = new FramePoint(); stateEstimatorWithPorts.getEstimatedCoMPosition(estimatedCoMPosition); estimatedCoMPosition.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); estimatedCoMPosition.set(initialCoMPosition); FrameOrientation estimatedOrientation = null; if(!assumePerfectIMU) { estimatedOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame()); stateEstimatorWithPorts.getEstimatedOrientation(estimatedOrientation); estimatedOrientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); estimatedOrientation.set(initialEstimationLinkOrientation); } else { estimatedOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), initialEstimationLinkOrientation); } sensorAndEstimatorAssembler.initializeEstimatorToActual(estimatedCoMPosition, estimatedOrientation); }
temporaryDesiredHandOrientation.setIncludingFrame(desiredHandOrientation); temporaryDesiredHandOrientation.checkReferenceFrameMatch(lowerArmsFrames.get(robotSide));
temporaryDesiredUpperArmOrientation.checkReferenceFrameMatch(fullRobotModel.getChest().getBodyFixedFrame());
desiredOrientation.checkReferenceFrameMatch(footOrientation);