public void setIncludingFrame(ReferenceFrame referenceFrame, Quat4d quaternion) { this.referenceFrame = referenceFrame; set(quaternion); }
desiredOrientation.set(footOrientation);
double qz = -temporaryDesiredHandOrientation.getQz(); double qs = temporaryDesiredHandOrientation.getQs(); temporaryDesiredHandOrientation.set(qx, qy, qz, qs);
private void updateStartAndGoalOffsetErrorRotation() { //Rotation stateEstimatorReferenceFrame.getTransformToDesiredFrame(stateEstimatorTransform_Rotation, worldFrame); stateEstimatorTransform_Rotation.zeroTranslation(); stateEstimatorPose_Rotation.setPose(stateEstimatorTransform_Rotation); stateEstimatorReferenceFrame_Rotation.setPoseAndUpdate(stateEstimatorPose_Rotation); startOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Rotation); startOffsetErrorPose.getOrientation(updatedStartOffset_Rotation_quat); startOffsetErrorPose.changeFrame(worldFrame); startOffsetErrorPose.getOrientationIncludingFrame(updatedStartOffset_Rotation); goalOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Rotation); goalOffsetErrorPose.getOrientation(updatedGoalOffset_Rotation_quat); goalOffsetErrorPose.changeFrame(worldFrame); goalOffsetErrorPose.getOrientationIncludingFrame(updatedGoalOffset_Rotation); startOffsetTransform_Rotation.setRotationAndZeroTranslation(updatedStartOffset_Rotation_quat); offsetBetweenStartAndGoal_Rotation.setOrientationFromOneToTwo(updatedGoalOffset_Rotation, updatedStartOffset_Rotation); offsetBetweenStartAndGoal_Rotation.getYawPitchRoll(temporaryYawPitchRoll); goalYawRaw.set(temporaryYawPitchRoll[0]); goalYawWithDeadZone.update(); RotationTools.convertYawPitchRollToQuaternion(goalYawWithDeadZone.getDoubleValue(), temporaryYawPitchRoll[1], temporaryYawPitchRoll[2], updatedGoalOffsetWithDeadZone_Rotation_quat); updatedGoalOffsetWithDeadZone_Rotation_quat.mul(updatedStartOffset_Rotation_quat, updatedGoalOffsetWithDeadZone_Rotation_quat); updatedGoalOffsetWithDeadZone_Rotation.set(updatedGoalOffsetWithDeadZone_Rotation_quat); goalOffsetTransform_Rotation.setRotationAndZeroTranslation(updatedGoalOffsetWithDeadZone_Rotation_quat); }
public void setIncludingFrame(ReferenceFrame referenceFrame, Quat4f quaternion) { this.referenceFrame = referenceFrame; set(quaternion); }
public void setIncludingFrame(ReferenceFrame referenceFrame, double qx, double qy, double qz, double qs) { this.referenceFrame = referenceFrame; set(qx, qy, qz, qs); }
@Override public void getEstimatedOrientation(FrameOrientation estimatedOrientation) { estimatedOrientation.set(rotationFromRootJointFrameToWorld); }
public void set(AxisAngle4d axisAngle) { frameOrientation.set(axisAngle); getYoValuesFromFrameOrientation(); }
public void setIncludingFrame(ReferenceFrame referenceFrame, AxisAngle4d axisAngle) { this.referenceFrame = referenceFrame; set(axisAngle); normalize(); }
public void set(Quat4d quaternion) { frameOrientation.set(quaternion); getYoValuesFromFrameOrientation(); }
public void getFrameOrientation(FrameOrientation frameOrientationToPack) { putYoValuesIntoFrameOrientation(); frameOrientationToPack.set(this.frameOrientation); }
public void getAverageFrameOrientation(FrameOrientation averageFrameOrientationToPack) { if (referenceFrame != null) referenceFrame.checkReferenceFrameMatch(averageFrameOrientationToPack); averageFrameOrientationToPack.set(averageQuaternion); }
public void set(Matrix3d matrix) { frameOrientation.set(matrix); getYoValuesFromFrameOrientation(); }
public void setOrientationAndUpdate(FrameOrientation orientation) { this.parentFrame.checkReferenceFrameMatch(orientation.getReferenceFrame()); this.frameOrientation.set(orientation); this.update(); }
public static void extractFrameOrientationFromEJMLVector(FrameOrientation frameOrientation, DenseMatrix64F matrix, int rowStart) { int index = rowStart; double x = matrix.get(index++, 0); double y = matrix.get(index++, 0); double z = matrix.get(index++, 0); double w = matrix.get(index++, 0); frameOrientation.set(x, y, z, w); }
private void updateRootJointPosition(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, FramePoint centerOfMassPosition) { tempCenterOfMassPositionState.setIncludingFrame(centerOfMassPosition); estimationFrame.getTransformToDesiredFrame(worldFrame).getRotation(tempOrientationStateReconstructMatrix); tempOrientationStateReconstruct.set(tempOrientationStateReconstructMatrix); computeEstimationLinkToWorldTransform(estimationFrame, tempEstimationLinkToWorld, tempCenterOfMassPositionState, tempOrientationStateReconstruct); computeRootJointToWorldTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); rootJoint.setPositionAndRotation(tempRootJointToWorld); }
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 propagateState(double dt) { angularVelocityPort.getData().get(angularVelocity); orientationPort.getData().getQuaternion(quaternion); tempRotationVector.set(angularVelocity); tempRotationVector.scale(dt); RotationTools.convertRotationVectorToAxisAngle(tempRotationVector, tempAxisAngle); quaternionDelta.set(tempAxisAngle); quaternion.mul(quaternionDelta); quaternion.normalize(); // the previous operation should preserve norm, so this might not be necessary every step orientation.set(quaternion); orientationPort.setData(orientation); }
private void applyCorrection(FramePoint desiredPosition, FrameOrientation desiredOrientation) { ReferenceFrame originalFrame = desiredPosition.getReferenceFrame(); desiredPosition.changeFrame(controlFrame); desiredOrientation.changeFrame(controlFrame); desiredPosition.sub(totalLinearCorrection); totalAngularCorrection.negate(); RotationTools.convertRotationVectorToAxisAngle(totalAngularCorrection.getVector(), angularDisplacementAsAxisAngle); angularDisplacementAsMatrix.set(angularDisplacementAsAxisAngle); desiredOrientation.getMatrix3d(correctedRotationMatrix); correctedRotationMatrix.mul(angularDisplacementAsMatrix, correctedRotationMatrix); desiredOrientation.set(correctedRotationMatrix); desiredPosition.changeFrame(originalFrame); desiredOrientation.changeFrame(originalFrame); }
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); }