I am attempting to code iLQG in mujoco for a model that contains a couple equality constraints. My state x is a subset of the mujoco state qpos and qvel. The states that are in qpos that are not in x are constrained by equality constraints in the model. As part of the iLQG algorithm I need to find df/dx and df/du at points along the trajectory. I can set the relevant values of qpos and qvel from x but the constrained values won't be updated right away. I could calculate what these values would have to be but I thought mujoco must already be doing that calculation. I guess what is the best way of setting the position for a model with equality constraints?
Equality constraints (and all other constraint) in MuJoCo are somewhat soft, meaning that they are not exactly satisfied. So it is better to just treat the entire system as if it is unconstrained, compute the derivatives with respect to all state variables, and leave it to the dynamics to impose soft constraints. What this means numerically is that the resulting Jacobians will be nearly rank-deficient (in the soft-constrained subspace) but this will not interfere with iLQG. The only hard constraint involves quaternion joints -- in that case qpos is 4D but it is always normalized to unit length, so the corresponding qvel is 3D. Look at derivatives.cpp to see how this can be handled.
Thanks for all of your prompt replies it has certainly helped. The way I interpreted your response was that it is fine to just run FD just like it is done in derivatives.cpp but maybe I didn't interpret that correctly. After implementing the iLQG algorithm I was able to get it working with simple problems and some standard mujoco models but when I try the custom mujoco model that has multi bar linkages I can't get the algorithm to work. After a lot of debugging I am thinking that the FD aren't quite correct which causes the gradient update step in iLQG to be pointed in the wrong direction. So I am suspecting that the FD results I am getting are incorrect due to the equality constraints. I'm not sure exactly how to prove it... but one thing that makes me suspicious is seeing a big difference in linearization when the solref parameter is changed on one of the constraints. So now when I am trying to calculate dxdd/dx it is saying the linear acceleration of the main body is incredibly sensitive so some small internal joint position change which shouldn't be true... so that makes me think that maybe I misinterpreted your original response. Thanks again!
The constraint dynamics are indeed very sensitive to the solref and solimp parameters. This is by design. If you think of the constraint force in a linear context as a spring-damper, the derivative with respect to position is the stiffness coefficient and the derivative with respect to velocity is the damping coefficient. Assuming you keep solref[1] = 1 you get critical damping, with time-constant given by solref[0]. The exact formulas are in the documentation, but basically, the stiffness coefficient depends on 1/solref[0]^2. So when solref[0] gets close to zero, you should indeed expect the derivative to be very sensitive to this parameter. Now, you may prefer to impose strict constraints, and only compute derivatives in the unconstrained space - which has lower dimensionality. If you happen to know what that space is, you could rearrange the finite difference algorithm: instead of perturbing along the position axes, perturb along the axes of some coordinate frame that spans this lower-dimensional space. derivatives.cpp illustrates how to do this in the case of quaternion joints. Such joints have 4 positions but only 3 dofs. The derivative is defined over a 3D space, and the perturbations are computed using the properties of unit quaternions. BTW, derivatives.cpp used to have an indexing bug which is now fixed in the file linked from the documentation. In general though, the above analytical approach is not feasible and you have to deal with the system numerically. My solution is to develop a new algorithm called acceleration-based iterative LQR (AILQR) which is not yet published. It uses inverse dynamics rather than forward dynamics, and is specifically designed to deal with constrained systems. In the meantime, iLQG is your best bet. Pick some reasonable solref and solimp parameters, fix them, adjust the finite difference epsilons, and watch out for ill-conditioned matrices in the Riccati equations. You could simply add a constant to the diagonal of the matrix that gets inverted. This typically results in suboptimal controllers that are more robust, in the sense that the feedback gains are less aggressive. In our work with iLQG in the context of MPC we had to play similar games, including adapting this diagonal term in Levenberg-Marquardt fashion and line search in the forward pass. I had hoped that by now this kind of trajectory optimization would be a solved problem, but it isn't, so my current project is to develop and fine-tune new algorithms that solve it, and make them available in MuJoCo so people don't have to reinvent the wheel every time. I cannot promise anything about the timeline though, so don't wait for me
As always thanks a lot! Ya I have basically recreated everything you had in the synthesis and stabilization paper as well as the boxqp variant for limiting motor torques. I think all of that is working as it should... I just need to get the right linearization of the dynamics. I look forward to AILQR... I think I have read all of your other papers in trying to get this to work. I was thinking of changing my action space to target accels in which case I would have to use the inverse dynamics along with the QP constrained optimization approach... haven't really thought it all the way through... need to figure out my FD issues first. I think I may know where I am going wrong... I am performing FD on the full mujoco state (32 for my model) but then I am only taking the dimensions I care about (basically throwing out the extra links that cause the model to be constrained) so now I have 16 dimensions when going forward with the iLQG algorithm. So it sounds like I should either keep the full 32 constrained dof or perform FD on the lower 16 dof system (given the analytical relationships)... however besides some slight intuition I can't say with too much confidence why this would be true. I guess the internal forces created by equality constraints need to be balanced out so it could be I am getting rid of some DOF that balance out that relationship in the final A matrix which makes it look like the dynamics allow positive net work to be done just by moving constrained links. Thanks, Taylor
What I said about limiting the derivatives to the correct subspace assumes you can impose the equality constraints exactly -- but MuJoCo does not do that. If you ignore the softness of the constraints and project in the lower-dimensional space anyway, you will obtain some embedding that seems like a good idea intuitively but who knows how well it will work. On the other hand 32 is not that high, so you might as well work in the full space. In the limit of infinitely hard equality constraints, the linearized dynamics in the full space will become a singular matrix. But you will not be operating in that limit.