NLP with high volume of constraints for obstacle avoidance

[Rowan Dempster]

Problem setup 

I have implemented a model predictive controller to solve a path following problem, and I am using nlpsol to solve the resulting NLP. Next, I want to avoid obstacles in the environment around the path using a 150x150 cell occupancy grid around the robot. To do so, I am attempting to add constraints to the NLP which enforce that the robot's state does not intersect with an occupied cell in the grid. 

Intuitive implementation

Currently, I am passing the 150*150 cells into the NLP via a SX parameter vector. For each step in the predictive horizon, I calculate the 150*150 euclidian distance vector, where each entry is the distance between a cell and the robot's predicted state. I then add constraints that this distance vector needs to be greater than some safety distance.

Issue I am encountering

Adding these constraints over the predictive horizon (25 steps) adds 25*150*150 new constraints to the NLP. Under these constraints, solve runtime has increased from <0.05s to >1s.

Implementation variants I have tried

1. Most of the 150*150 cells are unoccupied, so their parameter vector entry is zero. I need to take advantage of this sparsity. The difficulty is that the sparsity changes at each iteration and is only known at runtime. I have tried using SX.nonzeros() on the parameter vector before calculating the distance vector. However this give the warnings:
   • CasADi - 2021-04-23 18:22:08 WARNING("solver:nlp_jac_g failed: NaN detected for output jac_g_x, at nonzero index 1 (row 7, col 0).") [.../casadi/core/oracle_function.cpp:265]
   • CasADi - 2021-04-23 18:22:08 WARNING("solver:nlp_grad failed: NaN detected for output grad_gamma_p, at (row 21, col 0).") [.../casadi/core/oracle_function.cpp:265]
   • CasADi - 2021-04-23 18:22:08 WARNING("Failed to calculate multipliers") [.../casadi/core/nlpsol.cpp:621]
2. After reading up on MX vs. SX (https://web.casadi.org/docs/#the-mx-symbolics) I tried to switch my NLP formulation to using MX symbolics which I thought might decrease the cost of the algebraic distance vector calculation. However, after making the switch my initialization call to nlpsol hangs at (it moves on to Star coloring on 4x4  after about 5 minutes)
   • CasADi - 2021-04-23 18:27:57 MESSAGE("Using live variables: work array is 203 instead of 1970") [.../casadi/core/mx_function.cpp:305]
   • CasADi - 2021-04-23 18:27:57 MESSAGE("Block size: 207") [.../casadi/core/function_internal.cpp:1106]
   • CasADi - 2021-04-23 18:27:57 MESSAGE("Star coloring on 1x1: 1 <-> 1") [.../casadi/core/function_internal.cpp:1119]

Any suggestions of how to enforce a high volume of constraints in an efficient manner, or another way to avoid occupied cells without introducing a high volume of constraints?

Thanks!

Rowan

[Joris Gillis]

Dear Rowan,

There is a wide range of options to encode obstacle avoidance in a dynamic optimization environment.

Our research group ( https://www.mech.kuleuven.be/en/pma/research/meco ) will be organizing a workshop in the future on this topic.
If you stick to raw data (not segmented into geometric primitives), you could for example code an custom Function (see Callback in docs) that computes the distance to a point cloud.

Best regards,

  Joris