💻 computer science

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

This paper proposes ToMPC, a task-oriented model predictive control framework that utilizes ADMM to decompose complex optimization into DDP and QP subproblems, enabling a Franka Panda robot to plan safe, efficient, and redundant motion and force trajectories in real-time for manipulation in obstructed open workspaces.

Original authors: Xinyu Jia, Wenxin Wang, Jun Yang, Yongping Pan, Haoyong Yu

Published 2026-03-17

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Xinyu Jia, Wenxin Wang, Jun Yang, Yongping Pan, Haoyong Yu

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are a waiter carrying a tray of full coffee cups through a crowded, chaotic party. Your goal is twofold: you must deliver the coffee to a specific table (the task), but you also must not bump into the guests, the furniture, or the walls (the safety).

If you just walk straight, you might spill the coffee. If you just try to dodge every person, you might never reach the table. This is the exact problem robots face when they try to move in real-world environments.

This paper introduces a new "brain" for robots called ToMPC (Task-Oriented Model Predictive Control). Here is how it works, explained through simple analogies:

1. The Problem: The "Freeze" or the "Crash"

Traditional robot planners are like a nervous driver.

The "Freeze": If the robot sees an obstacle, it might stop completely or move very slowly to be safe, ruining its efficiency.
The "Crash": If the robot focuses only on the goal, it might ignore obstacles and crash.
The "Rigid" Approach: Some robots treat safety as a hard wall. If they get too close, they stop. This often means they can't reach their goal because they are too scared to get close enough.

2. The Solution: The "Smart Waiter" (ToMPC)

The authors created a system that acts like a highly skilled, agile waiter. Instead of just avoiding people, the waiter uses their body to dodge.

Task-Oriented Avoidance: If a guest is blocking the path to the table, a normal robot might stop. This "Smart Waiter" twists its body, moves its elbow, or shifts its hips to squeeze past the guest while still keeping the tray steady. It uses its extra flexibility (called "redundancy") to solve two problems at once: dodging the guest and reaching the table.

3. The Engine: The "Two-Brain" System (ADMM)

Doing all this math in real-time is incredibly hard. It's like trying to solve a complex Sudoku puzzle while juggling. To make it fast, the authors split the problem into two specialized "brains" that talk to each other using a method called ADMM (Alternating Direction Method of Multipliers).

Think of it like a Chef and a Safety Inspector working in a kitchen:

Brain A (The Chef / DDP): This brain is an expert at cooking the perfect meal (planning the smoothest, most efficient path). It says, "I want to move the arm this way to get the coffee there fast." It ignores the safety rules for a moment to find the best path.
Brain B (The Safety Inspector / QP): This brain is an expert at rules. It looks at the Chef's plan and says, "Whoa! You're about to hit the wall! You need to move your elbow that way." It fixes the plan to ensure no collisions happen.
The Conversation (ADMM): They don't argue; they iterate quickly.
1. Chef proposes a path.
2. Inspector tweaks it to be safe.
3. Chef tweaks it again to be efficient.
4. They repeat this thousands of times per second until they agree on a plan that is both safe and efficient.

4. The "Hard" vs. "Soft" Rules

The system distinguishes between two types of rules:

Hard Constraints (The Wall): You cannot touch the wall. If you do, the robot crashes. The "Safety Inspector" brain makes sure this never happens.
Soft Constraints (The Crowd): You don't have to avoid the crowd perfectly if it means you can't deliver the coffee. The system gently nudges the robot away from people, but if it gets too close, it prioritizes the coffee delivery, using the robot's body to twist around the person rather than stopping.

5. Real-World Results

The team tested this on a Franka Panda robot (a 7-jointed arm that looks like a human arm).

The Test: They made the robot move a block, wipe a whiteboard, and push against a wall, all while avoiding a ball obstacle.
The Result: Compared to older methods, this new system didn't just avoid the ball; it twisted its whole body to get closer to the target. It moved faster, was smoother, and never crashed, even when the obstacle was right in its face.

Summary

In short, this paper gives robots a new way to think. Instead of seeing obstacles as stop-signs, they see them as puzzles to be solved by moving their bodies creatively. By splitting the complex math into two cooperating parts, the robot can make these decisions in real-time, making it safe enough for a factory floor but smart enough to work in a busy human environment.

1. Problem Statement

Robotic manipulators operating in open workspaces face a dual challenge: they must perform complex tasks (such as pick-and-place or force-controlled interaction) while strictly adhering to safety constraints to avoid collisions with obstacles.

