Pretty straightforward question: is there a way to get the actuator jacobian in MuJoCo? It is for instance mentioned here: http://www.mujoco.org/forum/index.php?threads/qfrc_inverse.3463/#post-3986 The actuator jacobian maps the system inputs to generalized forces: qfrc_actuator = B*u During forward simulation it must be used in some form.
I don't think there is a built-in feature for this. Instead, I now determine it numerically. Which is okay because I only need to do it once, since all my actuators apply directly to a generalized coordinates. Code: MatrixXd jac(m->nv, m->nu); const int var_size = m->nu; // Size of ctrl mjtNum qfrc_center[m->nv]; mju_copy(qfrc_center, d->qfrc_actuator, m->nv); // Loop over variables to be varied for (int i_var = 0; i_var < var_size; i_var++) { double original = d->ctrl[i_var]; d->ctrl[i_var] += eps; // Perturb current variable // Perform torque forward operation mj_fwdActuation(m, d); // Loop through columns for (int c = 0; c < m->nv; c++) { mjtNum qfrc_perturbed_c = d->qfrc_actuator[c]; // Take finite difference double derivative = (qfrc_perturbed_c - qfrc_center[c]) / eps; // Place inside derivative vector / matrix jac(c, i_var) = derivative; } d->ctrl[i_var] = original; } mj_fwdActuation(m, d); // Reset actuation return jac;