# qpos/qvel meaning of freejoint

Discussion in 'Simulation' started by foolyc, Mar 20, 2018.

1. ### foolyc

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?

2. ### Jongwoo Lee

'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.

3. ### Coding Cat

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?

Last edited: Apr 16, 2018
4. ### Jongwoo Lee

Where can I find walker2d.xml ?

5. ### Coding Cat

Jongwoo Lee likes this.