search

leggedrobotics / ocs2

Optimal Control for Switched Systems
https://leggedrobotics.github.io/ocs2
BSD 3-Clause "New" or "Revised" License
915 stars 229 forks source link

hard constriants violation #108

Open min-dai opened 3 months ago

min-dai commented 3 months ago

Hello,

I was testing OCS2 and had problems with hard equality and inequality constraints. With default SQP and IPM settings, both solvers ignore the hard input inequality constraints. When testing the same constraint as equality constraints, only the linear portion of the constraints are satisfied. I have verified the getValue and getLinearApproximation functions. Also even when constraints are not satisfied, for both solvers MPC continued to run without reporting infeasibility.

FenglongSong commented 2 months ago

I don't know much about the IPM implementation in OCS2, but the SQP solver uses Relaxed Barrier Functions to deal with inequality constraints, which is actually a penalty method, meaning that there is not hard constraints at all (therefore the optimization is always feasible). As a result, it cannot guarantee constraint satisfaction.

You can try to tune the delta and mu parameters as here. This might helps, but still, there is no guarantee.

JunJieChenpete commented 1 month ago

I met similar questions these days. I found that the hard constraint is not dealed with Relaxed Barrier Functions. Actually, they are directly transport into HPIPM, https://github.com/leggedrobotics/ocs2/blob/main/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp#L367. The Relaxed Barrier Functions is uesd only in soft constraints(you must use the softConstraintPtr with specific barrier penaty configs), which is totally differ from what is declared in https://leggedrobotics.github.io/ocs2/optimal_control_modules.html, which said "Hard constraints (referred to as constraints) are handled with higher precision through different techniques depending on their type. ...The state-only equality and inequalities are handled either through a relaxed-barrier method or an augmented Lagrangian technique...". When I really looked into the HPIPM, I found that it uses interior point method for inequality constraints, which indicates that all the constraints is hold as hard constraints. @FenglongSong , sorry to bother you, It seems that you are familiar with SQP implement in ocs2, is there somthing wrong with what I mentioned? It really made me confused these days.

FenglongSong commented 1 month ago

Hi @JunJieChenpete , actually I don't know much about implementation details of OCS2. But as I checked the code and trying to find out how do they deal with the constraints, I can see they pass the state-input equality constraints into HPIPM here. This deals with the state-input equality constraints each through augmenting the system dynamics (so called projection method), or directly through the $d_l \leq Cx_k+Du_k \leq d_u$ by setting same value for lower and upper bounds (you can find it here)

But I don't see how the inequality constraints are passed to HPIPM (at least I didn't find it after a little search). So, either the inequalities are not dealt with, or they're added into the cost function somewhere.

So maybe my previous statement "inequality constraints are dealt with by Relaxed Barrier Function" is wrong. This doesn't solve your problem, but hope it's a little helpful. If you find the answer please let me know :)