PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Mohak Bhardwaj, Balakumar Sundaralingam, Arsalan Mousavian, Nathan Ratliff, Dieter Fox, Fabio Ramos, Byron Boots

arXiv:2104.13542v2cs.RO

TL;DR

High-dimensional manipulation makes sampling-based MPC computationally demanding, while task-space formulations cannot directly enforce several joint-space constraints. The paper presents GPU-parallelized joint-space STORM with learned perception costs and demonstrates fast, reactive manipulation on a Franka Panda. Its discussion identifies state uncertainty and high-speed model bias as remaining robustness boundaries.

  • Problem

    Sampling-based MPC is computationally demanding for high-dimensional manipulation, and task-space formulations cannot directly account for joint limits, singularities, and link collisions.

  • Method

    STORM is a GPU-parallelized joint-space sampling-based MPC framework combining smooth trajectory generation, behavior-based costs, learned collision costs, and adaptive sampling.

  • Results

    STORM achieves a control rate of 125Hz on a single GPU and demonstrates dynamic manipulation on a real Franka Panda, including median ball-positioning error of 3.9 cm without dropping the ball.

  • Takeaways & Limitations

    The framework demonstrates that sampling-based joint-space MPC can produce smooth, reactive manipulation while respecting constraints and incorporating learned perception components.

  • Takeaways & Limitations

    Robustness remains limited by unmodeled state uncertainty and potential high-speed bias from the kinematic model.

Abstract

from arXiv · show

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

1 Introduction

The paper targets real-time manipulation control that must combine perception, task objectives, and joint-space constraints. STORM addresses this with highly parallelized sampling-based MPC designed for smooth, reactive control on robot manipulators.

  • Motivation: Real-world manipulation requires perception-driven feedback while respecting joint limits, singularity avoidance, collisions, and task constraints.These competing objectives make real-time control difficult.
  • Motivation: Operational-space control projects task costs into joint commands but optimizes only the next time step, ignoring future actions and states.This limits its ability to plan over a horizon.
  • Approach: Sampling-based MPC handles discontinuous dynamics and complex costs without restrictive assumptions about the cost, dynamics, or policy class.Its rollout-based formulation is also straightforward to parallelize.
  • Approach: STORM uses a highly parallelized sampling-based MPC architecture to optimize complex objectives while enforcing smoothness, constraint satisfaction, and low control latency.The framework is designed to test sampling-based control on high-dimensional manipulators.
  • Contributions: The framework combines low-discrepancy sampling, smooth trajectory generation, behavior-based costs, and learned perception components for joint-space manipulation.Learned self-collision and environment-collision costs are integrated into the control loop.
  • Contributions: 125Hz is achieved by an open-source GPU implementation of sampling-based joint-space MPC, reported as a 100x speedup over existing MPPI manipulation implementations.The method is empirically evaluated in simulation and on a real Franka Panda robot.

2 Problem Definition

The paper formulates real-time manipulator feedback control as finite-horizon optimization under nonlinear constraints and imperfect models. MPC repeatedly samples and evaluates policies, then executes an action and re-optimizes from the resulting state.

  • Problem formulation: The problem is to generate a real-time feedback control law for a d-joint manipulator performing user-specified tasks in an unstructured environment.The robot must react to errors from dynamics modeling, state estimation, and perception while satisfying nonlinear constraints.
  • Algorithm: Algorithm 1 defines sampling-based MPC using an initial state, horizon H, batch size N, and K optimization iterations.The horizon indexes steps, while the batch indexes sampled control sequences.
  • Algorithm: The policy parameters comprise sequences of Gaussian means and covariances, while sampled controls, predicted states, and costs are batched across trajectories.These quantities support batched sampling, rollout, and cost evaluation.
  • Overview: Figure 2 presents the paper’s notation alongside the sampling-based MPC algorithm.The notation and algorithm are introduced together before the sampling-based manipulation formulation.
  • MPC formulation: MPC performs a shorter-horizon lookahead using an approximate dynamics model, cost function, and parameterized policy to minimize an objective.This is framed as optimizing a finite-horizon continuous-state, continuous-action MDP.
  • MPC formulation: After executing a sampled action, MPC re-optimizes from the resulting state and warm-starts from the previous solution using a shift operator.The terminal cost approximates cost-to-go beyond the planning horizon.

