Get actuator jacobian

Discussion in 'Simulation' started by Robert, Apr 2, 2020.

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

        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;