NMPC-based Motion Planning with Adaptive Weighting for Dynamic Object Interception

TL;DR

The paper addresses dynamic interception of fast-moving objects using two cooperative robotic arms under closed-chain constraints. It introduces an Adaptive-Terminal NMPC (AT-MPC) with cost shaping to adapt terminal weights during optimization, contrasted with a Primitive-Terminal (PT) baseline. Experimental validation on two 7-DoF Franka arms demonstrates real-time feasibility: planner at 25 Hz with an average cycle time of $19.21\,\text{ms}$ under a sampling period of $T_s=40\,\text{ms}$, intercepting 13 of 35 throws (37.1%) and 3 abortions due to actuator power limits. AT yields smoother trajectories and lower control effort than PT, mitigating power-limit violations with negligible computational overhead, supporting robust, dynamic cooperative interception in practice; future work will explore learning-augmented policies to further improve performance.

Abstract

Catching fast-moving objects serves as a benchmark for robotic agility, posing significant coordination challenges for cooperative manipulator systems holding a catcher, particularly due to inherent closed-chain constraints. This paper presents a nonlinear model predictive control (MPC)-based motion planner that bridges high-level interception planning with real-time joint space control, enabling dynamic object interception for systems comprising two cooperating arms. We introduce an Adaptive- Terminal (AT) MPC formulation featuring cost shaping, which contrasts with a simpler Primitive-Terminal (PT) approach relying heavily on terminal penalties for rapid convergence. The proposed AT formulation is shown to effectively mitigate issues related to actuator power limit violations frequently encountered with the PT strategy, yielding trajectories and significantly reduced control effort. Experimental results on a robotic platform with two cooperative arms, demonstrating excellent real time performance, with an average planner cycle computation time of approximately 19 ms-less than half the 40 ms system sampling time. These results indicate that the AT formulation achieves significantly improved motion quality and robustness with minimal computational overhead compared to the PT baseline, making it well-suited for dynamic, cooperative interception tasks.

Figures (8)

• Figure 4: The experimental setup, showcasing the dual Franka Emika Panda manipulators, the cooperative catching container, and the perception system.
• Figure 5: Visualization of a successful ball catch using the AT-MPC framework. 'Measured Ball Pos' (blue markers) are raw vision system detections, filtered into the 'KF Estimate' trajectory (orange line). The Kalman filter generates future path predictions: an early prediction potentially outside the workspace ('KF Prediction (Early/Out)', red line) and a later 'KF Prediction (Valid)' (green dashed line) intersecting the 'Safe Zone' (black cube). This valid prediction determines the 'Target Pose' (red circle) for the container center. The 'Container Trajectory' (bold blue line) shows the executed path of the container center, starting from 'Container Start Pose' (black circle), successfully intercepting the ball.
• Figure 6: Close Up view
• Figure 7: Real-time performance: End-to-end computation times per AT-MPC cycle during a real-world experiment (cf. Fig. \ref{['fig:Fig_3D_Catch']}). Includes full node processing from state input to reference output for the MPC node. Average ($19.21\,\text{ms}$) and worst-case times remained well below $T_s=40\,\text{ms}$.
• Figure 8: Average control effort ($E = \sum \|\ddot{\bm{q}}(k)\|_2^2$) for AT vs. PT modes across 500 trials. The AT mode demonstrates significantly reduced effort for both manipulators.