Abstract
Ensuring safe physical interaction between torque-controlled manipulators and humans is essential for deploying robots in everyday environments. Model Predictive Control (MPC) has emerged as a suitable framework thanks to its capacity to handle hard constraints, provide strong guarantees and zero-shot adaptability through predictive reasoning. However, Gradient-Based MPC (GB-MPC) solvers have demonstrated limited performance for collision avoidance in complex environments. Sampling-based approaches such as Model Predictive Path Integral (MPPI) control offer an alternative via stochastic rollouts, but enforcing safety via additive penalties is inherently fragile, as it provides no formal constraint satisfaction guarantees. We propose a collision avoidance framework called COSMIK-MPPI combining MPPI with the the toolbox for human motion estimation RT-COSMIK and the Constraints-as-Terminations transcription, which enforces safety by treating constraint violations as terminal events, without relying on large penalty terms or explicit human motion prediction. The proposed approach is evaluated against state-of-the-art GB-MPC and vanilla MPPI in simulation and on a real manipulator arm. Results show that COSMIK-MPPI achieves a 100\% task success rate with a constant computation time (22 ms) largely outperforming GB-MPC. In simulated infeasible scenarios, COSMIK-MPPI consistently generates collision-free trajectories, contrary to vanilla MPPI. These properties enabled safe execution of complex real-world human–robot interaction tasks in shared workspaces using an affordable markerless human motion estimator, demonstrating a robust, compliant and practical solution for predictive collision avoidance.
Method
Figure 1: Visualization of the NLF-based human mesh recovery (left) and human collision capsules (rendered as cylinders only visually) and output of the inverse kinematics pipeline used to position collision primitives (right).
Results
The simulation benchmark consists of six scenarios of increasing geometric complexity, all based on the same pick-and-place task. Scenarios 1–3 range from an obstacle-free workspace to a single elongated capsule, testing baseline behavior and simple avoidance. Scenarios 4 and 5 introduce non-convex multi-obstacle configurations that stress the planner’s ability to navigate complex, highly constrained environments in real time. Scenario 6 is intentionally infeasible, a blocking obstacle prevents any direct path to the target, and serves as a stress test for safe behavior under unsolvable conditions.
Figure 2: The six simulation scenarios, from free workspace (Sc. 1) to fully blocked path (Sc. 6).
Simulation Comparison
| Scenario | Method | Success Rate | Dist. to Target (cm) | Dist. to Obstacles (cm) | Smoothness | Comp. Time (ms) |
|---|---|---|---|---|---|---|
| Sc. 1 Baseline |
GB-MPC | 1.00 | 1.05 | — | 1.50 | 15 |
| Vanilla MPPI | 1.00 | 1.88 | — | 18.59 | 22 | |
| COSMIK-MPPI | 1.00 | 1.98 | — | 20.14 | 22 | |
| Sc. 2 Single capsule |
GB-MPC | 1.00 | 0.51 | 8.78 | 2.31 | 89 |
| Vanilla MPPI | 1.00 | 1.79 | 9.32 | 20.83 | 22 | |
| COSMIK-MPPI | 1.00 | 1.62 | 10.65 | 17.54 | 22 | |
| Sc. 3 Elongated capsule |
GB-MPC | 1.00 | 0.26 | 4.69 | 2.18 | 81 |
| Vanilla MPPI | 1.00 | 1.95 | 4.09 | 24.65 | 22 | |
| COSMIK-MPPI | 1.00 | 1.83 | 5.35 | 19.97 | 22 | |
| Sc. 4 Non-convex (4 primitives) |
GB-MPC | 0.00 | 54.21 | 23.10 | 0.00 | 415 |
| Vanilla MPPI | 1.00 | 1.91 | 3.57 | 24.57 | 22 | |
| COSMIK-MPPI | 1.00 | 1.95 | 1.62 | 20.72 | 22 | |
| Sc. 5 Dense circular arrangement |
GB-MPC | 0.00 | 73.97 | 14.87 | 0.00 | 1812 |
| Vanilla MPPI | 1.00 | 1.86 | 4.35 | 17.82 | 22 | |
| COSMIK-MPPI | 1.00 | 1.76 | 5.87 | 18.75 | 22 | |
| Sc. 6 Infeasible (blocked path) |
GB-MPC | 0.00 | 11.29 | 4.75 | 11.75 | 80 |
| Vanilla MPPI | 0.00 | 14.65 | -0.91 | 19.37 | 21 | |
| COSMIK-MPPI | 0.00 | 11.99 | 5.28 | 69.57 | 21 |
All three methods succeed in the simpler scenarios (1–3), but their computational profiles diverge sharply. MPC’s solve time jumps 5× from 15 ms to over 80 ms as soon as collision constraints appear, while both MPPI variants hold a flat 22 ms across every scenario. The advantage of sampling-based methods becomes decisive in Scenarios 4 and 5: MPC spends 415 ms and 1812 ms respectively, far beyond real-time, and fails to complete either task. COSMIK-MPPI and vanilla MPPI both succeed with identical 22 ms compute time, confirming that rollout-based planning scales gracefully with constraint complexity where gradient-based solvers do not.
Comparing the two MPPI variants, COSMIK-MPPI consistently maintains larger clearance from obstacles than vanilla MPPI in Scenarios 2, 3, and 5, without sacrificing goal accuracy. The clearest difference appears in Scenario 6: vanilla MPPI records a negative obstacle distance (−0.91 cm), meaning it penetrates the obstacle, while COSMIK-MPPI holds a positive 5.28 cm clearance throughout. This illustrates the core benefit of treating constraint violations as terminations rather than soft penalties, unsafe rollouts lose influence under the sampling distribution instead of simply incurring a higher cost.
Experiments on a real robot.
The experiments were done on a Franka Emika Panda manipulator.