The Conflict: Traditional Model Predictive Control (MPC) methods often struggle to simultaneously optimize for task execution (motion/force tracking) and safety (collision avoidance).
- Methods relying on soft constraints (e.g., barrier functions) may violate safety limits if the penalty weights are not tuned perfectly.
- Methods using hard constraints often suffer from high computational complexity, making real-time implementation difficult, especially when coupling full-body dynamics with obstacle avoidance.
- Existing approaches often treat obstacle avoidance as a secondary constraint that degrades task performance (e.g., reducing the manipulator's reachable workspace or causing tracking errors).
The Goal: Develop a unified control framework that plans collision-free motion and compliant interaction forces in real-time, leveraging kinematic redundancy to maintain task efficiency even in obstructed environments.

2. Methodology

The authors propose ToMPC (Task-oriented Model Predictive Control), a framework that unifies motion and force control with safety constraints, solved via the Alternating Direction Method of Multipliers (ADMM).

A. System Modeling

Dynamics: The robot is modeled using full rigid-body dynamics ( $M\ddot{q} + C\dot{q} + g = \tau + \tau_{ext}$ ).
Interaction: An impedance-based interaction model is integrated directly into the dynamics. External wrenches ( $F$ ) are treated as states rather than disturbances, allowing the controller to plan compliant contact forces without needing external force sensors for feedback.
Kinematics: The system utilizes kinematic redundancy ( $n > m$ ) to perform secondary tasks (like obstacle avoidance) in the null space of the primary task.

B. Optimization Formulation

The MPC problem is formulated to minimize a cost function comprising:

Motion/Force Tracking: Minimizing error between the end-effector pose/force and reference trajectories.
Task-Oriented Avoidance: A novel soft constraint that generates a virtual repulsive velocity when links enter a "repulsive region" around obstacles. Crucially, this is projected into the null space of the primary task ( $N_{task}$ ), ensuring that avoidance maneuvers do not interfere with the main task execution unless absolutely necessary.
Control Regularization: Penalizing deviations from gravity-compensating torques.

Constraints:

Hard Constraints: Minimum distance to obstacles ( $d \ge d_{th1}$ ), joint limits, and velocity/torque limits.
Soft Constraints: Task-oriented repulsive motion and goal relaxation (reducing task priority as the robot gets closer to obstacles).

C. Distributed Optimization via ADMM

To solve the complex, non-linear, constrained optimization problem in real-time, the authors decompose the problem using ADMM:

Decomposition: The problem is split into two subproblems linked by an equality constraint ($Gy - z = 0$).
- Subproblem 1 (Task Execution): Solves for the trajectory considering dynamics and task costs. This is a non-linear problem solved efficiently using Differential Dynamic Programming (DDP). It handles equality constraints (dynamics) but ignores hard inequality constraints.
- Subproblem 2 (Safety Guarantee): Solves for state constraints (obstacle distances, joint limits). This is formulated as a Quadratic Programming (QP) problem, which is convex and fast to solve.
Iteration: The ADMM algorithm alternates between the DDP and QP solvers, updating Lagrange multipliers to ensure consistency between the two subproblems. This allows the system to enforce hard safety constraints strictly while maintaining the computational efficiency of DDP for dynamics.

3. Key Contributions

Unified MPC Framework: A single framework that simultaneously plans collision-free motion and compliant force trajectories in shared workspaces, utilizing full-body dynamics without relying on external force sensors for control.
Task-Oriented Avoidance Strategy: A novel approach that leverages kinematic redundancy to proactively move robot links away from obstacles. By projecting avoidance motions into the task's null space, it maximizes the end-effector's manipulation range and minimizes tracking errors compared to standard avoidance methods.
Real-Time ADMM Implementation: The successful application of ADMM to decouple non-linear dynamics (solved by DDP) from hard inequality constraints (solved by QP). This enables real-time planning while strictly satisfying safety constraints, overcoming the limitations of pure DDP (which cannot handle hard constraints) and pure QP (which struggles with non-linear dynamics).

4. Results and Validation

The approach was validated on a Franka Emika Panda robot (7-DOF) through simulations and hardware experiments.

Scenario 1 (Obstacle Avoidance):
- Compared against a standard FDDP solver using barrier functions.
- Result: The proposed ADMM-based ToMPC strictly maintained the minimum safety distance ( $d_{th1}$ ), whereas the FDDP baseline violated safety constraints. ToMPC also achieved significantly lower computation times (e.g., ~0.9ms vs ~9ms for similar horizons).
Scenario 2 (Task-Oriented Avoidance):
- The robot traced a complex path (Lemniscate of Bernoulli) near a ball obstacle.
- Result: The ToMPC utilized whole-body redundancy to twist the robot's links away from the obstacle, maintaining high task accuracy. The baseline (standard avoidance) exhibited large Cartesian tracking errors to maintain safety.
Scenario 3 (Environment Interaction):
- The robot performed force control against a rigid surface.
- Result: ToMPC produced smooth force trajectories with minimal noise, outperforming an instantaneous inverse dynamics controller which exhibited oscillations.
Scenario 4 (Hybrid Motion-Force):
- The robot wiped a whiteboard with a constant contact force while avoiding an obstacle.
- Result: ToMPC successfully cleaned two marker points by expanding its reachable workspace via proactive link avoidance. The baseline only reached one point. Quantitative analysis showed ToMPC had significantly lower position error (0.0201m vs 0.0303m) with a negligible trade-off in force accuracy.

5. Significance

This work addresses a critical gap in robotic manipulation: the trade-off between safety and efficiency.

Safety: It guarantees hard constraint satisfaction (no collisions) through the QP subproblem, which is often lacking in non-linear MPC approaches.
Efficiency: By using "task-oriented" avoidance, it prevents the robot from becoming overly conservative, allowing it to utilize its full kinematic redundancy to complete tasks even in cluttered environments.
Scalability: The ADMM decomposition makes it feasible to run full-dynamics MPC with hard constraints on standard hardware (Intel i9 CPU) at 1kHz, paving the way for safer, more autonomous robots in dynamic human environments.

In conclusion, ToMPC represents a significant step forward in making robotic manipulation safer and more capable in unstructured, obstacle-rich environments without sacrificing task performance.