3 Sampling-Based Model Predictive Control

The controller samples and updates finite-horizon control policies, using GPU-parallelized joint-space dynamics and behavior costs to support reactive manipulation. It combines low-discrepancy sampling, smooth trajectory generation, adaptive covariance, and constraints for safety and task execution.

  • MPC formulation: Sampling-based MPC optimizes a finite-horizon Gaussian policy by rolling out sampled control sequences, evaluating costs, and updating policy parameters with a sample-based gradient.After optimization, the controller executes an action and shifts the distribution forward to warm-start the next timestep.
  • Policy adaptation: Covariance adaptation updates exploration independently across action dimensions, while mean and covariance updates are shifted forward after each executed action.The authors report that covariance updates improve performance with fewer particles and produce stable behavior near the goal.
  • Approximate model: The controller uses a joint-space kinematic model with batched tensor operations to compute trajectories, achieving an 8ms control latency.Forward kinematics, Jacobians, and state propagation are parallelized across batches and horizons for GPU acceleration.
  • Behavior and constraints: Weighted cost terms encode task-space reaching alongside joint-limit, stopping, and manipulability requirements in the MPC objective.The stopping term penalizes velocities that exceed a horizon-dependent limit, and the manipulability term penalizes configurations with small manipulability scores.
  • Collision avoidance: A learned neural network predicts closest inter-link distance from joint configurations to provide a computationally efficient self-collision cost.The network uses positional encoding of joint angles and a three-layer ReLU architecture.
  • Sampling and smoothing: Low-discrepancy Halton sampling and cubic B-spline interpolation are used to explore controls while producing smooth joint acceleration, velocity, and position trajectories.The paper contrasts this design with comb filtering, which requires tuning and does not guarantee smooth neighboring samples.

4 Experimental Evaluation

STORM is evaluated on reactive manipulation tasks involving moving targets, obstacle avoidance, orientation constraints, and dynamic ball balancing. Across these experiments, it maintains constraints and reactivity despite noisy perception and model bias.

  • Obstacle Avoidance: STORM handles moving targets in cluttered tabletop environments, prioritizing collision avoidance when requested poses are unsafe.Experiments used both block obstacles and thin walls, including targets unreachable without risking link collisions.
  • Orientation Constraints: 1.2485% median quaternion error was achieved while tracking a moving ball under end-effector orientation constraints.The controller maintained the specified orientation while continuing to track the ball.
  • Dynamic Object Balancing: The ball-balancing task used RGBD tracking at 30Hz and a simplified rolling model without explicit friction, inertial properties, or system identification.The task was designed to test robustness under model bias and noisy perception.
  • Overall Findings: The experiments indicate that simple models and intuitive cost functions can produce accurate, reactive manipulation while coupling perception tightly with control.Sampling-based MPC also permits directly encoding behaviors such as smooth motion in the optimization.

5 Related Work

Prior manipulation controllers trade off speed, global reasoning, and joint-space capability. STORM is positioned against fast local controllers, slower replanning methods, gradient-based joint-space MPC, and earlier sampling-based approaches.

  • Operational-Space Control: Operational-space controllers achieve 1–2 ms latency but rely heavily on higher-level planning to avoid local minima such as obstacles.Their speed comes with limited standalone global obstacle reasoning.
  • Online Replanning: Online-replanning methods provide a more global feedback approach but typically incur 140–1000 ms control latencies on high-dimensional systems.A custom-chip planner is cited as an exception, replanning at 1 ms for Cartesian pose reaching in semi-structured settings.
  • Joint-Space MPC: Gradient-based joint-space MPC methods have demonstrated acceptable 20–125 ms feedback latency on real manipulation systems.Examples include two-stage hierarchical iLQR and simulator-based iLQR on humanoid robots.
  • Sampling-Based Methods: Sampling-based manipulation control had mainly optimized joint positions without jointly considering velocity and acceleration limits.Earlier approaches also used learned collision classifiers or smooth joint-position sampling for planning.

