RichMap: A Reachability Map Balancing Precision, Efficiency, and Flexibility for Rich Robot Manipulation Tasks

Yupu Lu; Yuxiang Ma; Jia Pan

cs.ROApr 8, 2026

RichMap: A Reachability Map Balancing Precision, Efficiency, and Flexibility for Rich Robot Manipulation Tasks

Authors: Yupu Lu, Yuxiang Ma, Jia Pan

Organizations: School of Computing and Data Science, The University of Hong Kong, HKSAR · Massachusetts Institute of Technology, Cambridge, USA

Abstract

This paper presents RichMap, a high-precision reachability map representation designed to balance efficiency and flexibility for versatile robot manipulation tasks. By refining the classic grid-based structure, we propose a streamlined approach that achieves performance close to compact map forms (e.g., RM4D) while maintaining structural flexibility. Our method utilizes theoretical capacity bounds on $\mathbb{S}^2$ (or $SO(3)$ ) to ensure rigorous coverage and employs an asynchronous pipeline for efficient construction. We validate the map against comprehensive metrics, pursuing high prediction accuracy ( $>98\%$ ), low false positive rates ( $1\sim2\%$ ), and fast large-batch query ( $\sim$ 15 $μ$ s/query). We extend the framework applications to quantify robot workspace similarity via maximum mean discrepancy (MMD) metrics and demonstrate energy-based guidance for diffusion policy transfer, achieving up to $26\%$ improvement for cross-embodiment scenarios in the block pushing experiment.

Figures & tables

Panel of Figure 1 — Figure 1: Reachability map representation. Left: 3D workspace discretization with cell size $\Delta$ . Each cell stores approach direction vectors $\{\mathbf{v}_{k}\}$ (red arrows). Right: Orientation coverage via spherical cap packing. Directions $\mathbf{v}_{1},\mathbf{v}_{2}$ maintain separation $\theta$ (green arc), with exclusive $\theta/2$ -radius caps (blue dashed) ensuring non-overlapping packing.

Panel of Figure 2 — Figure 2: Asynchronous pipeline architecture. Multiple CPU worker processes continuously generate collision-free poses in parallel while the main process executes GPU-accelerated batch insertions. The inter-process queue decouples the two stages, and batch size adjusts dynamically based on insertion throughput.

Query Efficiency ( $\mu$ s/query) with Varying Batch Sizes
Method	Panda			IIWA7			UR5e			Kinova3
Method	Ins.	Acc.	FPR	Ins.	Acc.	FPR	Ins.	Acc.	FPR	Ins.	Acc.	FPR
RM4D	1.1M	.980	.035	1.1M	.982	.035	1.5M	.984	.058	1.0M	.982	.045
CM	10.7M	.974	.052	9.8M	.972	.058	16.4M	.973	.082	9.1M	.972	.063
Ours	11.3M	.975	.037	10.2M	.978	.036	18.2M	.978	.063	10.0M	.978	.046
Method	Panda			IIWA7			UR5e			Kinova3

Table 1: Comparison of Map Performance and Query Efficiency against Counterparts (

\Delta=0.05

\theta=0.1/0.4

rad).

Robot	$\Delta$ (m)	Inserted	Occ.	$\nicefrac{{\text{Avg}}}{{\text{Cell}}}$	Acc.	TPR	FPR	$\text{IR}_{\text{whole}}$	Time
Panda	0.05	11.3M	35.2%	503	0.975	0.982	0.037	6.4%	1.2
IIWA7	0.05	10.2M	43.5%	367	0.978	0.987	0.036	6.0%	1.3
UR5e	0.05	18.2M	52.7%	541	0.978	0.992	0.063	5.5%	1.3
Kinova3	0.05	10.0M	34.5%	454	0.978	0.991	0.046	5.4%	1.2
Panda	0.02	164.1M	32.8%	501	0.982	0.981	0.016	6.8%	13.5
IIWA7	0.02	145.1M	40.5%	358	0.984	0.984	0.015	6.7%	15.2

Table 2: Reachability Map Construction Results (

\theta=0.1

rad).

Figure 3: Prediction metrics evolution during reachability map construction for four robots at two spatial resolutions ( $\Delta=0.05$ m solid lines, $\Delta=0.02$ m dashed lines). All robots show rapid initial improvement followed by gradual saturation. Accuracy converges to $>97\%$ for both resolutions, demonstrating reliable feasibility prediction. The finer resolution ( $\Delta=0.02$ m) achieves significantly lower FPR ( $<$ 3%) compared to coarser resolution ( $\sim$ 4–7%), confirming that boundary approximation errors dominate false positives. Insertion rate decreases logarithmically as the map saturates, with both resolutions converging to similar final rates ( $\sim$ 7%), indicating consistent orientation coverage density.

