Confusion about the root joint of freeflyer model

[Mr-Red331]

Hi,

I wanted to discuss a point of confusion I've encountered regarding the orientation of the root joint in the freeflyer model. It seems that the orientation of the body, represented as R_base, should ideally align with the orientation of the root joint, represented as R_root. However, based on my recent testing, it appears that R_base is computed as R_base = R_root.transpose(). I'd like to outline my testing procedure and results to seek your insights and expertise on this matter.

1. To represent the body's orientation, I randomly assigned a quaternion quat = [qx, qy, qz, qw]. In my test, I used the values quat = [sqrt(1.0/2), sqrt(1.0/8), sqrt(1.0/4), sqrt(1.0/8)].
2. I initialized a zero vector q and set q.segment(3,4) = quat.
3. Using the model and data, I calculated the forward kinematics with the command forwardKinematics(model, data, q).
4. I retrieved the orientation of the root joint with the command R_root = data.oMi[1].rotation().
5. I also calculated the orientation of the robot body using the quaternion-to-rotation-matrix conversion: R_base = quaternionToRotationMatrix(quat).

After this process, I obtained the following results for R_root and R_base:

R_root = 
[ 0.25,  0.15,  0.96
  0.85, -0.50, -0.15
  0.46,  0.85, -0.25  ]

R_base = 
[ 0.25,  0.85,  0.46
  0.15, -0.50,  0.85
  0.96, -0.15, -0.25  ]

My question is whether there is a misunderstanding on my part regarding the root joint's orientation or if there might be an error in the process of calculating it. Your guidance on this matter would be greatly appreciated.

Thank you for your time and assistance.

[JunJieChenpete]

I ran the test as you said and I think your R_ Root is correct. The problem may be in the function quaternionToRotationMatrix(). What is the source of the quaternionToRotationMatrix() function you called? I couldn't find this function inside Pinocchio.
If you are still confused about the result, you can run the following code, which use the method provided by Eigen to convert quaternion into rotation matrix, and the result obtained are consistent with the R_root you calculated.

  Eigen::Vector4d quat;
  quat << sqrt(1.0/2), sqrt(1.0/8), sqrt(1.0/4), sqrt(1.0/8);
  Eigen::Matrix3d R;
  R = Eigen::Quaterniond(quat).toRotationMatrix();
  std::cout << "R = \n" << R << std::endl;