6 Discussion

STORM combines joint-space sampling-based MPC, GPU-accelerated tensorized rollouts, behavior-oriented costs, and diverse sampling strategies to produce constrained, smooth, reactive manipulation. The discussion identifies uncertainty and high-speed model bias as remaining concerns.

  • Contributions: STORM operates in joint space and produces smooth, reactive motions while respecting constraints on dynamic manipulation tasks.Its fully tensorized kinematic model enables GPU-accelerated rollout computation.
  • Design Choices: Intuitive cost terms and diverse sampling strategies embed desirable behaviors directly into the optimization.The formulation uses sampling choices as a way to shape motion properties.
  • Limitations and Future Work: Directly accounting for state uncertainty could make performance more robust.This is identified as an unresolved question rather than a demonstrated capability.
  • Limitations and Future Work: At higher speeds, the kinematic model may introduce significant model bias, motivating residual dynamics models or terminal Q-functions.These additions are proposed as ways to mitigate bias while maintaining computational speed.

A Real-Time Control Implementation

The real-time implementation batches the MPC computation in PyTorch, separates it into a multiprocessing process, and connects perception, state estimation, MPC, and low-level torque control. The system is evaluated through latency comparisons for collision-aware high-dimensional control.

  • MPC Pipeline: The MPC pipeline uses PyTorch with batched forward kinematics, cost terms, and update equations, while multiprocessing avoids latency issues.Timing is compared against leading manipulator control approaches using a Titan RTX GPU.
  • Command Generation: MPC commands are evaluated at 100Hz and integrated forward to obtain desired joint positions.These desired positions feed the downstream robot-control pipeline.
  • Low-Level Control: A custom low-level torque controller computes desired torques at 1000Hz for the Franka robot.The controller receives commands from the MPC layer.
  • Low-Level Control: The control architecture combines model-based torque computation with position and velocity feedback gains.The implementation uses inertia and Coriolis matrices from libfranka.
  • State Estimation: A joint-state filter predicts state from commanded acceleration and blends sensor readings with an exponential moving average.The filter addresses prohibitive noise in joint states, especially velocities.
  • Perception: Raw scene and robot point clouds are generated from an Intel RealSense D455 setup and supplied to SceneCollisionNet for collision computation.Scene points inside the robot body are filtered before processing.
  • Latency Comparison: Table 1 reports control latency for methods capable of collision avoidance in high-dimensional systems.The table is intended as a comparison of real-time performance.

B Further Experimental Details

The ball-balancing task uses perception and a simplified kinematic model to predict ball motion from the robot’s end-effector trajectory. The model assumes continuous ball–tray contact and omits friction and system identification.

  • Experimental Setup: The Franka Panda balances a ball on a tray, with 30-Hz RGBD perception tracking the ball’s position during 30-second episodes.Each episode begins with the ball at an arbitrary tray location and ends with human repositioning.
  • Ball Dynamics: The controller predicts ball states using a kinematic rolling-on-plane model driven by end-effector positions and orientations.The model includes gravity-induced acceleration and uses the tensorized arm model for batched predictions.
  • Ball Dynamics: The ball-dynamics model assumes the ball remains in contact with the tray and does not explicitly model friction or perform system identification.The contact assumption is considered reasonable at lower speeds.

B.2 Reaching Cartesian Poses

