How are constraints and efc_J initialized after initializing mjData and mjModel?

Discussion in 'Priority support' started by alei7, Dec 16, 2019.

  1. Our model has a closed-loop constraint with partially unmeasured states. So we need to use closed loop equality constraint to estimate those unmeasured states. The process as follows,

    1. assign entire qpos to mjData with partially incorrect for unmeasured states.
    2. mj_forward() to get constrained Jacobian (efc_J) and violation vectors (efc_pos).
    3. Use corresponding submatrices in efc_J and efc_pos, along with efc_pos, to get velocity vectors dq.
    4. Use mju_quatIntegrate() for quaternions and q=q+dq for joints to update unmeasured states.
    5. repeat 1-4 until convergence.

    After the mjModel and mjData being initialized, and after calling mj_forward() with partially unmeasured qpos, all matrices and vectors related to constraints are empty, ie efc_J, efc_pos, efc_id. Also, the njmax or nemax are all 0, which should be nonzero or at least -1(default).

    Can you give us some hints what could cause empty efc_J and even njmax here?
     
  2. Emo Todorov

    Emo Todorov Administrator Staff Member

    mjModel.njmax and nemax are the maximum number of constraints allocated at compile time. The number of active constraints at runtime is in mjData.nefc, ne, nf, ncon. Did you check the latter? How do you know that the efc_XXX quantities are empty -- are they numerically zero? If so, it could be that you have not defined the constraints in the model, or that you need to increase the maximum numbers (nemax, njmax) so that the model compiler allocates sufficient space for your constraints.

    Note that mjData.efc_vel is the constraint velocity, so you do not have to compute it from efc_Jac and efc_pos.
     
  3. Thanks for the quick reply. We will try to use efc_vel directly.

    We checked couple things. nefc, ne, nf, ncon are initialized to the correct size after mj_forward() and they are all 0 before calling forward(). We tried to initialize njmax with 1000, nemax with 100 and the efc_XXX are still numerically 0 after calling mj_forward(). We are pretty sure that the equality constraints work fine as we are using a model that has been working consistently. The only difference in the setup is we are calling this closed-loop constraint as an API-like function that will initialize mjModel and mjData with input qpos every-time calling it.

    Thank you!
     
  4. Emo Todorov

    Emo Todorov Administrator Staff Member

    You should not be changing constants in mjModel after it is compiled by the MuJoCo compiler. njmax determines the static allocation of memory in mjData. Make sure you specify large enough values in the XML before compiling.

    So if you are seeing nefc>0, then efc_XXX has been populated.

    Could it be that your model is using sparse Jacobians and you are treating them as dense? There is an XML <option> to use dense Jacobians. The default is to decide automatically based on how large the model is.