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.

    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;