CentroidalMPC incompatible with Casadi 3.6.7

[GiulioRomualdi]

Casadi 3.6.7 was released yesterday and landed on conda-forge with conda-forge/casadi-feedstock#125. Unfortunately, it introduces some breaking changes and our software does not work anymore (CentroidalMPC) https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/11123408780/job/30906799659?pr=895#step:11:128

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
CentroidalMPCUnitTests is a Catch2 v3.7.1 host application.
Run with -? for options

-------------------------------------------------------------------------------
CentroidalMPC
-------------------------------------------------------------------------------
/home/runner/work/bipedal-locomotion-framework/bipedal-locomotion-framework/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp:84
...............................................................................

/home/runner/work/bipedal-locomotion-framework/bipedal-locomotion-framework/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp:[134](https://github.com/ami-iit/bipedal-locomotion-framework/actions/runs/11123408780/job/30906799659?pr=895#step:11:135): FAILED:
  REQUIRE( mpc.initialize(handler) )
due to unexpected exception with message:
  Exception translation was disabled by CATCH_CONFIG_FAST_COMPILE

/home/runner/work/bipedal-locomotion-framework/bipedal-locomotion-framework/src/ReducedModelControllers/tests/CentroidalMPCTest.cpp:134: FAILED:
  {Unknown expression after the reported line}
due to unexpected exception with message:
  Error in Opti::to_function [OptiNode] at .../casadi/core/optistack.cpp:339:
  Error in Opti::to_function [OptiNode] at .../casadi/core/optistack.cpp:339:
  .../casadi/core/optistack_internal.cpp:975: Psd constraints not implemented
  yet. Perhaps you intended an element-wise inequality? In that case, make sure

[traversaro]

Probably related to:

• bipedal-locomotion-framework/src/ReducedModelControllers/src/CentroidalMPC.cpp

  Line 934 in 3f19707

  this->opti.subject_to(casadi::MX::mtimes(rotatedFrictionCone, //

  
  as this is the only non-scalar inequality in the problem.

[traversaro]

But that seems already to be a vector, probably there is something wrong going on.

[GiulioRomualdi]

But that seems already to be a vector, probably there is something wrong going on.

You are right 🤔

[traversaro]

I tried a simple test on Python to understand to reproduce the issue, but this works fine:

import casadi
import numpy as np

opti = casadi.Opti()

x = opti.variable()
y = opti.variable()

xy = casadi.vertcat(x+1000.0, y+1000.0)

opti.minimize(  (y-x**2)**2   )
opti.subject_to( casadi.mtimes(casadi.DM.eye(2),xy) >= casadi.DM.zeros(2,1) )
opti.subject_to(       x+y>=1 )

opti.solver('ipopt')


sol = opti.solve()

print(sol.value(x))
print(sol.value(y))

[GiulioRomualdi]

I'm able to reproduce the issue on my PC. I'm trying to understand why this is happening

[GiulioRomualdi]

The problem is this line:

bipedal-locomotion-framework/src/ReducedModelControllers/src/CentroidalMPC.cpp

Lines 939 to 943 in 3f19707

	this->opti.subject_to(
	0 <= casadi::MX::mtimes(casadi::MX::reshape(contact.orientation(Sl(), i),
	3,
	3),
	corner.force(Sl(), i)(2)));

[GiulioRomualdi]

 this->opti.subject_to( 
     0 <= casadi::MX::mtimes(casadi::MX::reshape(contact.orientation(Sl(), i), 
                                                 3, 
                                                 3), 
                             corner.force(Sl(), i)(2))); 

should be

 this->opti.subject_to( 
     0 <= casadi::MX::mtimes(casadi::MX::reshape(contact.orientation(Sl(), i), 
                                                 3, 
                                                 3), 
                             corner.force(Sl(), i))(2)); 

I'm wondering how it has worked so far @mebbaid @evelyd @LoreMoretti

[GiulioRomualdi]

I tried to run the test without and with the fix on my setup

Total number of variables............................:      555
                     variables with only lower bounds:        0
                variables with lower and upper bounds:        0
                     variables with only upper bounds:        0
Total number of equality constraints.................:      267
Total number of inequality constraints...............:      384
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:      384


Number of Iterations....: 18

                                   (scaled)                 (unscaled)
Objective...............:   1.6304533651274522e+01    1.6304533651274522e+01
Dual infeasibility......:   4.8691290815759236e-11    4.8691290815759236e-11
Constraint violation....:   6.9944050551384862e-15    6.9944050551384862e-15
Variable bound violation:   0.0000000000000000e+00    0.0000000000000000e+00
Complementarity.........:   2.5127650985070973e-09    2.5127650985070973e-09
Overall NLP error.......:   2.5127650985070973e-09    2.5127650985070973e-09


Number of objective function evaluations             = 19
Number of objective gradient evaluations             = 19
Number of equality constraint evaluations            = 19
Number of inequality constraint evaluations          = 19
Number of equality constraint Jacobian evaluations   = 19
Number of inequality constraint Jacobian evaluations = 19
Number of Lagrangian Hessian evaluations             = 18
Total seconds in IPOPT                               = 0.118

EXIT: Optimal Solution Found.
      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  | 249.00us ( 13.11us) 245.76us ( 12.93us)        19
       nlp_g  | 721.00us ( 37.95us) 710.01us ( 37.37us)        19
  nlp_grad_f  | 483.00us ( 24.15us) 480.14us ( 24.01us)        20
  nlp_hess_l  | 673.00us ( 37.39us) 670.40us ( 37.24us)        18
   nlp_jac_g  |   1.09ms ( 54.65us)   1.10ms ( 54.89us)        20
       total  | 118.65ms (118.65ms) 118.66ms (118.66ms)         1

after the fix

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit https://github.com/coin-or/Ipopt
******************************************************************************

Total number of variables............................:      555
                     variables with only lower bounds:        0
                variables with lower and upper bounds:        0
                     variables with only upper bounds:        0
Total number of equality constraints.................:      267
Total number of inequality constraints...............:      480
        inequality constraints with only lower bounds:       96
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:      384


Number of Iterations....: 21

                                   (scaled)                 (unscaled)
Objective...............:   1.6304533651274522e+01    1.6304533651274522e+01
Dual infeasibility......:   5.1074157832289900e-11    5.1074157832289900e-11
Constraint violation....:   6.9944050551384862e-15    6.9944050551384862e-15
Variable bound violation:   0.0000000000000000e+00    0.0000000000000000e+00
Complementarity.........:   2.5125615829873444e-09    2.5125615829873444e-09
Overall NLP error.......:   2.5125615829873444e-09    2.5125615829873444e-09


Number of objective function evaluations             = 22
Number of objective gradient evaluations             = 22
Number of equality constraint evaluations            = 22
Number of inequality constraint evaluations          = 22
Number of equality constraint Jacobian evaluations   = 22
Number of inequality constraint Jacobian evaluations = 22
Number of Lagrangian Hessian evaluations             = 21
Total seconds in IPOPT                               = 0.134

EXIT: Optimal Solution Found.
      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  | 254.00us ( 11.55us) 250.10us ( 11.37us)        22
       nlp_g  | 722.00us ( 32.82us) 710.29us ( 32.29us)        22
  nlp_grad_f  | 467.00us ( 20.30us) 611.34us ( 26.58us)        23
  nlp_hess_l  | 720.00us ( 34.29us) 718.72us ( 34.22us)        21
   nlp_jac_g  |   1.16ms ( 50.30us)   1.19ms ( 51.92us)        23
       total  | 132.79ms (132.79ms) 134.54ms (134.54ms)         1

With the fix, the number of constraints increases. So I think before the fix the constraints were just discarded

[mebbaid]

So the limit on the normal force was not restricted to be positive always, but somehow it finds positive normal forces. In my case there is also an additional constraint on the force here https://github.com/mebbaid/bipedal-locomotion-framework/blob/0fdd9c605f6f7dd3d9d67fb631c5a813c9ff3509/src/ReducedModelControllers/src/CentroidalMPC.cpp#L1287-L1292 which may explain it, since we already have a positive force coming from this term computation https://github.com/mebbaid/bipedal-locomotion-framework/blob/0fdd9c605f6f7dd3d9d67fb631c5a813c9ff3509/src/ReducedModelControllers/src/CentroidalMPC.cpp#L1282-L1284

But in the nominal case I am not sure why we do not need to limit the normal force from below

[mebbaid]

I'm wondering how it has worked so far @mebbaid @evelyd @LoreMoretti

On possibility is that even if the constraint is discarded, this cost term https://github.com/mebbaid/bipedal-locomotion-framework/blob/0fdd9c605f6f7dd3d9d67fb631c5a813c9ff3509/src/ReducedModelControllers/src/CentroidalMPC.cpp#L1356-L1361 somehow ensures that it is always non-negative ?