STORM: An Integrated Framework for Fast Joint-Space Model-Predictive Control for Reactive Manipulation

Sampling-based model-predictive control (MPC) is a promising tool for feedback control of robots with complex, non-smooth dynamics, and cost functions. However, the computationally demanding nature of sampling-based MPC algorithms has been a key bottleneck in their application to high-dimensional robotic manipulation problems in the real world. Previous methods have addressed this issue by running MPC in the task space while relying on a low-level operational space controller for joint control. However, by not using the joint space of the robot in the MPC formulation, existing methods cannot directly account for non-task space related constraints such as avoiding joint limits, singular configurations, and link collisions. In this paper, we develop a system for fast, joint space sampling-based MPC for manipulators that is efficiently parallelized using GPUs. Our approach can handle task and joint space constraints while taking less than 8ms~(125Hz) to compute the next control command. Further, our method can tightly integrate perception into the control problem by utilizing learned cost functions from raw sensor data. We validate our approach by deploying it on a Franka Panda robot for a variety of dynamic manipulation tasks. We study the effect of different cost formulations and MPC parameters on the synthesized behavior and provide key insights that pave the way for the application of sampling-based MPC for manipulators in a principled manner. We also provide highly optimized, open-source code to be used by the wider robot learning and control community. Videos of experiments can be found at: https://sites.google.com/view/manipulation-mpc

In the video below we demonstrate the effect of our different cost terms on the behavior of the robot in simulation. Note that the goal pose used for pose reaching costs in the video is unreachable once the robot takes self and environment collisions into account. Collisions with the table have been turned off in the simulator.

In the plot below, we show sampling controls (accelerations) with comb-filtering on the left and B-spline sampling (ours) on the right. The robot is trying to reach a Cartesian position, whose error is shown in “Euclidean Error(m)” plot. We can see that the joint velocity doesn’t ramp up and stays within 0.1 rad/s while B-splines allow the robot to reach high joint velocities (0.6 rad/s).

Our framework automatically adapts the covariance of the Gaussian distribution used by MPPI based on the costs from the rollouts. Covariance adaptation reduces the tuning effort required in setting the initial covariance and overcomes jittery behavior near the goal with a fixed covariance.