WorksComputer Science
Cooperative Mobile Manipulation using Learned Dynamical Systems
Abstract
Cooperative mobile manipulation requires robots to coordinate their motions while ensuring whole-body collision avoidance and preserving task-relevant behaviors. We propose CALDS (Cooperative Adaptation of Learned Dynamical Systems), a framework that enables mobile manipulators to adapt learned single-agent behaviors for coordinated execution. Building on individual dynamical systems (DSs) learned from demonstrations, CALDS enables cooperative adaptation through two key components: Cooperative Policy Selection, which identifies compatible combinations of learned policies, and Temporal Adaptation, which ensures collision-free cooperation while preserving the spatial structure and convergence properties of the original behaviors. We evaluate CALDS across three diverse scenarios: assistive dressing, restaurant cleaning and serving, and drawer opening and object placement. Quantitative results demonstrate its effectiveness in improving task success and enabling safe, coordinated simultaneous execution while maintaining fidelity to demonstrated behaviors.
Introduction
Mobile manipulators are versatile platforms capable of assisting humans in household tasks ((Wu et al., 2023)), and performing labor-intensive jobs, such as operating in greenhouses ([23]) or construction sites ([11]). In these domestic and professional environments, efficiency rises when robots collaborate with other agents, anticipating on their actions and intentions. For instance, consider a robot assisting a human in putting on a jacket while another shows how the outfit looks using a mirror, or a restaurant where one robot places a glass on the table-area cleaned by another robot. Such scenarios require robots to proactively adapt their behaviors to other agents, ensuring both task effectiveness and safety.
Learning from Demonstration (LfD) has emerged as a powerful framework for transferring human skills to robots, with successful applications in manipulators ([20, 16]), dual-arm systems ((Zhao et al., 2023)), and mobile manipulators ([10, 8] (Sundaresan et al. 2025)). However, extending these skills to collaborative behaviors involving multiple mobile manipulators presents several challenges. Firstly, collecting multi-agent demonstrations is inherently complex, making even a small number of demonstrations difficult to obtain. Secondly, mobile manipulators operate in cluttered and dynamic environments, requiring whole-body collision avoidance with other robots and surrounding obstacles. Existing approaches often delegate collision avoidance to low-level planners ((Sundaresan et al., 2025)), whose deviations from demonstrated motions can compromise task execution and push learned policies into off-distribution states, resulting in potentially unreliable or unintuitive actions. These challenges motivate frameworks that enable collaborative mobile manipulation from small amounts of demonstrations, while accounting for whole-body constraints and anticipating the actions of other agents.
Rather than depending on extensive multi-agent demonstrations, recent approaches have explored composing multi-agent behavior from single-agent demonstrations. Singh et al. [24] define Optimized Normalized Collaborative DMPs (ONCol-DMPs) to generate collaborative trajectories leveraging a time-heuristic and end-effector spatial collision avoidance to modulate learned skills. Furthermore, single-agent demonstrations can be combined with multi-agent cost functions to train coordinated dual-manipulator diffusion policies [18]. Extending this approach to mobile manipulators, however, requires reliable visual perception for both diffusion-based policy generation and whole-body collision avoidance. Other approaches learn collaborative behaviors from multi-manipulator demonstrations, while focusing on maintaining global consistency during decentralized execution [12, 7]. Learning-free task and motion planning (TAMP) methods, such as those using temporal logic [22] or long-horizon planning frameworks [11], also offer solutions for collaborative tasks incorporating whole-body collision avoidance. However, their computational demands can limit real-time applicability. Moreover, collision-free collaboration does not necessarily imply efficient execution: sequential motion or waiting for other robots to pass can cause unnecessary delays, while concurrent execution requires explicit coordination to avoid collisions [21].
To address these challenges, we propose CALDS (Cooperative Adaptation of Learned Dynamical Systems), a framework that enables mobile manipulators to coordinate learned single-agent behaviors in shared workspaces while ensuring whole-body collision avoidance and task constraints (Fig. 1). Each robot is equipped with a set of learned dynamical systems (DSs), which encode diverse strategies for accomplishing the assigned task. These learned DSs, such as proposed in [16, 1] (Urain et al. 2022), can be generated from only a few (e.g. 1–10) demonstrated end-effector pose trajectories in , while guaranteeing convergence to the desired goal. CALDS comprises two centralized stages: Cooperative Policy Selection, which identifies compatible combinations of single-agent policies, and Temporal Adaptation, which adjusts their temporal profiles to achieve collision-free collaboration while preserving the spatial structure, interpretability, and convergence of the original learned DSs. Both stages generate trajectories in which the adapted learned DSs drive the motion at every step, leveraging JAX ([5, 4]) and FRAX ([15]) kinematics for batched, differentiable computation. We validate the proposed framework through quantitative experiments in three collaborative scenarios. We compare CALDS against existing collaborative policy adaptation ([24]) whole-body collision avoidance via control barrier functions (CBFs) ((Singletary et al., 2021)) and nominal single-agent policies, demonstrating its effectiveness in enabling safe and task-consistent operation.
Preliminaries on Learned Dynamical Systems
DSs provide an effective framework for encoding motion behaviors either from human demonstrations ([2]) or through manual design by combining multiple sub-behaviors ([19]). A motion is represented as an autonomous continuous-time DS of the form,
where denotes the system state and is the equilibrium point corresponding to the task goal, such that , with denoting the zero vector of appropriate dimension. The learned behavior is typically defined in the end-effector state space, which enables adaptation to changes in goal pose and the robot’s embodiment. In this work, we represent the end-effector pose as , where denotes the Cartesian position and denotes the orientation represented as a quaternion. The output of the DS, , therefore corresponds to the end-effector twist.
Stability of learned DSs can be enforced in several ways. In (Urain et al., 2022), stability is achieved through a diffeomorphic mapping between a simple stable system and task-space dynamics, extending the formulation to Lie groups. Other approaches enforce stability through the training objective of Deep Neural Networks (DNNs) ([16]), or guarantee convergence toward a geometric curve learned from demonstrations ([1]). More recently, ([17]) incorporated Lyapunov stability constraints into flow-matching-based DSs, including Lie group formulations.
Problem formulation
Consider a multi-robot system consisting of mobile manipulators. Each robot , using shorthand notation , is assigned a task , which can be executed using one of distinct learned DSs.
We refer to these learned DSs as task strategies, each encoding a single-agent strategy for the assigned task without considering interactions with other agents.
Our objective is to select an appropriate combination of task strategies and adapt their execution to enable safe and effective collaboration, while minimizing deviations from the original learned behaviors. Specifically, we formulate the following optimization problem:
Here, denotes the adapted velocity action, an appropriate distance metric, a task-strategy dependent cost, and (4) the robot dynamics. Constraint (5) enforces whole-body collision avoidance between the robots throughout the horizon, where denotes the volume occupied by robot at configuration . Constraint (6) accounts for temporal dependencies between tasks, where denotes the set of robot pairs subject to ordering constraints, and represents the completion time of task . Finally, constraint (7) ensures that the adapted execution retains the convergence properties of the selected learned DSs, such that each robot ultimately reaches its corresponding goal state.
Cooperate Adaption for Learned Dynamical Systems
In this section, we introduce CALDS (Figure 1). First, we learn distinct DSs from a small set of trajectory demonstrations to obtain single-agent policies. To enable cooperative multi-agent adaptation, CALDS comprises two layers: Cooperative Policy Selection (§ 4.2) and Temporal Adaptation (§ 4.3). Both layers rely on generating robot trajectories along a horizon, as described in § 4.1.

