public double getPitch() { return RotationTools.computePitch(this); }
public double getPitch() { tempMatrixForYawPitchRollConversion.set(quaternion); return RotationTools.computePitch(tempMatrixForYawPitchRollConversion); }
public static void convertMatrixToYawPitchRoll(Matrix3d rotationMatrix, double[] yawPitchRollToPack) { yawPitchRollToPack[0] = computeYaw(rotationMatrix); yawPitchRollToPack[1] = computePitch(rotationMatrix); yawPitchRollToPack[2] = computeRoll(rotationMatrix); }
public void setRotation(Matrix3d rotationMatrix, int imuIndex) { Quat4d q = new Quat4d(); RotationTools.convertMatrixToQuaternion(rotationMatrix, q); RotationTools.checkQuaternionNormalized(q); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // q.set(rotationMatrix); p_qs.set(q.getW()); p_qx.set(q.getX()); p_qy.set(q.getY()); p_qz.set(q.getZ()); p_yaw.set(RotationTools.computeYaw(rotationMatrix)); p_pitch.set(RotationTools.computePitch(rotationMatrix)); p_roll.set(RotationTools.computeRoll(rotationMatrix)); }
private void updateAfterYoVariables() { error_pitch.set(RotationTools.computePitch(rotationError)); error_roll.set(RotationTools.computeRoll(rotationError)); error_yaw.set(RotationTools.computeYaw(rotationError)); error_x.set(translationError.getX()); error_y.set(translationError.getY()); error_z.set(translationError.getZ()); pelvisPose.getTranslation(pelvisTranslation); processedRootJointPosition.set(pelvisTranslation); pelvisPose.getRotation(rot); processedRootJointQuaternion.set(rot); processedRootJointQuaternion.getYawPitchRoll(tempRots); processedRootJointYaw.set(tempRots[0]); processedRootJointPitch.set(tempRots[1]); processedRootJointRoll.set(tempRots[2]); }