public static void convertYawPitchRollToMatrix(double[] yawPitchRoll, Matrix3d rotationMatrixToPack) { convertYawPitchRollToMatrix(yawPitchRoll[0], yawPitchRoll[1], yawPitchRoll[2], rotationMatrixToPack); }
public void getRotation(Matrix3d rotationToPack) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToMatrix(0.0, yawPitchRoll[1], 0.0, rotationToPack); }
public void setYawPitchRoll(double yaw, double pitch, double roll) { RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, temporaryMatrix); setRotation(temporaryMatrix); }
public void getMatrix3d(Matrix3d rotationMatrixToPack) { RotationTools.convertYawPitchRollToMatrix(yaw.getDoubleValue(), pitch.getDoubleValue(), roll.getDoubleValue(), rotationMatrixToPack); }
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 integrateError() { double yawNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_yaw.getDoubleValue() + noiseBias_yaw.getDoubleValue(); double pitchNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_pitch.getDoubleValue() + noiseBias_pitch.getDoubleValue(); double rollNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_roll.getDoubleValue() + noiseBias_roll.getDoubleValue(); RotationTools.convertYawPitchRollToMatrix(yawNoise, pitchNoise, rollNoise, rotationNoise); rotationError.mul(rotationNoise); double xNoise = (random.nextDouble() - 0.5) * noiseScalar_x.getDoubleValue() + noiseBias_x.getDoubleValue(); double yNoise = (random.nextDouble() - 0.5) * noiseScalar_y.getDoubleValue() + noiseBias_y.getDoubleValue(); double zNoise = (random.nextDouble() - 0.5) * noiseScalar_z.getDoubleValue() + noiseBias_z.getDoubleValue(); translationNoise.set(xNoise, yNoise, zNoise); pelvisPose.getRotation(pelvisRotation); pelvisRotation.mul(rotationError); pelvisRotation.transform(translationNoise); translationError.add(translationNoise); } }
public FramePose getPoseFromCylindricalCoordinates(RobotSide robotSide, ReferenceFrame frame, double radiansFromYAxis, double radius, double z, double outwardRotation, double pitchRotation) { getPosition(position, frame, radiansFromYAxis, radius, z); RotationTools.convertYawPitchRollToMatrix(0.0, Math.PI / 2.0, -Math.PI / 2.0, preRotation); rotX.rotX(robotSide.negateIfRightSide(Math.PI / 2.0) - radiansFromYAxis); rotZ.rotZ(robotSide.negateIfRightSide(outwardRotation)); rotY.rotY(pitchRotation); rotation.mul(preRotation, rotX); rotation.mul(rotZ); rotation.mul(rotY); orientation.setIncludingFrame(frame, rotation); return new FramePose(position, orientation); }
Vector3d translation = new Vector3d(0.630000, 0.100000, 0.580000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromWorldToParent = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(-0.038500, 0.000000, -0.086000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromParentToChild = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(0.640000, 0.270000, 0.580000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromWorldToParent = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(-0.040000, 0.000000, -0.086000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromParentToChild = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(0.340000, 0.300000, 1.290000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromWorldToParent = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(0.530000, 0.070000, 1.050000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromWorldToParent = new RigidBodyTransform(matrix3d, translation);
Vector3d translation = new Vector3d(0.630000, 0.100000, 0.580000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromWorldToParent = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(-0.038500, 0.000000, -0.086000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromParentToChild = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(0.640000, 0.270000, 0.580000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromWorldToParent = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(-0.040000, 0.000000, -0.086000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromParentToChild = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(0.340000, 0.300000, 1.290000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromWorldToParent = new RigidBodyTransform(matrix3d, translation); Vector3d translation = new Vector3d(0.530000, 0.070000, 1.050000); Matrix3d matrix3d = new Matrix3d(); RotationTools.convertYawPitchRollToMatrix(yaw, pitch, roll, matrix3d); transform3DfromWorldToParent = new RigidBodyTransform(matrix3d, translation);