ParaMaP: Parallel Mapping and Collision-free Motion Planning for Reactive Robot Manipulation
Abstract
Real-time and collision-free motion planning remains challenging for robotic manipulation in unknown environments due to continuous perception updates and the need for frequent online replanning. To address these challenges, we propose a parallel mapping and motion planning framework that tightly integrates Euclidean Distance Transform (EDT)-based environment representation with a sampling-based model predictive control (SMPC) planner.
On the mapping side, a dense distance-field-based representation is constructed using a GPU-based EDT and augmented with a robot-masked update mechanism to prevent false self-collision detections during online perception. On the planning side, motion generation is formulated as a stochastic optimization problem with a unified objective function and efficiently solved by evaluating large batches of candidate rollouts in parallel within a SMPC framework, in which a geometrically consistent pose tracking metric defined on SE(3) is incorporated to ensure fast and accurate convergence to the target pose.
The entire mapping and planning pipeline is implemented on the GPU to support high-frequency replanning. The effectiveness of the proposed framework is validated through extensive simulations and real-world experiments on a 7-DoF robotic manipulator. More details are available at: https://zxw610.github.io/ParaMaP.
Contributions
Sampling-based Motion Planning on SE(3)
We propose a novel sampling-based motion planning formulation for robotic manipulation that integrates distance-field-based collision costs with a Lie-algebra-based pose tracking objective on SE(3) within a stochastic MPC framework, enabling safe and reactive motion generation in unknown environments.
Geometrically Consistent Pose Error Metric
We introduce a Lie-algebra-based pose error metric on SE(3) and incorporate it into the proposed stochastic MPC formulation, providing a geometrically consistent treatment of rotational and translational deviations and enabling fast convergence under practical execution constraints.
Parallel Mapping and Distance Field Construction
We develop an efficient parallel mapping and Euclidean Distance Field construction method based on a gather-then-transform strategy and robot-masked updates, supporting low-latency distance queries and reliable collision avoidance during online replanning.
Unified GPU-based Architecture
The proposed mapping and planning framework achieves real-time and high-frequency replanning for reactive robot manipulation by leveraging a unified CUDA-based architecture that fully exploits massive GPU parallelism.
Benchmark Comparisons
Mapping Evaluation
We evaluate the efficiency of the proposed mapping algorithm on two public datasets covering both depth-camera and LiDAR sensing modalities, namely the Flat Dataset and the Dynablox Dataset . The proposed method is compared against the state-of-the-art GIE-Mapping under identical experimental settings.
Quantitative results show that our approach consistently achieves lower total mapping time (occupancy grid mapping + Euclidean distance transform) across different voxel resolutions. The performance gain is primarily attributed to a significantly faster EDT update, while the OGM stage also exhibits a slight reduction in computation time due to a streamlined system implementation.
Notably, the proposed EDT construction remains efficient even at fine voxel resolutions and large map sizes, demonstrating its effectiveness across different sensing modalities and suitability for high-frequency replanning in reactive manipulation scenarios.
Motion Planning Evaluation
We evaluate the proposed SMPC-based motion planner and compare it against RRTConnect (OMPL/MoveIt) and STORM in a Gazebo benchmark with known obstacles. A representative planning example is shown in the video below.
RRTConnect
STORM
Proposed
The experimental results are summarized in Table I, with all metrics averaged over multiple runs. The proposed method achieves the shortest planning time and motion time, the shortest path length, and the smallest final pose error. These improvements are enabled by a fully GPU-parallelized SMPC implementation and a geometrically consistent pose error formulation on SE(3).
Quantitative Comparison
| Metric | RRTConnect | STORM | Proposed |
|---|---|---|---|
| Planning Time (ms) ↓ | 61.202 | 18.875 | 5.322 |
| Motion Time (s) ↓ | 13.720 | 16.580 | 12.412 |
| Path Length (rad.) ↓ | 13.153 | 15.316 | 12.341 |
| Position Error (mm) ↓ | 3.674 | 9.299 | 2.778 |
| Orientation Error (rad.) ↓ | 0.104 | 0.327 | 0.049 |
Table I: Quantitative comparison of motion-planning performance across different methods. Best results are highlighted in bold.
Integrated System Evaluation
To further assess the proposed approach, we conduct an integrated system evaluation of the full pipeline that integrates online mapping and motion planning. The experiments are performed in a cluttered Gazebo simulation environment, as shown in the following video.
We further report system-level runtime performance. The results indicate that the proposed mapping and planning pipeline can operate at update rates exceeding 150 Hz in real time. These results demonstrate that the proposed framework enables collision-free and reactive motion planning for robotic manipulation in dynamic environments.
Real-world Experiments
We further validate the proposed approach through real-world experiments on a 7-DoF Flexiv Rizon 4 robotic manipulator equipped with an Intel RealSense D435i depth camera. Two representative scenarios are evaluated: (1) point-to-point motion planning, and (2) reactive motion planning under dynamic obstacles introduced by a moving human arm. The experimental videos are illustrated below.
The first scenario
The second scenario
These results validate the effectiveness of the proposed mapping and motion planning framework in real-world scenarios.
More Experiments
We demonstrate the capabilities of the proposed framework through both simulation and real-world experiments, covering pose reaching, obstacle avoidance, and reactive planning by more experiments.
Simulation Experiments
Cartesian Pose Reaching
The robot reaches target Cartesian poses accurately and quickly, validating the effectiveness of the geometrically consistent pose error formulation on SE(3).
Obstacle Avoidance with Known Environment
The robot avoids known static obstacles using pre-defined environment models, validating planning performance in fully known scenes.
Obstacle Avoidance with Unknown Environment
The robot constructs and maintains an online EDT map and replans motions in real time to ensure collision avoidance.
Obstacle Avoidance with Unknown Dynamic Obstacles
The robot reacts to previously unknown and moving obstacles by continuously updating the map and replanning collision-free motions.