Is there a function for mapping global xpos/xquat to local?

Discussion in 'Priority support' started by alei7, Oct 3, 2019.

  1. Hello,

    I noticed there is a function mapping local pos/xquat to global ones. But I could not find the reverse function. I am trying to convert global xquat into local xquat w.r.t. another body in the tree (Pelvis body). Attached is the way I try to do this, but the resulting local quaternions do not match with the true data. Is there any built-in way to do this in mj functions? Thank you!

    Screenshot from 2019-10-03 12-22-38.png
  2. Emo Todorov

    Emo Todorov Administrator Staff Member

    In general, the spatial frame (or pose) of a body has position and orientation, and they should be treated as one object. The functions mju_mulPose and mju_negPose can be used to multiply (i.e. accumulate poses) as well as invert them.

    If you only need the orientation component, positions can be ignored (while the opposite is not true). It is better to work with quaternions directly instead of converting them to matrices. The functions you need are mju_mulQuat and mju_negQuat. Suppose A is a global quaternion, you apply local quaternion rotation B to it, and obtain global quaternion C:

    A * B = C

    To solve for B, multiply both sides by the conjugate of A:

    B = conjugate(A) * C

    Using MuJoCo functions:

    mjtNum A[4], B[4], C[4], Aconjugate[4];
    // initialize A and C
    mju_negQuat(Aconjugate, A);
    mju_mulQuat(B, Aconjugate, C);
    alei7 likes this.
  3. Thank you Professor!