Robot	Direct Transfer		Guided Transfer		Improvement
Robot	Success	Range	Success	Range	Improvement
xArm6 (source)	95.40%	92.94–98.00%	–	–	–
UR3	64.64%	62.82–71.88%	91.34%	87.84–95.96%	+26.70%
UR5	95.15%	92.86–98.98%	94.60%	89.88–98.98%	$-$ 0.54%
UR10	75.96%	74.70–82.74%	82.72%	77.64–86.78%	+6.76%
Average	78.58%	–	89.56%	–	+10.97%

Table 3: Success rates for policy transfer from xArm6 source to UR series robots over 9 models.

Figure 4: Top view field visualizations across UR series transfers. Rows show similarity (top, via MMD) and gradient magnitude (bottom, $\|\nabla E\|$ ), columns show target robots. Similarity colormap: blue indicates low MMD (high similarity), red indicates high MMD (low similarity). The red dashed rectangle marks the dangerous region near the robot base where UR3 and UR10 exhibit low similarity (high MMD, orange/red), prone to generating self-constrained poses during direct transfer. In contrast, UR5 maintains high similarity (low MMD, blue) in this region, implying better direct policy transfer performance. Gradient fields (bottom) show how the direction and magnitude of energy increase, indicating how guidance pushes trajectories away from low-similarity regions. Rectangle annotations denote robot base (dot), table area (white dashed), initial block positions (orange dotted), and target regions (red and green solid).

Figure 5: Qualitative results showing the block pushing task execution. Top row depicts the xArm6 source policy execution. Subsequent rows show the same task transferred to UR3 with varying guidance strengths ( $\lambda=0.0$ direct transfer, $\lambda=0.3,0.5,1.0$ with increasing guidance). The sequence spans the first 5 seconds at regular time intervals. The block is progressively pushed toward the farther side of the target regions as guidance strength increases, illustrating how reachability guidance influences the trajectory generation.

Explore similar work

ContraMap: Contrastive Uncertainty Mapping for Robot Environment Representation
Mar 29, 2026Chi Cuong Le, Weiming ZhiUncertainty Quantification Semantic Occupancy Mapping
School of Computer Science and the Australian Centre for Robotics, University of Sydney, Australia
RAM: Reachability Across Morphologies
Jun 8, 2026Tim Walter, Xinyu Chen, Jonathan Külz +1Reachability Analysis Implicit Neural Representations
Department of Computer Engineering, Technical University Munich, Germany · German Electron Synchrotron, University of Hamburg, Germany
M2-SMap: Memory-Efficient Semantic Mapping with Hierarchical Multi-Model Representation
Aug 7, 2026QiYing Deng, ZhongLai Wang, Yuan Gao +13D Scene Representation Semantic Mapping
University of Electronic Science and Technology of China · State Key Laboratory of Mechanical System and Vibration, School of Mechanical Engineering, Shanghai Jiao Tong University, China

Mar 29, 2026J/K move · Enter open · S save

ContraMap: Contrastive Uncertainty Mapping for Robot Environment Representation

Chi Cuong Le, Weiming Zhi

School of Computer Science and the Australian Centre for Robotics, University of Sydney, Australia

Uncertainty Quantification Semantic Occupancy Mapping Mobile Robotics Contrastive Learning

Open paper AskPDF arXiv

Reliable robot perception requires not only predicting scene structure, but also identifying where predictions should be treated as unreliable due to sparse or missing observations. We present ContraMap, a contrastive continuous mapping method that augments kernel-based discriminative maps with an explicit uncertainty class trained using synthetic noise samples. This formulation treats unobserved regions as a contrastive class, enabling joint environment prediction and spatial uncertainty estimation in real time without Bayesian inference. Under a simple mixture-model view, we show that the probability assigned to the uncertainty class is a monotonic function of a distance-aware uncertainty surrogate. Experiments in 2D occupancy mapping, 3D semantic mapping, and tabletop scene reconstruction show that ContraMap preserves mapping quality, produces spatially coherent uncertainty estimates, and is substantially more efficient than Bayesian kernelmap baselines.

Abstract

Explore similar work

ContraMap: Contrastive Uncertainty Mapping for Robot Environment Representation

RAM: Reachability Across Morphologies

M2-SMap: Memory-Efficient Semantic Mapping with Hierarchical Multi-Model Representation