Center of mass jacobian

Discussion in 'Simulation' started by Jongwoo Lee, Apr 26, 2018.

  1. Hello,

    What is the best way to get center of mass Jacobian? My best technique is to use the following expression, using the fact that

    x_COM = sum { mi/M*x_com_i }


    . Is there any better way to get this ?


    Code:
            // --------------------------- COM Jacobian
            mjtNum mi;
            mjtNum *jacp_i, *jacr_i;
            mjtNum *jacp_COM = new mjtNum[3 * (m->nv)]{}, *jacr_COM = new mjtNum[3 * (m->nv)]{};
            for (int i = 1; i < m->nbody; i++) {    // exclude i=0 the world body
                mi = m->body_mass[i];
                mi = mi / m_tot;
                mj_jacBodyCom(m, d, jacp_i, jacr_i, i);
                mju_addToScl(jacp_COM, jacp_i, mi, 3 * (m->nv));
                mju_addToScl(jacr_COM, jacr_i, mi, 3 * (m->nv));
            }
            //------------------------------------------
     
  2. Emo Todorov

    Emo Todorov Administrator Staff Member

    Yes, this is how you do it.
     
    Jongwoo Lee likes this.
  3. Thanks!