ToMPC: Task-oriented Model Predictive Control via ADMM for Safe Robotic Manipulation

Robots operating in human-centric environments must be both robust to disturbances and provably safe from collisions. Achieving these properties simultaneously and efficiently remains a central challenge. While Dynamic Movement Primitives (DMPs) offer inherent stability and generalization from single demonstrations, they lack formal safety guarantees. Conversely, formal methods like Control Barrier Functions (CBFs) provide provable safety but often rely on computationally expensive, real-time optimization, hindering their use in high-frequency control. This paper introduces SafeDMPs, a novel framework that resolves this trade-off. We integrate the closed-form efficiency and dynamic robustness of DMPs with a provably safe, non-optimization-based control law derived from Spatio-Temporal Tubes (STTs). This synergy allows us to generate motions that are not only robust to perturbations and adaptable to new goals, but also guaranteed to avoid static and dynamic obstacles. Our approach achieves a closed-form solution for a problem that traditionally requires online optimization. Experimental results on a 7-DOF robot manipulator demonstrate that SafeDMPs is orders of magnitude faster and more accurate than optimization-based baselines, making it an ideal solution for real-time, safe, and collaborative robotics.

Robotic manipulators operating in unstructured environments face significant challenges in safely executing goal-directed tasks due to dynamic and unforeseen obstacles, while traditional methods rely on prior knowledge or fixed perception pipelines, limiting adaptability. We propose a framework for safe task execution with effective obstacle avoidance. The environment module performs real-time obstacle detection, 3D localization, and ground surface geometry estimation. It then generates a structured semantic report that includes obstacle positions, object geometry and shape, and whether obstacles lie inside, outside, or within critical interaction zones. A central coordination module manages the overall system by handling tool invocation (e.g., memory and MoveIt collision scene updates), facilitating communication between modules, and continuously monitoring task progress until completion. Furthermore, a planning module selects an appropriate motion planning algorithm, such as RRTConnect, RRT*, or BiTRRT, based on the current environment configuration and goal requirements. The trajectory generated by the planner is further analyzed and refined to ensure safe and collision-free task execution. The proposed approach is evaluated in Gazebo Classic , demonstrating robustness in dynamic scenarios.

Model Predictive Control (MPC) is widely used for torque-controlled robots, but classical formulations often neglect real-time force feedback and struggle with contact-rich industrial tasks under collision constraints. Deburring in particular requires precise tool insertion, stable force regulation, and collision-free circular motions in challenging configurations, which exceeds the capability of standard MPC pipelines. We propose a framework that integrates force-feedback MPC with diffusion-based motion priors to address these challenges. The diffusion model serves as a memory of motion strategies, providing robust initialization and adaptation across multiple task instances, while MPC ensures safe execution with explicit force tracking, torque feasibility, and collision avoidance. We validate our approach on a torque-controlled manipulator performing industrial deburring tasks. Experiments demonstrate reliable tool insertion, accurate normal force tracking, and circular deburring motions even in hard-to-reach configurations and under obstacle constraints. To our knowledge, this is the first integration of diffusion motion priors with force-feedback MPC for collision-aware, contact-rich industrial tasks.