Why does an MPC formulation in Drake lead to a slow simulation results even for a simple system?

I have a box resting on a table. I am trying to find a point on the box and a force with which to push at the point such that the box reaches a final position. I designed an MPC problem whose objective function minimizes the position error and the force over a short time horizon. The optimization problem uses the dynamics of the box (forward Euler), distance of the point to the box surface, distance to the final position as constraints. The problem is formulated with direct collocation, which means the position, velocity, force, and point on the box are optimization variables at every time step. I am encountering that this approach leads to a really slow simulation and optimization results. The optimization on average takes 0.5-0.8s and the controller seems to wait for full convergence before executing a step. I am looking for some insight into how to fix this and where the problem could be coming from. More specifically:

Is there a way to make the simulation more continuous? Are there any parameters of the simulation that I can change so that I can query the optimization faster?
Or could the problem arise from the fact that we are dealing with a highly nonlinear problem because we are dealing with contact and plant dynamics?

We tried different solvers and the results remain slow. We decreased the prediction horizon and maximum iterations. We also played around the discretization time step in the forward Euler, in the sampling, in the zero order hold, etc. We are using Python at this time.

Solution

The first thing I would check is: how often is your trajectory optimization getting evaluated? The MultibodyPlant typically has a timestep of ~1ms. If you are wiring the input port the multibody plant directly to the output of your MPC, then you could be trying to solve those optimizations at 1 kHz (simulation time). That's probably vastly more than you need. Consider adding a Zero-Order Hold block in between the two systems, or defining your MPC controller as a discrete time system (so that the computation happens on the state update, but not on the output port).

The second tip is that creating a new MathematicalProgram from scratch every time is too inefficient for a fast MPC loop. Standard practice would be to create the program once, but update the costs and constraints and resolve on each timestep. For simple costs/constraints we offer methods to support these updates. For user-defined constraints, it should not be hard to just update the data.