Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Dual-Arm Whole-Body Motion Planning: Leveraging Overlapping Kinematic Chains

Richard Cheng, Peter Werner, Carolyn Matl

TL;DR

This paper tackles real-time motion planning for high-DoF dual-arm robots in changing environments by decomposing the problem into two lower-dimensional dynamic roadmaps, each covering a arm+torso chain. By enforcing shared-torso structure and introducing inter-arm collision maps, the authors construct a pair of composable DRMs that collectively encode full collision information without brute-forcing the 19D space. An online dual-roadmap graph search alternates between the two DRMs to find collision-free, consistent paths, followed by lazy collision checks to finalize feasibility. Real-world validation on a 19-DoF mobile manipulator performing grocery fulfillment demonstrates fast planning (average 0.42 s) with high reliability (99.9% success over 2101 plans), underscoring the practical impact for complex manipulation in dynamic environments.

Abstract

High degree-of-freedom dual-arm robots are becoming increasingly common due to their morphology enabling them to operate effectively in human environments. However, motion planning in real-time within unknown, changing environments remains a challenge for such robots due to the high dimensionality of the configuration space and the complex collision-avoidance constraints that must be obeyed. In this work, we propose a novel way to alleviate the curse of dimensionality by leveraging the structure imposed by shared joints (e.g. torso joints) in a dual-arm robot. First, we build two dynamic roadmaps (DRM) for each kinematic chain (i.e. left arm + torso, right arm + torso) with specific structure induced by the shared joints. Then, we show that we can leverage this structure to efficiently search through the composition of the two roadmaps and largely sidestep the curse of dimensionality. Finally, we run several experiments in a real-world grocery store with this motion planner on a 19 DoF mobile manipulation robot executing a grocery fulfillment task, achieving 0.4s average planning times with 99.9% success rate across more than 2000 motion plans.

Dual-Arm Whole-Body Motion Planning: Leveraging Overlapping Kinematic Chains

TL;DR

This paper tackles real-time motion planning for high-DoF dual-arm robots in changing environments by decomposing the problem into two lower-dimensional dynamic roadmaps, each covering a arm+torso chain. By enforcing shared-torso structure and introducing inter-arm collision maps, the authors construct a pair of composable DRMs that collectively encode full collision information without brute-forcing the 19D space. An online dual-roadmap graph search alternates between the two DRMs to find collision-free, consistent paths, followed by lazy collision checks to finalize feasibility. Real-world validation on a 19-DoF mobile manipulator performing grocery fulfillment demonstrates fast planning (average 0.42 s) with high reliability (99.9% success over 2101 plans), underscoring the practical impact for complex manipulation in dynamic environments.

Abstract

High degree-of-freedom dual-arm robots are becoming increasingly common due to their morphology enabling them to operate effectively in human environments. However, motion planning in real-time within unknown, changing environments remains a challenge for such robots due to the high dimensionality of the configuration space and the complex collision-avoidance constraints that must be obeyed. In this work, we propose a novel way to alleviate the curse of dimensionality by leveraging the structure imposed by shared joints (e.g. torso joints) in a dual-arm robot. First, we build two dynamic roadmaps (DRM) for each kinematic chain (i.e. left arm + torso, right arm + torso) with specific structure induced by the shared joints. Then, we show that we can leverage this structure to efficiently search through the composition of the two roadmaps and largely sidestep the curse of dimensionality. Finally, we run several experiments in a real-world grocery store with this motion planner on a 19 DoF mobile manipulation robot executing a grocery fulfillment task, achieving 0.4s average planning times with 99.9% success rate across more than 2000 motion plans.

Paper Structure

This paper contains 13 sections, 2 theorems, 2 equations, 5 figures, 2 tables, 2 algorithms.

Table of Contents

  1. Introduction
  2. Related Work
  3. Sampling-based motion planning
  4. Optimization-based motion planning
  5. Learning-based motion planning
  6. Problem Setup
  7. Preliminaries - Dynamic Roadmaps
  8. Methods
  9. Building Composable Roadmaps (offline)
  10. Planning within Composition of Roadmaps (online)
  11. Results
  12. Conclusion
  13. Appendix

Key Result

Proposition 1

A node pair $(x_{\mathcal{R}_1}, x_{\mathcal{R}_2})$ (where $x_{\mathcal{R}_i} \subset \textnormal{supp}(\mathcal{R}_{arm_i})$) is in collision if and only if at least one of the following holds:

Figures (5)

  • Figure 1: Image of the robot grasping a can in the grocery store. The robot has a wheeled base, a 5 DoF torso, and two 7 DoF arms.
  • Figure 2: A DRM extends a PRM by adding lookup tables that enable rapid updates when obstacles appear in the task space. The task space is discretized into voxels, each represented by a circumscribing sphere for collision detection. A lookup table $\mathcal{M}_c$ maps each sphere to roadmap nodes that would collide with it. The illustration shows a yellow sphere with three colliding configurations (red, green, blue) stored in $\mathcal{M}_c$. The configuration space diagram displays the underlying PRM and obstacles, with the three colliding configurations highlighted and the yellow sphere's corresponding configuration obstacle marked in yellow. Figure adapted from Werner2025.
  • Figure 3: (a) This depicts a 3D representation of the high-dimensional space spanned by $\mathcal{C}_{torso} \times \mathcal{C}_{arm_1} \times \mathcal{C}_{arm_2}$. The 2D projection on the left represents $\mathcal{R}_{arm_1}$, the DRM for kinematic chain 1, and the 2D projection on the right represents, $\mathcal{R}_{arm_2}$, the DRM for kinematic chain 2. In the center, the 2D projections for each kinematic chain are composed to form an implicit 3D graph spanning $\mathcal{C}_{torso} \times \mathcal{C}_{arm_1} \times \mathcal{C}_{arm_2}$ with $\mathcal{C}_{torso}$ shared by the two chains. (b) Once a voxel map is provided, nodes in $\mathcal{R}_{arm_1}$ and $\mathcal{R}_{arm_2}$ that are in/near collision are invalidated (red). (c) The remaining (yellow) nodes and edges form the implicit graph spanning $\mathcal{C}_{torso} \times \mathcal{C}_{arm_1} \times \mathcal{C}_{arm_2}$. The nodes of this graph are guaranteed to be collision-free, and we can rapidly find collision-free paths following Algorithm
  • Figure 4: Dual-Roadmap Graph Search
  • Figure 5: Snapshot images of robot motion plans while grabbing items in the grocery store.

Theorems & Definitions (4)

  • Proposition 1
  • Remark 1
  • Proposition 1
  • proof