How to get a bone vector after qpos value has changed?

[Rakesh-Paul01]

Hi,

I'm a student and I'm trying to use MuJoCo, to model a humanoid. Currently I am using the cmu_humanoid from dm_control

I understand that each body is wrt to a parent, and have axis defined wrt to the parent.

Suppose I want a vector for a bone how can I get that vector?

consider that snippet below

Here the ltibia(the knee) is child of lfemur (near the pelvis)

at the T-pose(which this image is in) the lfemur to ltibia is a vector in -y direction (wrt parent i.e lfemur), suppose that we change the qpos of lfemur, the axis of lfemur will also change (and the new vector from lfemur to ltibia will still be in the -y direction)

But if I want the new vector from lfemur to ltibia wrt to the original axis of lfemur, how can I obtain it? (does mujoco by default provide it?)

[Balint-H]

Hello,

Here are some of the approaches you could take (there might be faster ones, these came to mind):

1. Apply the rotations around the axes of the hip joints to the -y vector(paying attention to their order, and that the axes should be affected by previous rotations).
2. Store the original relative quaternion between femur and pelvis (should be available in model->body_quat), then you can get the current quaternions of the pelvis and femur from data->xquat, get the relative rotation, compare with original, apply difference to -y vector.

[Rakesh-Paul01]

Thanks for the reply @Balint-H. I don't fully understand what your trying to say could you elaborate a bit more?
What I understood is I have to store the original quaternion, then compare that with the current quaternion, so that will give me the relative rotation. I don't understand there after what do you mean by apply the difference?

Also I am not sure but I think this will work only on a single bone at a time. What if I want to apply this on all the bones?

i.e I move a bone let's say the upper arm, now the forearm is pointing at a direction wrt it's local axis (and will always point at that direction unless the forearm's qpos is changed). So this is the original vector then when we change the qpos of the forearm we get a new vector wrt to the local axis (this is what we are interested in). Now due to this changes the wrist will also be in a new position but this is the initial position then when we change the qpos of the wrist that will be the new vector for the wrist.

I get that we store the quaternion but won't that quaternion change as we move one bone after the other? (I guess in some sense the xquat will come in handy but can't figure out how)

[Balint-H]

Sorry for the vague terminology!

Local quaternions are the "difference" of 2 global quaternions, let's call it q_local = q_child * q_parent^-1. You are interested in what the current local quaternion is compared to the starting local quaternion (which will be a "difference" as well), i.e. q_rel_t = q_local_t * q_local_t0^-1 . q_child and q_parent are available in data->xquat, which is updated after the forward kinematics step in the simulation loop.

I'm putting "difference" in quotes as "quaternion difference" may refer to another concept as well, here I just meant multiply a quaternion by the inverse of another.

By "applying" a rotation to a vector, I mean to rotate a vector by it. If you are in your resting position, q_rel will be the identity quaternion, and will not move your -y vector when your rotate it using q_rel. At any other time, if you rotate your -y vector with q_rel_t, it will give you the direction the femur is pointing described in the current pelvis frame.

I think this approach is extendable to all body segments in your kinematic tree.

[Rakesh-Paul01]

@Balint-H Thanks for the detailed explanation. It makes sense to me now. Also correct me if I am wrong q_local_t0 is the same as model->quat right?

[Balint-H]

That's my expectation for model->body_quat, but please double check that. Also, wanted to mention that instead of xquat you can also operate with xmat, if you are more comfortable operating with rotation matrices instead of quaternions.