Abstract
In dynamic environments shared by humans and robots, mobile manipulators must continuously adapt to real-time changes to execute tasks successfully. While global planning methods are effective at considering the full task scope, they lack the computational efficiency required for reactive adaptation. In contrast, local planning approaches can be executed online but are limited by their inability to account for the full task's duration.
To tackle this, we propose Globally-Guided Geometric Fabrics (G3F), a framework for real-time motion generation along the full task horizon, by interleaving an optimization-based planner with a fast reactive geometric motion planner, called geometric fabrics.
The approach adapts the path and explores alternative target poses, while accounting for collision avoidance and the robot's physical constraints.
This results in a real-time adaptive framework considering whole-body motions, where a robot operates in close proximity to other robots and humans.
We validate our approach through various simulations and real-world experiments on mobile manipulators in multi-agent settings, achieving improved success rates compared to vanilla geometric fabrics, prioritized rollout fabrics and model predictive control.
Index Terms: Path and motion planning, geometric fabrics, optimization-based planning, multi-agent environments
Real-world experiments
Comparison GF and G3F
Geometric Fabrics
- Unsuccessful grasps -
G3F (ours)
- Successful grasps -
G3F explores alternative grasp poses based on the current state of the dynamic environment, allowing both robots to reach their target
Pick and place scenario
The proposed approach optimizes the path from start to goal for a pick-and-place task, adapting the grasp and place pose accordingly.
Human-centered environments
In the presence of a human agent, G3F is able to react to the human changing the target pose online by moving the object, while avoiding collisions with the human.
Static obstacles
In the presence of static obstacles, G3F achieves its goal considering the full task horizon from start to goal.
Static agents
In the presence of static agents, G3F achieves its goal by adapting its grasp and place pose while considering the full task horizon.
Crossover scenario
In this decentralized multi-agent environment, the robots solve a crossover scenario due to the globally-guided solution via G3F.
Simulation experiments
Comparative examples of G3F versus GF and MPC demonstrating the benefits of globally-guided motions in aiding the overcoming of local minima.
The proposed G3F outperforms Geometric Fabrics (GF), Prioritzed Rollout Fabrics (P-RF) and MPC in terms of success rate over all four scenarios consisting of 20 randomized multi-agent environments.
Illustration of grasp pose optimization
Alternative target poses are explored based on the current state of the robot and the dynamic environment. In the Figure below, it is shown that the target pose is optimized with respect to the current end-effector pose of the robot. The axis along which the rotations are allowed, can vary based on the task.
Specifications of G3F
Table 1: Selection of the parameters used within G3F.
|
Parameter |
Value |
| QP |
solver-type |
OSQP |
| εabsolute |
10-3 |
| εrelative |
10-3 |
| max iterations |
1000 |
| H |
5 |
| T |
5 or 10 |
| Collision links |
[chassis_link, arm_upper_wrist_link] |
| Collision link radii |
[0.65, 0.15] rad |
| RF |
K |
500 |
| Δt |
0.05 s |
| αvelocity |
0.7 |
| Collision links |
[chassis_link, arm_upper_wrist_link] |
| Collision link radii |
[0.55, 0.2] rad |
| GF |
Collision links |
[chassis_link, arm_shoulder_link, arm_forearm_link,
arm_lower_wrist_link, arm_upper_wrist_link, arm_end_effector_link]
|
| Collision link radii |
[0.45, 0.1, 0.1, 0.1, 0.1, 0.1] rad |
Pushing Through Clutter With Movability Awareness of Blocking Obstacles
Joris J. Weeda,
Saray Bakker,
Gang Chen,
Javier Alonso-Mora.
In IEEE Int. Conf. on Robotics and Automation (ICRA),
2025.
Navigation Among Movable Obstacles (NAMO) poses a challenge for traditional path-planning methods when obstacles block the path, requiring push actions to reach the goal. We propose a framework that enables movability-aware planning to overcome this challenge without relying on explicit obstacle placement. Our framework integrates a global Semantic Visibility Graph and a local Model Predictive Path Integral (SVG-MPPI) approach to efficiently sample rollouts, taking into account the continuous range of obstacle movability. A physics engine is adopted to simulate the interaction result of the rollouts with the environment, and generate trajectories that minimize contact force. In qualitative and quantitative experiments, SVG-MPPI outperforms the existing paradigm that uses only binary movability for planning, achieving higher success rates with reduced cumulative contact forces.
Globally-Guided Geometric Fabrics for Reactive Mobile Manipulation in Dynamic Environments
T. Merva,
S. Bakker,
M. Spahn,
D. Zhao,
I. Virgala,
J. Alonso-Mora.
In IEEE Robotics and Automation Letters (RA-L),
2025.
Mobile manipulators operating in dynamic environments shared with humans and robots must adapt in real time to environmental changes to complete their tasks effectively. While global planning methods are effective at considering the full task scope, they lack the computational efficiency required for reactive adaptation. In contrast, local planning approaches can be executed online but are limited by their inability to account for the full task’s duration. To tackle this, we propose Globally-Guided Geometric Fabrics (G3F), a framework for real-time motion generation along the full task horizon, by interleaving an optimization-based planner with a fast reactive geometric motion planner, called Geometric Fabrics (GF). The approach adapts the path and explores a multitude of acceptable target poses, while accounting for collision avoidance and the robot’s physical constraints. This results in a real-time adaptive framework considering whole-body motions, where a robot operates in close proximity to other robots and humans. We validate our approach through various simulations and real-world experiments on mobile manipulators in multi-agent settings, achieving improved success rates compared to vanilla GF, Prioritized Rollout Fabrics and Model Predictive Control.
TamedPUMA: safe and stable imitation learning with geometric fabrics
S. Bakker,
R. Pérez-Dattari,
C. Della Santina,
W. Böhmer,
J. Alonso-Mora.
In Learning for Dynamics and Control (L4DC),
2025.
Using the language of dynamical systems, Imitation learning (IL) provides an intuitive and effective way of teaching stable task-space motions to robots with goal convergence. Yet, IL techniques are affected by serious limitations when it comes to ensuring safety and fulfillment of physical constraints. With this work, we solve this challenge via TamedPUMA, an IL algorithm augmented with a recent development in motion generation called geometric fabrics. As both the IL policy and geometric fabrics describe motions as artificial second-order dynamical systems, we propose two variations where IL provides a navigation policy for geometric fabrics. The result is a stable imitation learning strategy within which we can seamlessly blend geometrical constraints like collision avoidance and joint limits. Beyond providing a theoretical analysis, we demonstrate TamedPUMA with simulated and real-world tasks, including a 7-DoF manipulator.
Safe and stable motion primitives via imitation learning and geometric fabrics
Saray Bakker,
Rodrigo Pérez-Dattari,
Cosimo Della Santina,
Wendelin Böhmer,
Javier Alonso-Mora.
In Robotics: Science and Systems, Workshop on Structural Priors as Inductive Biases for Learning Robot Dynamics,
2024.
Using the language of dynamical systems, Imitation learning (IL) provides an intuitive and effective way of teaching stable task-space motions to robots with goal convergence. Yet, these techniques are affected by serious limitations when it comes to ensuring safety and fulfillment of physical constraints. With this work, we propose to solve this challenge via TamedPUMA, an IL algorithm augmented with a recent development in motion planning called geometric fabrics. We explore two variations of this approach, which we name the forcing policy method and the compatible potential method. Making these combinations possible requires two enabling factors: the possibility of learning second-order dynamical systems by imitation and the availability of a potential function that is compatible with the learned dynamics. In this paper, we show how these conditions can be met when using an IL strategy called PUMA. The result is a stable imitation learning strategy within which we can seamlessly blend geometrical constraints like collision avoidance and joint limits. Beyond providing a theoretical analysis, we demonstrate TamedPUMA with simulated and real-world tasks, including a 7-degree-of-freedom manipulator that is trained to pick a tomato from a crate in the presence of obstacles.
Reactive grasp and motion planning for adaptive mobile manipulation among obstacles
Tomas Merva,
Saray Bakker,
Max Spahn,
Ivan Virgala,
Javier Alonso-Mora.
In Robotics: Science and Systems, Workshop on Frontiers of Optimization for Robotics,
2024.
Mobile manipulators are susceptible to situations in which the precomputed grasp pose is not reachable as the result of conflicts between collision avoidance behaviour and the manipulation task. In this work, we address this issue by combining real-time grasp planning with geometric motion planning for decentralized multi-agent systems, referred to as Reactive Grasp Fabrics (RGF). We optimize the precomputed grasp pose candidate to account for obstacles and the robot's kinematics. By leveraging a reactive geometric motion planner, specifically geometric fabrics, the grasp optimization problem can be simplified, resulting in a fast, adaptive framework that can resolve deadlock situations in pick-and-place tasks. We demonstrate the robustness of this approach by controlling a mobile manipulator in both simulation and real-world experiments in dynamic environments.
Multi-Robot Local Motion Planning Using Dynamic Optimization Fabrics
Saray Bakker,
Luzia Knoedler,
Max Spahn,
Wendelin Boehmer,
Javier Alonso-Mora.
In Proc. IEEE International Symposium on Multi-Robot and Multi-Agent Systems,
2023.
In this paper, we address the problem of real-time motion planning for multiple robotic manipulators that operate in close proximity. We build upon the concept of dynamic fabrics and extend them to multi-robot systems, referred to as Multi-Robot Dynamic Fabrics (MRDF). This geometric method enables a very high planning frequency for high-dimensional systems at the expense of being reactive and prone to deadlocks. To detect and resolve deadlocks, we propose Rollout Fabrics where MRDF are forward simulated in a decentralized manner. We validate the methods in simulated close-proximity pick-and-place scenarios with multiple manipulators, showing high success rates and real-time performance.
Group-based Distributed Auction Algorithms for Multi-Robot Task Assignment
Xiaoshan Bai,
Andres Fielbaum,
Maximilian Kronmuller,
Luzia Knoedler,
Javier Alonso-Mora.
In IEEE Transactions on Automation Science and Engineering (T-ASE),
2023.
This paper studies the multi-robot task assignment problem in which a fleet of dispersed robots needs to efficiently transport a set of dynamically appearing packages from their initial locations to corresponding destinations within prescribed time-windows. Each robot can carry multiple packages simultaneously within its capacity. Given a sufficiently large robot fleet, the objective is to minimize the robots' total travel time to transport the packages within their respective time-window constraints. The problem is shown to be NP-hard, and we design two group-based distributed auction algorithms to solve this task assignment problem. Guided by the auction algorithms, robots first distributively calculate feasible package groups that they can serve, and then communicate to find an assignment of package groups. We quantify the potential of the algorithms with respect to the number of employed robots and the capacity of the robots by considering the robots' total travel time to transport all packages. Simulation results show that the designed algorithms are competitive compared with an exact centralized Integer Linear Program representation solved with the commercial solver Gurobi, and superior to popular greedy algorithms and a heuristic distributed task allocation method.
Integrated Task Assignment and Path Planning for Capacitated Multi-Agent Pickup and Delivery
Z. Chen,
J. Alonso-Mora,
X. Bai,
D. D. Harabor,
P. J. Stuckey.
In , IEEE Robotics and Automation Letters (RA-L),
2021.
Multi-agent Pickup and Delivery (MAPD) is a challenging industrial problem where a team of robots is tasked with transporting a set of tasks, each from an initial location and each to a specified target location. Appearing in the context of automated warehouse logistics and automated mail sortation, MAPD requires first deciding which robot is assigned what task (i.e., Task Assignment or TA) followed by a subsequent coordination problem where each robot must be assigned collision-free paths so as to successfully complete its assignment (i.e., Multi-Agent Path Finding or MAPF). Leading methods in this area solve MAPD sequentially: first assigning tasks, then assigning paths. In this work we propose a new coupled method where task assignment choices are informed by actual delivery costs instead of by lower-bound estimates. The main ingredients of our approach are a marginal-cost assignment heuristic and a meta-heuristic improvement strategy based on Large Neighbourhood Search. As a further contribution, we also consider a variant of the MAPD problem where each robot can carry multiple tasks instead of just one. Numerical simulations show that our approach yields efficient and timely solutions and we report significant improvement compared with other recent methods from the literature.
Anticipatory Vehicle Routing for Same-Day Pick-up and Delivery using Historical Data Clustering
J. van Lochem,
M. Kronmueller,
P. van 't Hof,
J. Alonso-Mora.
In Proc. IEEE Int. Conf. on Intelligent Transportation Systems (ITSC),
2020.
In this paper we address the problem of same-day pick-up and delivery where a set of tasks are known a priori and a set of tasks are revealed during operation. The vehicle routes are precomputed based on the known and predicted requests and adjusted online as new requests are revealed. We propose a novel anticipatory insertion method which incorporates a set of predicted requests to beneficially adjust the routes of a fleet of vehicles in real-time. Requests are predicted based on historical data, which is clustered in advance. We exploit inherent patterns of the demand, which are captured by historical data and include them in a dynamic vehicle routing solver based on heuristics and adaptive large neighborhood search. The proposed method is evaluated using numerical simulations on a variety of real-world problems with up to 1655 requests per day. Their degree of dynamism ranges from 0.70 to 0.93. These instances represent dynamic multi-depot pickup and delivery problems with time windows. The method has shown to require less driven kilometers than comparable methods.
