public void setIncludingFrame(ReferenceFrame referenceFrame, RigidBodyTransform transform3D) { this.referenceFrame = referenceFrame; transform3D.getRotation(quaternion); }
/** * Return rotation in AxisAngle form. * * @param axisAngle */ public void getRotation(AxisAngle4d axisAngle) { getRotation(axisAngle, 1.0e-12); }
/** * Pack rotation part into Matrix3d and translation part into Tuple3d * * @param matrixToPack * @param translationPack */ public void get(Matrix3d matrixToPack, Tuple3d translationPack) { getRotation(matrixToPack); getTranslation(translationPack); }
/** * Convert and pack rotation part of transform into Quat4f and pack * translation into Vector3f. * * @param quaternionToPack * @param translationToPack */ public void get(Quat4f quaternionToPack, Tuple3f translationToPack) { getRotation(quaternionToPack); getTranslation(translationToPack); }
public static void convertTransformToQuaternion(RigidBodyTransform transform, Quat4d quaternionToPack) { Matrix3d rotationMatrix = rotationMatrixForQuaternionConvertor.get(); transform.getRotation(rotationMatrix); convertMatrixToQuaternion(rotationMatrix, quaternionToPack); }
/** * Convert and pack rotation part of transform into Quat4d and pack * translation into Tuple3d. * * @param quaternionToPack * @param translationToPack */ public void get(Quat4d quaternionToPack, Tuple3d translationToPack) { getRotation(quaternionToPack); getTranslation(translationToPack); }
private boolean isTransformationInPlane(RigidBodyTransform transform) { // arguably not a sufficient condition. ReferenceFrame2d needed! transform.getRotation(tempRotation); return ReferenceFrame.isRotationInPlane(tempRotation); }
/** * Pack rotation part into Matrix3f and translation part into Tuple3f * * @param matrixToPack * @param translationToPack */ public void get(Matrix3f matrixToPack, Tuple3f translationToPack) { getRotation(matrixToPack); getTranslation(translationToPack); }
public void setPose(RigidBodyTransform transform) { transform.getTranslation(position); transform.getRotation(orientation); }
public static double getMagnitudeOfAngleOfRotation(RigidBodyTransform rigidBodyTransform) { AxisAngle4d axisAngle4d = new AxisAngle4d(); rigidBodyTransform.getRotation(axisAngle4d); return Math.abs(axisAngle4d.getAngle()); }
@Override public void applyTransform(RigidBodyTransform transform) { transform.getRotation(temporaryMatrix); this.mul(temporaryMatrix, this); this.mulTransposeRight(this, temporaryMatrix); }
public static boolean isTransformationInPlane(RigidBodyTransform transform) { // true if the transform preserves the z axis direction and magnitude, paying no heed to transformations. // for SO(3) matrices (orthogonal with determinant 1, equivalent to rotations) Matrix3d rotation = new Matrix3d(); transform.getRotation(rotation); return isRotationInPlane(rotation); }
@Override public void applyTransform(RigidBodyTransform transform) { if (tempQuaternionForTransform == null) tempQuaternionForTransform = new Quat4d(); transform.getRotation(tempQuaternionForTransform); orientation.mul(tempQuaternionForTransform, orientation); transform.transform(angularVelocity); }
private void computeAngularVelocityStateOutputBlock() { estimationFrame.getTransformToDesiredFrame(tempTransform, measurementFrame); tempTransform.getRotation(tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(angularVelocityStatePort)); }
public void computeMatrixBlocks() { estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame()); tempTransform.getRotation(rotationFromEstimationToWorld); if (!assumePerfectIMU) { computeOrientationStateOutputBlock(rotationFromEstimationToWorld); computeAngularVelocityStateOutputBlock(rotationFromEstimationToWorld); } }
@Override public void applyTransform(RigidBodyTransform transform3D) { if (tempQuaternionForTransform == null) tempQuaternionForTransform = new Quat4d(); transform3D.getRotation(tempQuaternionForTransform); this.mul(tempQuaternionForTransform, this); if (!containsNaN()) normalize(); }
private Matrix3d computeDesiredFootRotation(ReferenceFrame desiredHeadingFrame) { RigidBodyTransform footToSupportTransform = desiredHeadingFrame.getTransformToDesiredFrame(worldFrame); Matrix3d footToSupportRotation = new Matrix3d(); footToSupportTransform.getRotation(footToSupportRotation); double yaw = RotationTools.computeYaw(footToSupportRotation); double pitch = stepPitch.getDoubleValue(); double roll = 0.0; RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, footToSupportRotation); return footToSupportRotation; }
private void updateRootJointRotation(FloatingInverseDynamicsJoint rootJoint, FrameOrientation estimationLinkOrientation, ReferenceFrame estimationFrame) { tempOrientationEstimatinLink.setIncludingFrame(estimationLinkOrientation); computeEstimationLinkToWorldTransform(tempEstimationLinkToWorld, tempOrientationEstimatinLink); computeRootJointToWorldTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); Matrix3d rootJointRotation = new Matrix3d(); tempRootJointToWorld.getRotation(rootJointRotation); rootJoint.setRotation(rootJointRotation); }
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); }
protected Footstep createFootstepPlacedAtBipedfootLocation(RobotSide side) { ReferenceFrame soleFrame = soleFrames.get(side); Vector3d translation = new Vector3d(); Quat4d rotation = new Quat4d(); soleFrame.getTransformToWorldFrame().getTranslation(translation); soleFrame.getTransformToWorldFrame().getRotation(rotation); FramePose2d solePose2d = new FramePose2d(soleFrame, new Point2d(translation.getX(), translation.getY()), RotationTools.computeYaw(rotation)); Footstep foot = createFootstep(side, solePose2d); return foot; }