Hierarchical Task Model Predictive Control for Sequential Mobile Manipulation Tasks

Xintong Du; Siqi Zhou; Angela P. Schoellig

cs.ROMar 10, 2026

Hierarchical Task Model Predictive Control for Sequential Mobile Manipulation Tasks

Authors: Xintong Du, Siqi Zhou, Angela P. Schoellig

Organizations: Learning Systems and Robotics Lab (www.learnsyslab.org) at the Technical University of Munich, Germany, and the University of Toronto Institute for Aerospace Studies, Canada · University of Toronto Robotics Institute · Munich Institute of Robotics and Machine Intelligence (MIRMI) · Vector Institute for Artificial Intelligence

Abstract

Mobile manipulators are envisioned to serve more complex roles in people's everyday lives. With recent breakthroughs in large language models, task planners have become better at translating human verbal instructions into a sequence of tasks. However, there is still a need for a decision-making algorithm that can seamlessly interface with the high-level task planner to carry out the sequence of tasks efficiently. In this work, building on the idea of nonlinear lexicographic optimization, we propose a novel Hierarchical-Task Model Predictive Control framework that is able to complete sequential tasks with improved performance and reactivity by effectively leveraging the robot's redundancy. Compared to the state-of-the-art task-prioritized inverse kinematic control method, our approach has improved hierarchical trajectory tracking performance by 42% on average when facing task changes, robot singularity and reference variations. Compared to a typical single-task architecture, our proposed hierarchical task control architecture enables the robot to traverse a shorter path in task space and achieves an execution time 2.3 times faster when executing a sequence of delivery tasks. We demonstrated the results with real-world experiments on a 9 degrees of freedom mobile manipulator.

Figures & tables

Fig. 1: Our mobile manipulator executes a sequence of delivery tasks while leveraging redundancy for efficiency. Our robot needs to deliver to/pick up from three people following the order, Red , Blue , Yellow . The high-level goal is decomposed into a sequence of alternating base trajectories (strips) and end effector waypoints (dots). At time $t_{0}$ , the robot leverages its redundancy to the current EE task $\mathcal{T}_{4}$ (Blue dot) to perform the subsequent base task $\mathcal{T}_{5}$ (Yellow strip) so that it can reach its next EE waypoint $\mathcal{T}_{6}$ faster. In this test, our proposed HTMPC motion control architecture is 2.3 times faster than the typical single-task method. A complete video can be found at http://tiny.cc/htmpc .

Fig. 2: Proposed HTMPC planning and control architecture for sequential mobile manipulation tasks. Autonomy modules are represented as blocks. Arrows indicate the direction of information flow. Feedback loops from the robot are presented along with their desired close-loop frequency for reactive behaviours. External disturbances or changes coming from the robot or environment are specified as incoming arrows to each autonomy module affected.


$\displaystyle f^{*}_{l}(x)=\min_{x\in\mathcal{X}}\>$	$\displaystyle f_{l}(x)$	(2a)
$\displaystyle s.t.\>$	$\displaystyle f_{i}(x)=f^{*}_{i}(x),$	(2b)
	$\displaystyle\;\text{for }i=1,\cdots,l-1$	(2c)

Algorithm 1 Lexicographic Optimization

Fig. 3: Illustration of 25 random square wave tests plotted in end effector frame using polar coordinates. The hierarchical tasks are,

\mathcal{T}_{0}

minimizing constraints violation,

\mathcal{T}_{1}

holding its end effector in home position, and

\mathcal{T}_{2}

base tracking a square wave trajectory. The square wave trajectories have a duration of

16s

that peaks during

t=0\sim 8s

(Part 1) and bottoms during

t=8\sim 16s

(Part 2). Valley target is the same for all cases which is the robot’s home position. Peak targets are randomly selected outside the robot’s workspace. An example base trajectory executed by HTMPC is provided with arrows indicating the direction of motion. Its corresponding peak target is marked as a yellow star. Note that Fig. 7 is plotted in the same coordinate to show the correlation between the base tracking performance and the base targets’ position.

Fig. 4: Experimental results of the three competing methods for the example shown in Fig. 3 . Since the peak target is not reachable, all three methods converge to a non-zero base error in Part 1 when the robot arm becomes fully extended and singular. For Part 2, only the proposed methods HTMPC and HTMPC_WPT are converged to its valley target. HTIDKC did not converge because the parameters were optimized for Part 1.

Eq	Low $\delta$		High $\delta$
Eq	avg	std	avg	std
Baseline ( 11 )	3.24	0.56	6.25	2.73
Proposed ( 12 )	3.07	0.73	3.42	1.55

TABLE I: HTMPC EE Tracking Error with Different Lexicographic Optimality Constraints (mm)

Fig. 5: HTMPC normalized base tracking error with different lexicographic constraints. The red cross indicates that the base tracking has diverged and is removed from the plot. A desired steady state error bound of 5% is shaded in red. Formulation ( 12 ) outperforms ( 11 ) with shorter convergence time for both tolerance levels. Increasing the constraint tolerance improves base tracking performance at the cost of the EE task ( Table I ).

Fig. 6: Violin plot showing the hierarchical task results of the three competing methods for the 20 random square wave tests. Both distributions (shaded area) and individual data points (dots) are shown. Results are presented separately for Part 1 (light blue) and Part 2 (dark blue). Part 2 results for the singularity impacted cases ( Fig. 7 ) are highlighted (red) . Parameters are designed so that all methods have similar performance for

