in my model, it has only a body (pelvis) with freejoint i suppose that the mean of qpos[3:7] is (w, x, y, z) but RPY from it is not the same with RPY from xmat(xquat) Code: RPY from qpos : 3.02325, -1.12901, -1.41925 pelvis RPY from xmat: 3.02568, -1.12426, -1.42081 pelvis RPY from xquat: 3.02568, -1.12426, -1.42081 pos from qpos is also not the same with pos from xpos Code: qpos[0:3] = -0.460483 -0.209694 0.0161131 xpos of pelivs = -0.460487, -0.209804, 0.0160862 what wrong with it ? additionaly, i suppose the qvel[3:6] is the \dot_{\phi}, but it still not exactly the same Code: w = pelvis com based rot vel from data.cvel : -4.82823, 0.335953, -0.616537 qvel : -1.00669, 4.79616, -0.113876 R(pelvis).T * w : -1.01108, 4.77124, -0.133129 is qpos and qvet not reffered to the first link?
'q' in qpos and qvel refer to the generalize coordinate. The order of q is also dependent on the joint id you specify in the .xml model. For the 'simulation.cpp' example, qpos[0]~qpos[6] corresponds to the 'root' joint cartesian position (qpos[0]~qpos[2]) and orientation (qpos[3]~qpos[6]), and qvel[0]~qvel[5] correspond to the 'root' joint velocity, translational (qvel[0]~qvel[2]) and rotational (qvel[3]~qvel[5]). Note that for orientation you follow the quaternion notation thus need four element but for velocity you use angular velocity which consists of 3 elements. Sorry that I don't have a good understanding of cvel, xmat, xpos.
When I debug the code, I found qvel[0] is not exactly the same as the following (reference: https://github.com/openai/gym/blob/master/gym/envs/mujoco/walker2d.py): posbefore = self.sim.data.qpos[0] # simulate posafter = self.sim.data.qpos[0] vel = (posafter - posbefore) / self.dt Some experimental values are vel=-0.004, qvel[0] = -0.007; vel=-0.16, qvel[0] =-0.325; vel=-0.55, qvel[0]=-0.71; vel=-0.538, qvel[0]=-0.353. Does anyone know what's the problem here?