The study evaluates Cartesian pose reaching on six difficult poses against MOVEIT! and MMC. STORM reaches a challenging orientation that MMC cannot, but its accuracy on the remaining poses is lower than the baselines.

  • Evaluation Setup: The evaluation compares STORM with RRTCONNECT and RRTSTAR through MOVEIT!, and with Manipulability Motion Control, across six difficult Cartesian poses.Reported Table 2 values are medians across five poses, excluding the first pose that MMC could not reach.
  • Results: MMC repeatedly enters self-collision on a pose requiring a large end-effector orientation change, whereas STORM and MOVEIT! reach it.The authors attribute MMC’s failure to its local nature and lack of self-collision avoidance.
  • Results: STORM has worse accuracy than the baselines on the other poses, while path lengths and maximum joint velocities are comparable.The authors report millimeter-level accuracy and identify the untuned lower-level controller as a performance limitation.

B.3 SceneCollisionNet Training

The experiments use a pre-trained SceneCollisionNet model for scene-collision prediction. Its tabletop training domain matches the experimental setting.

  • Model: The experiments use a pre-trained SceneCollisionNet model supplied by the authors of the referenced prior work.No additional training procedure is described here.
  • Training Domain: SceneCollisionNet was trained for tabletop environments, which the authors consider appropriate for their setting.
  • Implementation: The scene-collision model is used as provided rather than retrained for a different environment.

C Ablation Studies

The ablations examine sampling density and cost terms in simulation, showing trade-offs among accuracy, smoothness, collision avoidance, joint-limit compliance, manipulability, and stopping behavior. Increasing particles improves convergence, while appropriately weighted costs enforce safety and motion-quality objectives.

  • Number of Sampled Particles: Increasing the number of sampled particles improves median position accuracy and tightens its confidence interval during convergence.The position-error analysis uses the final 50 timesteps of episodes with ten orientation-challenging targets.
  • Number of Sampled Particles: Quaternion-error confidence intervals remain within 5 for the tested particle counts.
  • Sampling and Smoothness: The Halton plus B-spline strategy produces low-jerk motion with 200 particles and ramps joint velocity while maintaining low jerk as particle count increases.
  • Constraint Costs: Using self-collision cost prevents self-collision in tested target poses, but the robot does not reach the goal pose.The experiment reports timesteps spent in self-collision for five targets that initially collide with the robot.
  • Constraint Costs: Joint-limit violations decrease steadily as the joint-limit-cost weight increases, reaching zero at weights of 500 and above.
  • Behavior Costs: Manipulability improves pose-reaching accuracy up to a threshold, whereas excessive weight interferes with position accuracy; stop cost enables smooth stopping but low weight causes goal oscillations.The manipulability term regularizes the robot away from singular configurations, while stop cost penalizes velocities incompatible with stopping within the horizon.

C.3.1 Pseudo-random vs Halton Sampling

Halton sampling improves action-space coverage and quaternion accuracy in low-particle regimes, while mixed trajectory smoothing balances pose accuracy with high joint velocities. Learned collision detection and tensorized GPU computation further reduce MPC latency.

  • Sampling accuracy: Less than 5% quaternion error was achieved with Halton sampling at both 100 and 500 particles, whereas pseudo-random sampling with comb filtering failed within that confidence interval at low particle counts.Halton sampling reduces undesired sample clustering by providing better action-space coverage.
  • Trajectory smoothing: A mixed strategy using 0.6 B-Spline and 0.4 comb-filtered trajectories matched comb-filtering accuracy while achieving high joint velocities.B-Splines provide smooth acceleration and velocity ramp-up, while comb filtering preserves pose accuracy but can restrict velocity.
  • Collision computation: The learned self-collision detector was over 40x faster than the forward-kinematics baseline and maintained 0.4–0.6ms latency for large query batches.The benchmark evaluates increasing batch sizes of queried configurations.
  • GPU computation: The tensorized GPU forward model was over 5x faster than the CPU baseline across horizons with 500 particles and maintained similar latency as particle count increased.The stable latency with more particles is attributed to the fully tensorized implementation.
Loading 2104.13542v2…