Globally-guided geometric fabrics for reactive mobile manipulation in dynamic environments

Tomas Merva*1, 2 Saray Bakker*1 Max Spahn1 Danning Zhao1 Ivan Virgala2 Javier Alonso-Mora1
*Equal contribution 1Mechanical Engineering, TU Delft, The Netherlands 2Industrial Automation and Mechatronics, Technical University of Kosice, Slovakia.
Code (released on publication) Related Publications

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

(a)

(b)

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

(a)

(b)

In the presence of static obstacles, G3F achieves its goal considering the full task horizon from start to goal.

Static agents

(a)

(b)

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

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.

lab
lab

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

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.

web pdf
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.

pdf
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.

pdf code video
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.
Back to Publications