\mathcal{T}_{0}

\mathcal{T}_{1}

and Part 1 of

\mathcal{T}_{2}

. HTMPC achieves the best control performance in Part 2 of

\mathcal{T}_{2}

with a

42\%

improvement over the HTIDKC approach. Therefore, HTMPC achieves a lower cost of the hierarchical tracking problem in the sense of lexicographic order as per Definition III.2 .

Fig. 7: Radial heat map showing the spatial correlation between the normalized Part 2 base tracking error and peak targets. As in Fig. 3 , this plot is in polar coordinates where the angular component is the relative direction of base peak target to the end effector home position. Circled in red is the region where singularity plays a major role in tracking performance. HTMPC outperforms the other two methods in dealing with singularity.

Fig. 8: Mobile manipulator base path comparison of a typical single-task (ST-Arch) and our proposed hierarchical-task (HT-Arch) architectures for a sequence of two EE waypoint tasks. The EE waypoint

\#1

were set at different heights. The intermediate base waypoint for the ST-Arch is determined following [ 10 ] . Compared to ST-Arch, the proposed HT-Arch approach ( HT-Arch h=0.8m) optimally positions the robot base where the EE waypoint

\#1

just becomes reachable.

Fig. 9: Robot performing a sequence of delivery tasks to three people. Top figure shows our robot running an arm-base decoupled single-task control architecture. Bottom figure shows robot running a our proposed HTMPC control architecture. Our proposed architecture is more efficient in executing sequential mobile manipulation tasks by allowing the base to move its next delivery target while holding EE in place.

Hierarchical Robot Control

See all 17 papers

Perceptive Hierarchical-Task MPC for Sequential Mobile Manipulation in Unstructured Semi-Static Environments
Mar 10, 2026Xintong Du, Jingxing Qian, Siqi Zhou +1Mobile Manipulation Robot Task Planning
University of Toronto Institute for Aerospace Studies (UTIAS) and the Vector Institute for Artificial Intelligence, Canada. · School of Computing Science, Faculty of Applied Sciences, Simon Fraser University, Burnaby, BC, Canada. · Learning Systems and Robotics lab at the Technical University of Munich and the Munich Institute for Robotics and Machine Intelligence (MIRMI), Germany.
Hierarchical Planning with Latent World Models
Apr 3, 2026Wancong Zhang, Basile Terver, Artem Zholus +7Hierarchical Planning Hierarchical Robot Control
FAIR at Meta · New York University · Mila - Québec AI Institute +1
Closed-Loop Object-Informed Control for Non-Prehensile Robot Manipulation
May 29, 2026Nikola Raicevic, Hyomuk Kim, Shahid Mulla +5Non-Prehensile Robotic Manipulation Long-Horizon Robotic Manipulation
Department of Electrical and Computer Engineering, University of California San Diego, 9500 Gilman Drive, La Jolla, CA 92093, United States

Mar 10, 2026J/K move · Enter open · S save

Perceptive Hierarchical-Task MPC for Sequential Mobile Manipulation in Unstructured Semi-Static Environments

Xintong Du, Jingxing Qian, Siqi Zhou, Angela P. Schoellig

University of Toronto Institute for Aerospace Studies (UTIAS) and the Vector Institute for Artificial Intelligence, Canada. · School of Computing Science, Faculty of Applied Sciences, Simon Fraser University, Burnaby, BC, Canada. · Learning Systems and Robotics lab at the Technical University of Munich and the Munich Institute for Robotics and Machine Intelligence (MIRMI), Germany.

Mobile Manipulation Robot Task Planning Long-Horizon Robotic Manipulation Dynamic Robotic Manipulation Model Predictive Control

Open paper AskPDF arXiv

As compared to typical mobile manipulation tasks, sequential mobile manipulation poses a unique challenge -- as the robot operates over extended periods, successful task completion is not solely dependent on consistent motion generation but also on the robot's awareness and adaptivity to changes in the operating environment. While existing motion planners can generate whole-body trajectories to complete sequential tasks, they typically assume that the environment remains static and rely on precomputed maps. This assumption often breaks down during long-term operations, where semi-static changes such as object removal, introduction, or shifts are common. In this work, we propose a novel perceptive hierarchical-task model predictive control (HTMPC) framework for efficient sequential mobile manipulation in unstructured, changing environments. To tackle the challenge, we leverage a Bayesian inference framework to explicitly model object-level changes and thereby maintain a temporally accurate representation of the 3D environment; this up-to-date representation is embedded in a lexicographic optimization framework to enable efficient execution of sequential tasks. We validate our perceptive HTMPC approach through both simulated and real-robot experiments. In contrast to baseline methods, our approach systematically accounts for moved and phantom obstacles, successfully completing sequential tasks with higher efficiency and reactivity, without relying on prior maps or external infrastructure.

Abstract

Hierarchical Robot Control

Explore similar work

Perceptive Hierarchical-Task MPC for Sequential Mobile Manipulation in Unstructured Semi-Static Environments

Hierarchical Planning with Latent World Models

Closed-Loop Object-Informed Control for Non-Prehensile Robot Manipulation