[swing-up] Closed loop real-time NMPC using crocoddyl

[pierfabre]

For now we are able to generate a swing-up trajectory using crocoddyl offline.

• Can we go under 5Hz of control freq
  • Code generation
  • Multi-threading
  • implement calcDiff
  • try with urdf
  • ask for help
• Is the model accurate
  • sim2real xp
  • better visualization of sim

[pierfabre]

20240422 - First closed loop swing up with NMPC at 100Hz

For the simulation we use pinocchio with an urdf from the robot. We add to that viscous and dry friction so that the simulation differs from the model given to the controller. To solve the ode we use Euler with time step 1e-5.
T = 1.0s
control_freq = 100Hz
We first solve the swing up offline and use this as a warm start for the first loop.
At each iteration we use the previous solution as a warm start
To solve we use BoxDDP with 3 max iter and feasible=False.
state_cost=0.1
terminal_state_cost=1000
control_cost=1e5/dt

Here is the result

MPC has to replan at each timestep because of integration error and model error :

state	command

The solver takes less than 10ms to give the command so it would fit in 100Hz !

The trajectory found is pretty smooth

Capture.video.du.22-04-2024.19.54.10.webm

[pierfabre]

20240716 - Update on NMPC swing up with the new URDF

• Following the improvements made on system id (see system identification #68) I wanted to see if my optimal control solver crocoddyl was still able to find a way to swing up the pendulum
• Previously I used BoxDDP (which allows to constrain the control input) but this time I wanted to use the SQP recently introduced in mim_solvers that "solves nonlinear constrained OCPs efficiently by leveraging sparsity".
• In the future it would also be interesting to compare performance with ProxDDP

nmpc_swing_up.webm

• Next step : Swing up of the real robot! 🚀