Figure 1. Overview of CALDS for cooperative multi-agent adaptation in a restaurant scenario, where one robot cleans a table while the other places a cup on the cleaned area. Firstly, we generate a set of learned single-agent DSs for each task. In the multi-agent context, Cooperative Policy Selection ranks and selects the most promising combination, which Temporal Adaptation then adjusts to satisfy task constraints while ensuring whole-body collision-free trajectories, selecting the next-best combination if unsuccessful.
Trajectory Generation
Both the Cooperative Policy Selection and Temporal Adaptation require a trajectory and trajectory cost that simulates and evaluates the future actions of the robots. We therefore forward-simulate whole-body robot motion over a finite horizon of duration , discretized into equally spaced intervals of length , with time points indexed by .
As the learned DSs generate end-effector velocity commands and take the current end-effector pose as input, we obtain the latter through forward kinematics (FK) from the joint configuration of robot , . We introduce a velocity scaling factor for each robot and . These factors are used to generate candidate trajectories for Cooperative Policy Selection (§ 4.2) and serve as the optimization variables in Temporal Adaptation (§ 4.3). The scaled learned DSs are therefore given by,
The resulting end-effector velocity commands are mapped to joint velocities through the low level controller (LLC). In this work, we employ differential null-space inverse kinematics (Buss et al., [6]),
with Jacobian , damping matrix , null-space gain and homing configuration for the robot-arm joints only. The resulting joint velocities are then integrated to obtain the next joint configuration. The discrete-time system can thus be expressed compactly as
where represents the combined effect of the FK, the selected learned DS, the LLC, and the discrete-time integration, with current robot configuration . The result is a simulated trajectory along the discrete horizon, , .
Modulation. Although static obstacle avoidance may be reflected in the demonstrations, it is not explicitly guaranteed by the learned DSs, which may generate trajectories intersecting static objects such as tables or cabinets. While our primary focus is on inter-robot collision avoidance, we additionally modulate each learned DS in (8) via , with modulation matrix [13]. The modulation ellipse is fitted within the obstacle footprint while maintaining clearance from the demonstrated trajectory, enabling local deformation around the obstacle.
Cooperative Policy Selection
Cooperative Policy Selection evaluates and ranks candidate combinations of task strategies to identify policies suitable for collaborative execution, which are passed to the Temporal Adaptation planner. For each combination of learned single-agent DSs, we perform forward simulations using (10) with parallel, predefined velocity-scaling configurations . These include the nominal configuration and additional configurations with varying velocity-scaling factors along the trajectory. Among these forward simulations, we retain the one with the lowest cost for each combination . First, combinations whose demonstrated target poses collide are discarded by assigning a high static cost . The remaining combinations are ranked based on their demonstration closeness cost and trajectory cost ,
with weights and . The cost averages the distances from each robot’s initial pose to its closest point on the demonstration, . The trajectory cost penalizes deviations from the learned DSs velocity commands, as well as violations of collision-avoidance, task order, and speed constraints, as detailed in § 4.3.
Once the ranking of the combinations is established, the combination with the lowest cost is send to the Temporal Adaptation layer, which receives a computational budget for resolving the trajectories of all robots to meet the criteria. A solution is accepted, if it meets the hard constraints for collision-avoidance, task-ordering and goal convergence. If the criteria is not met after optimization, temporal adaptation is performed of the next-best option in the ranking. If the criteria are met before optimization, the initial solution is accepted.
Temporal Adaptation
Temporal Adaptation adjusts the velocity scaling factors of the learned DSs at each step along the horizon to minimize deviations from the selected policy combination while satisfying the required constraints. Unlike spatial collision avoidance, this approach preserves the motion direction prescribed by the learned vector fields, modifying only the velocity magnitude. This enables the robots to coordinate their timing while retaining the learned motion patterns, resulting in predictable trajectories with minimal deviations from the intended motion. We first formulate the collision-avoidance and task-ordering constraints, followed by the complete optimization problem.
Robot-robot collision avoidance. To ensure whole-body collision avoidance, the distance between every pair of points on the robots’ bodies must exceed a threshold and at every step . This is expressed as,
where maps each robot configuration to a geometric representation of its body, composed of primitives such as spheres or capsules. The constraint is evaluated for all pairs of primitives across all robots. For capsule-based representations, the distance is computed as the minimum distance between their centerline segments, minus the sum of their radii [9]. These geometric representations provide compact, almost-everywhere differentiable distance functions suitable for gradient-based optimization.
Task Order. Some tasks require one robot to complete part of its task before the other can proceed, as in (6). For example, in the restaurant scene (Figure 1), a glass may only be placed after the table has been wiped underneath it. Let denote the time step at which the first robot completes the order-relevant part of the task, defined as the moment its end-effector passes over the second robot’s goal . The second robot must therefore remain outside a radius of its goal until . The resulting task-ordering constraint is defined as follows,
where depends on and the initial configuration .
Temporal optimization. For a system of robots, the resulting optimization problem determines the velocity scaling variables over the horizon, minimizing deviations from the learned dynamics while satisfying the collision-avoidance and task-ordering constraints,
The objective function penalizes deviations from nominal execution, , for all robots and time steps, using weight . The system dynamics in (10b) forward-simulate each robot’s motion over the horizon leveraging and the learned single-agent DSs. Collision avoidance is enforced through (10c), whereas (10d) represents one possible task-ordering constraint. The factors are constrained to be strictly positive and bounded to prevent backward motion and excessive scaling. Additionally, the desired end-effector velocity is limited by . Constraints (10c)–(10e) are incorporated as penalty terms, while constraint (10f) is enforced through sigmoid reparameterization.
The resulting optimization problem is solved using gradient-based methods in JAX, with gradients propagated through the differentiable learned DSs, which can be represented by DNNs. Notes on stability. In continuous time, temporal scaling with reparameterizes time without changing the spatial trajectories of the learned DSs. Consequently, their asymptotic convergence properties are preserved. However, the stability of the complete robot system in (10b) also depends on the low-level controller, robot kinematics, and constraints, which may prevent reaching the goal. In discrete time, large scaling factors may cause numerical errors or overshooting, for which stabilizing bounds can be derived for certain DSs [14]. The bounds in (10e) and (10f), on the desired end-effector velocity and respectively, help mitigate these effects and support stable execution. By restricting adaptations to temporal scaling and explicitly enforcing goal-convergence and safety criteria during optimization, our approach aims to preserve the desirable properties of the learned behaviors while enabling safe collaborative execution.
Experimental Results
We evaluate the proposed framework in three cooperative mobile manipulation scenarios, describing the experimental setup and quantitatively analyzing the results. The supplementary material comprises an Appendix with additional details, and accompanying video (https://youtu.be/9AiIJNsJPlQ).
Experimental Setup
Simulation Setup. Experiments are conducted in simulation using two mobile manipulators, each consisting of a Clearpath Dingo omnidirectional base and a Kinova Gen3 Lite arm, resulting in a 9-DoF system. The FK and LLC are parallelized using JAX [4] and FRAX [15], a tailored version of MuJoCo XLA (MJX) (Todorov et al., 2012) for robot kinematics. Each robot is approximated by 8 capsules, slightly over-approximating its geometry while enabling efficient collision-avoidance computations.
Scenarios. We consider three cooperative scenarios (Figure 2): (1) Restaurant: Robot 1 (R1) cleans a table while Robot 2 (R2) places a cup on the cleaned portion, (2) Dressing: R1 dresses a human arm while R2 uses a mirror to show different viewpoints, and (3) Drawer: R1 opens a drawer while R2 places an item inside. To focus on cooperative motion adaptation, we simplify the simulated tasks: Dressing involves moving a cylinder over the human arm, while the drawer is attached to R1 by a flexible string, avoiding rigid grasp interactions. In Restaurant, R2 may place the cup only once R1 has cleaned the intended placement area. For mirror-showing, table-cleaning, and drawer-opening, the corresponding robot must first approach the task’s starting region within a margin of , preventing the robot from starting the task midway.

Figure 2. The three scenarios considered: Drawer, Dressing and Restaurant. The visuals are snapshots of successful runs of CALDS-CDSM.
Learned DSs and Comparison. We consider two methods for learning the single-agent DSs used by CALDS: Curve-induced Dynamical systems on Smooth Manifolds (CDSM) [1] and Policy via neUral Metric leArning (PUMA) [16]. CDSM fits a curve on the manifold to demonstrated trajectories and defines a DS that attracts towards the curve while progressing along its direction. PUMA instead learns a DS using a DNN, enforcing convergence through its loss function and connecting latent-space dynamics to task-space dynamics. Both methods support pose representations on , guarantee convergence to the goal under their stated assumptions, and can be learned from few demonstrations (1–10).
We compare against ONCoI-DMP [24], using its open-source implementation. We retain its collaborative adaptation framework but replace its skill library and policy selection with the same learned DSs and selection method used by CALDS. As ONCol-DMP was designed for dual manipulators, we adapt its implementation to mobile manipulators (Appendix § 9). As a whole-body spatial-avoidance baseline, we replace temporal adaptation with a centralized CBF safety filter ((Wang et al., 2017)), applied at the joint-velocity level ((Singletary et al., 2021)). Both robots execute the same learned DS options selected by our ranking, with nominal timing. We refer to these baselines as CBF-CDSM and CBF-PUMA, respectively. At each control step, a single quadratic program (QP) minimally modifies their nominal joint velocities subject to collision-avoidance constraints for the 32 closest capsule pairs. Finally, we consider an ablation using nominal learned DSs (), with each robot’s task strategy selected according to the closest goal to its initial end-effector position.
Demonstrations and Experimental Parameters. Demonstrations are collected through a teleoperation interface. For each task-option, , we collect five demonstrations: one illustrating the desired motion and four showing adaptations from different initial configurations. CDSM and ONCol-DMP use the first demonstration to define the reference motion, whereas PUMA trains its DNN on all five to learn adaptations to different initial configurations.
Each robot uses a planning horizon of steps, corresponding to 30s. Initial configurations are randomized, and further experimental details and parameters are provided in Appendix § 9. For static obstacle avoidance (table, dresser, and chair), we fit an ellipse to modulate the learned DS, maintaining 0.1m clearance from the first demonstration to preserve target reachability. We use the L-BFGS solver from JAXopt ([3]), with time budgets of 1s for CALDS-CDSM and 3s for CALDS-PUMA, accounting for PUMA’s approximately threefold longer rollout time to allow comparable solver iterations.
Performance Metrics. We evaluate CALDS-CDSM and CALDS-PUMA against CBF-CDSM, CBF-PUMA, ONCol-DMP and the aforementioned ablation. A run is considered successful if it reaches the goal, is collision-free and meets task ordering constraints, if applicable to the scenario. Goal reaching requires the final end-effector position to be within 0.15m of the desired goal. Whole-body collision avoidance evaluates if the minimum robot-robot capsule distance is positive and whether the end-effector governed by the DS enters the static obstacle ellipse. For the Restaurant scenario, task ordering requires the cup to be placed on the cleaned portion of the table. In addition to the success rate, we report the Path Deviation, defined as the maximum discrete Fréchet distance across robots, between their end-effector paths and the nominal paths (). In addition to the success rate, we report three computation times. Training/fitting time captures the average offline computation required to obtain each learned DS. Planning time measures the time required to generate cooperative trajectories for both robots, including Cooperative Policy Selection and Temporal Adaptation. Finally, query time measures the time required to compute a single action for both robots.
Experimental Analysis
When analyzing Figure 3, we observe that the proposed CALDS-CDSM and CALDS-PUMA enable cooperative adaptation of learned DSs in all three scenarios, considerably improving the success rate (91-100%) considerably, compared to Nominal CDSM and Nominal PUMA, while achieving small path deviations. Further improvements could be achieved through longer planning horizons, more parallel runs with diverse initial guesses, increased optimization time, and allowing multiple fallbacks to lower-ranked policy combinations. However, these improvements would come at the expense of real-time adaptability. An example of the resulting trajectories is shown in Figure 1 and is further illustrated in the accompanying video.
Figure 3. Results over 200 randomized runs per scenario, reporting the overall success rate, the individual metrics, i.e. goal reaching, collision avoidance, and task ordering (restaurant scenario), and path deviation.
Compared to CBF-CDSM and CBF-PUMA, respectively, the proposed CALDS-CDSM and CALDS-PUMA achieve smaller path deviations, indicating that temporal adaptation remains closer to the path of the nominal learned DSs. Moreover, the success rates of the CBF-based approaches drop drastically in the Restaurant scenario, as their local collision-avoidance constraints can lead to conflicting requirements when avoiding both the static obstacle and the other robot, and task-ordering constraints are not enforced. ONCol-DMP achieves end-effector collision-free trajectories in 98.5% of the cases, comparable to the proposed CALDS-CDSM and CALDS-PUMA (100% and 99.5%, respectively). However, it fails to avoid whole-body collisions, highlighting the limitation of its temporal adaptation in guaranteeing whole-body collision avoidance, as also acknowledged by the authors [24]. Moreover, task ordering is not explicitly enforced in the Restaurant scenario, even when R2 is instructed to wait for R1 when their temporal adaptation is triggered.
As shown in Table 1, fitting times for CALDS-CDSM are only 1.3 0.5s, representing a negligible overhead compared to the several minutes required for collecting demonstrations. Training CALDS-PUMA takes approximately 11min per DNN, which remains reasonable given the offline nature of this process. The planning time for CALDS-PUMA (1.491.73s) is approximately twice that of CALDS-CDSM (0.76 0.64s), reflecting both the higher computational cost of forward simulation in PUMA and its larger planning budget of 3s, compared to 1s for CALDS-CDSM. Nevertheless, both approaches achieve relatively low computation times considering whole-body collision avoidance over a 30s horizon, with individual action query times of 0.18ms. For real-time adaptation in uncertain dynamic environments, however, a low-level planner capable of handling dynamic changes at 10 Hz to 100 Hz may still be required, or further computational improvements to the proposed approach may be necessary. Closed-loop simulation results for our approach and the baselines, incorporating per-time-step joint-configuration noise and subject to a strict planning time limit of 1s, are provided in Appendix § 10.
| Training/Fitting time [s] | Planning time [s] | Query time [ms] | |
| CALDS-CDSM (Ours) | |||
| CALDS-PUMA (Ours) | |||
| CBF-CDSM | |||
| CBF-PUMA | |||
| ONCol-DMP | - |
Table 1. Computational costs for the evaluated methods, reporting mean standard deviation of training/fitting time, planning time, and query time, averaged over 600 runs. The planning time of CBF-CDSM and CBF-PUMA is attributed to the cooperative policy selection procedure, whereas that of ONCol-DMP includes both policy selection and DMP generation.
Conclusion and Future Work
This work introduced CALDS, a framework for cooperative adaptation of learned DSs for mobile manipulators, integrating cooperative policy selection and temporal adaptation. We demonstrated its applicability to collaborative tasks involving two mobile manipulators across three distinct scenarios. The resulting trajectories preserve the behavior encoded by the learned DSs, while satisfying whole-body collision avoidance and task requirements. Furthermore, exploiting the stability properties of the learned DSs enables learning from few demonstrations and supports pose representations on . Future work will investigate integrating the framework into a single neural network conditioned on discrete markers representing different behavioral modes (Sun et al., 2021), as well as extending it to human-robot collaboration, where increased uncertainty and behavioral diversity introduce additional challenges.
Generative AI Use
Generative AI tools were used to assist with code generation, text revision, and figure construction. All scenes and trajectories presented in the paper originate from simulated experiments and were not AI-generated. The authors have reviewed and verified the AI-assisted content and take full responsibility for the accuracy and integrity of the final manuscript and results.
Acknowledgments
This project has received funding from the European Union through ERC, INTERACT, under Grant 101041863. Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union. Neither the European Union nor the granting authority can be held responsible for them.
Experimental Details
Table 2 summarizes the experimental setup and parameters used in the simulations, including randomized initial conditions, simulation settings, method-specific parameters for PUMA, CDSM and the baselines, temporal adaptation settings, and dynamical-system parameters. Upon publication, we will open-source our code, including examples and the evaluation of the three scenarios.
| Parameter | Value | Parameter | Value |
| Randomized start1 | |||
| Base-position perturbation, [m] | Min. robot–robot distance [m] | 0.1 | |
| Base heading perturbation, [°] | Min. distance to static objects [m] | 1.0 | |
| Arm joint perturbation, [°] | Runs per scenario | 200 | |
| Simulation | |||
| Time step [s] | 0.1 | Horizon | 300 (30s) |
| PUMA | |||
| Optimizer | Adam | Learning rate | |
| Iterations (training) | 8 000 | Activation function | GELU |
| Number of layers (, ) | (3, 3) | Latent-space dimension | 300 |
| CDSM | |||
| Curve segments | 8 | Basis functions | 3 |
| Normal/Nominal twist factor | 7.0/3.0 | Max. iterations (fitting) | 200 |
| Temporal adaptation | |||
| Range of | [0, 3] | Collision margin2 [m] | 0.2 |
| Initializations () | 4 | Solver | L-BFGS (5 iter.) |
| Time budget [s] | 1 (CDSM), 3 (PUMA) | ||
| CBF | |||
| Capsules-pairs per step | 32 | Collision margin2 [m] | 0.2 |
| QP solver | qpax3 | ||
| ONCol-DMP | |||
| Collision distance (temporal) [m] | 1.0 | Robot that slows down (temporal) | R2 |
| Collision margin (end-effector) [m] | 0.15 | ||
| Dynamical systems | |||
| Start distance [m] | 0.1 | Ellipse clearance [m] | 0.1 |
| First attract to start region (R1/R2) | Drawer: ✓/–, Dressing: ✓/✓, Restaurant: ✓/– |
Table 2. Experimental parameters.
ONCol-DMP: As ONCol-DMP was originally designed for dual manipulators, we adapt the robot model to our mobile manipulators and modify the low-level controller to apply homing only to the manipulator, consistent with our approach. Finally, we increase the collision-checking distance threshold in the temporal adaptation from 0.5m to 1.0m (Table 2) to account for the larger robot size.
Closed-Loop Evaluation under State Noise
As a complementary evaluation, we investigate the robustness of Temporal Adaptation under state noise during closed-loop execution in the same three scenarios as § 5. The planned motions are executed in MuJoCo, with Gaussian noise added to the state estimates at every control step. The
noise standard deviations are 0.005m for the base position and 0.005rad for the base orientation and manipulator joint positions. Cooperative Policy Selection is performed only once at the beginning of each run, to isolate the effect of state estimation noise on Temporal Adaptation.
The control loop runs at s (10 Hz), while Temporal Adaptation replans at 1Hz, allowing ten control steps between updates. To reflect a limited computational budget, the planning time is capped at 1s per update. In contrast, ONCol-DMP, CBF-PUMA, and CBF-CDSM replan at every control step (10Hz), as they support faster updates. At each replanning step of the Temporal Adaptation, the velocity scaling factors from the previous iteration are shifted in time to align with the current timestep and used to warm-start the optimization. If the resulting trajectory maintains a minimum robot–robot distance above the collision margin of 0.2m (Table 2), it is accepted without further optimization. Otherwise, Temporal Adaptation is triggered to compute an updated feasible solution within the available planning time.
Figure 4 shows that CALDS-CDSM preserves its high success-rate in the Drawer and Dressing scenarios (98%, 98%), with a reduction only in the Restaurant scenario due to collisions (79% compared to 91% in Figure 3). CALDS-PUMA experiences a reduced success-rate (87%, 82%, 87%) across all three scenarios compared to Figure 3 (100%, 96%, 96%). This degradation likely stems from PUMA requiring approximately three times longer to perform a forward simulation than CDSM, making it more affected by the 1s update cap. In addition, PUMA attracts trajectories towards the goal rather than the demonstrated region, making it more susceptible to deviations from the nominal path under state noise. Despite operating at a substantially lower replanning frequency than the baselines, our method maintains effective cooperative multi-robot motion across all three scenarios under state noise, while accomplishing small path deviations.
Figure 4. Experimental results of closed-loop simulations under state noise. Results over 200 randomized runs per scenario, reporting the overall success rate, the individual metrics, i.e. goal reaching, collision avoidance, and task ordering (restaurant scenario), and path deviation.
References
- [1]Saray Bakker, Martin Schonger, Tobias Löw, Javier Alonso-Mora, and Sylvain Calinon. Curve-induced dynamical systems on Riemannian manifolds and Lie groups. arXiv preprint arXiv:2603.05268, 2026.arxiv.org/abs/2603.05268
- [2]Aude Billard, Sina Mirrazavi, and Nadia Figueroa. Learning for adaptive and reactive robot control: a dynamical systems approach. MIT Press, 2022.
- [3]Mathieu Blondel, Quentin Berthet, Marco Cuturi, Roy Frostig, Stephan Hoyer, Felipe Llinares-López, Fabian Pedregosa, and Jean-Philippe Vert. Efficient and modular implicit differentiation. In Advances in Neural Information Processing Systems (NeurIPS), 2022a.arxiv.org/abs/2105.15183
- [4]Mathieu Blondel, Quentin Berthet, Marco Cuturi, Roy Frostig, Stephan Hoyer, Felipe Llinares-López, et al. Efficient and modular implicit differentiation. Advances in neural information processing systems, 35:5230–5242, 2022b.arxiv.org/abs/2105.15183
- [5]James Bradbury, Roy Frostig, Peter Hawkins, Matthew James Johnson, Chris Leary, Dougal Maclaurin, et al. JAX: composable transformations of Python+NumPy programs, 2018. URL http://github.com/jax-ml/jax.
- [6]Samuel R Buss et al. Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods. IEEE Journal of Robotics and Automation, 17(1-19):16, 2004.
- [7]Dayi Dong, Maulik Bhatt, Seoyeon Choi, and Negar Mehr. Mimic-d: Multi-modal imitation for multi-agent coordination with decentralized diffusion policies. In Proc. IEEE International Conference on Robotics and Automation (ICRA), pages 17193–17200. IEEE, 2026.arxiv.org/abs/2509.14159
- [8]Ju Dong, Lei Zhang, Liding Zhang, Yao Ling, Yu Fu, Kaixin Bai, Zoltán-Csaba Márton, Zhenshan Bing, Zhaopeng Chen, Alois Christian Knoll, et al. M4diffuser: Multi-view diffusion policy with manipulability-aware control for robust mobile manipulation. arXiv preprint arXiv:2509.14980, 2025.arxiv.org/abs/2509.14980
- [9]Christer Ericson. Real-Time Collision Detection. Morgan Kaufmann, San Francisco, 2005.DOI
- [10]Zipeng Fu, Tony Z. Zhao, and Chelsea Finn. Mobile aloha: Learning bimanual mobile manipulation with low-cost whole-body teleoperation. In Proc. Conference on Robot Learning (CoRL), 2024.arxiv.org/abs/2401.02117
- [11]Valentin N Hartmann, Andreas Orthey, Danny Driess, Ozgur S Oguz, and Marc Toussaint. Long-horizon multi-robot rearrangement planning for construction assembly. IEEE Transactions on Robotics, 39(1):239–252, 2022.DOI
- [12]Chengyang He, Gadiel Sznaier Camps, Xu Liu, Mac Schwager, and Guillaume Sartoretti. Latent theory of mind: A decentralized diffusion architecture for cooperative manipulation. Proc. Conference on Robot Learning (CoRL), 2025.arxiv.org/abs/2505.09144
- [13]Seyed Mohammad Khansari-Zadeh and Aude Billard. A dynamical system approach to realtime obstacle avoidance. Autonomous Robots, 32(4):433–454, 2012.
- [14]Byeongho Lee, Yonghyeon Lee, Junsu Ha, and Frank C Park. Behavior-controllable stable dynamics models on Riemannian configuration manifolds. IEEE Transactions on Robotics, 2025.DOI
- [15]Daniel Morton and Marco Pavone. frax: Fast robot kinematics and dynamics in jax. arXiv preprint arXiv:2604.04310, 2026.arxiv.org/abs/2604.04310
- [16]Rodrigo Pérez-Dattari, Cosimo Della Santina, and Jens Kober. Puma: Deep metric imitation learning for stable motion primitives. Advanced Intelligent Systems, 6(11):2400144, 2024.
- [17]Rodrigo Pérez-Dattari, Francisco Leiva, Andrea Testa, Leonel Rozo, Javier Ruiz del Solar, and Noémie Jaquier. Let the dynamics flow: Stable flow matching dynamical systems. arXiv preprint arXiv:2606.03834, 2026.
- [18]Lasse Peters, Laura Ferranti, Andrea Bajcsy, and Javier Alonso-Mora. Coordinated diffusion: Generating multi-agent behavior without multi-agent demonstrations. arXiv preprint arXiv:2605.11485, 2026.arxiv.org/abs/2605.11485
- [19]Nathan D Ratliff, Jan Issac, Daniel Kappler, Stan Birchfield, and Dieter Fox. Riemannian motion policies. arXiv preprint arXiv:1801.02854, 2018.arxiv.org/abs/1801.02854
- [20]Moritz Reuss, Maximilian Li, Xiaogang Jia, and Rudolf Lioutikov. Goal-conditioned imitation learning using score-based diffusion policies. Proc. of Robotics: Science and Systems, 2023.arxiv.org/abs/2304.02532
- [21]Jarosław Rudy, Radosław Idzikowski, Elzbieta Roszkowska, and Konrad Kluwak. Multiple mobile robots coordination in shared workspace for task makespan minimization. Processes, 10(10):2087, 2022.DOI
- [22]Sayan Saha and Anak Agung Julius. Task and motion planning for manipulator arms with metric temporal logic specifications. IEEE Robotics and Automation Letters, 3(1):379–386, 2017.DOI
- [23]Jorge Antonio Sánchez-Molina, Francisco Rodríguez, José Carlos Moreno, Julián Sánchez-Hermosilla, and Antonio Giménez. Robotics in greenhouses. Scoping review. Computers and Electronics in Agriculture, 219:108750, 2024.
- [24]Siddharth Singh, Tian Xu, and Qing Chang. Collaborative motion planning for multi-manipulator systems through reinforcement learning and dynamic movement primitives. In Proc. IEEE International Conference on Robotics and Automation (ICRA), pages 3369–3375, 2025.DOI

