Set complementary conditions constraints

MPC Discussions
1

Hi :wave:

I’m using acados to do solve an mpc problem of quadruped robot. Everything went well and the solver is really fast. But I’m confuse that how could I set some complementary conditions as constraints.
For example, I’m trying to generate gait automatically (including whether a leg is swinging or stance at any shooting node) using mpc, where stance leg should be on the ground(the z position is 0) and swing leg should has no ground reaction force. This becomes a complementary condition that footPos_z*groundReactionForce_z >= 0.
When I add this into nonlinear constraints:

# complementary conditions
            constraints[i * 10 + 9] = self.controlVars[i * 3 + 2] * (
                self.stateVars[12 + i * 3 + 2] - footRadius)

and set both lh and uh to 0 or a little positive number(0.1), the solver only succeed at standing gait(four leg at stance).

Could anyone tell me how to treat complementary conditions with acados correctly?

Thanks in advance for your assistance!

2

Or is there any way to set the tolerance of this constraint individually to be more slack, considering that this constraint is too strong?

3

Hi,

From my experience, there might not be easy ways to solve this in acados (probably same for any state-of-the-art OCP solver), since contact implicit MPC itself is hard.

I guess what you want is to slack the complementary constraints as F \cdot z = \mu and shrink the value of \mu iteratively (similar to what interior point method does). Not sure how to achieve this in acados. I’m also waiting for an answer here :face_with_monocle:.

Besides, I’m curious how did you applied acados to quadrupeds :grinning:. Is there any open-source implementation available at the moment? Would be excited to have a look!

Best,
Fenglong

4

Hi,
Thanks for reply! Indeed, and I noticed someone use bilevel optimization in some paper, solving the LCP by interior point method individually, which needs organize the solver by themselves. I’m new in optimization so I just wanted to have a try directly with acados :joy:
I just use acados to solve an mpc of single body dynamics model of quadruped, where the state variables are position, linear velocity, rpy, angular velocity and the 3D position of feet in world frame, and the control variables are the ground reaction force and 3D velocity of feet, which is a little bit different from what was done in ocs2. I just substitute the mpc module in MIT’s framework with this and use mujoco to simulate. You may like to have a look at my code and give me some advice :joy:
https://github.com/lrchit/quad_nmpc.git
I also tried to recurrent the mpc of centroidal dynamics in ocs2 with casadi-pinocchio interface but something weird happened and it was too difficult for me to make it :joy:

Best,
lrc

5

Hi Irc,

thanks for sharing the code. It looks great!

I’ve also tried to use acados on legged robots, but as soon as I go further than a linear model, a rigid body dynamics library will be needed. Pinocchio2 doesn’t support CasADi in python interface and Pinocchio3 is still painful to work with. So I haven’t got a good idea neither…

Back to solving locomotion problems, I think the most realistic way is still to pre-specify contact sequences and timings, just same as what OCS2 does. This approach is proved to work well in many cases. But if your goal is to solve contact-implicit MPC in acados, I don’t know what is a good way to deal with complementarity constraints. I found there is a function called custom_update() which might provide some freedom to customize the solver. Not sure how to use that.

Hope you finally find out a solution!

Best,
Fenglong

6

Hi, Fenglong

It’s indeed that if you use pinocchio3-casadi interface, the code generation and compilation with C++ takes much more time. I’ve tried to do so(centroidal dynamics things) the same as what ocs2 did and it could keep the robot standing if I didn’t add too complex constaint. But the solver failed when I add non-velocity constraint to the stance leg with pinocchio. I shared the problem here https://discourse.acados.org/t/trouble-with-nonliear-constraints-on-derivative-of-state-sqp-rti-qp-solver-returned-error-status-3-qp-iteration-15/1487?u=lrc, but there’s no solution yet. I preliminary think that could be solved by provide external hessain and gradient of the dynamics and constraint using pinocchio for acados, but it’s too hard for me to do so. So I used ocs2 instead and it was really efficient in my another simulation. What I did with acados I mentioned before was a simplified version of that using single rigid body dynamics of the COM and substituting the joint states with foot position and it had similar performance. :joy:

What I’m interested in now was to generate a heuristic motion for quadruped robot without a given contact sequence, as it’s more intuitive for an animal’s motion. Older work on it was considering the feasible impulse set(FIS) which assess the contribution of each leg comparing their feasible impulse polytope(FIP) across the horizon(N steps), and it generate the foothold and contact sequences. While the benefit of this is that there’s no need to modify the well-tested mpc-wbc framework, there’s no room for further research. So I’m researching for ocp based methods considering contact-implicit model to generate the gait automatically. Most methods are hard to implement because of the calculation cost, while some more efficient methods are too hard to try it myself. It’s the first year for me as a graduate student and I know too little about this, but I’m going to keep on it. Thanks for sharing and looking forward to more future exchanges with you! :smiley:

Best,
lrc