Reachability Map

Example script: examples/reachability_map_demo.py

This example demonstrates how to compute and visualize a robot’s workspace reachability map using batch forward kinematics with JAX or NumPy backend.

Source Code

https://github.com/iory/scikit-robot/blob/main/examples/reachability_map_demo.py

Overview

A reachability map is a discretized representation of a robot’s workspace that indicates which positions (and orientations) are reachable by the end-effector. This is useful for:

• Task planning: Quickly check if a target is reachable before attempting IK

• Workspace analysis: Understand the extent and characteristics of the robot’s workspace

• Grasp planning: Filter candidate grasp positions to only reachable ones

• Robot placement: Optimize where to place the robot for a given task

The ReachabilityMap class uses batch forward kinematics computation, with optional JIT compilation via JAX for high performance.

Basic Usage

import skrobot
from skrobot.coordinates import CascadedCoords
from skrobot.kinematics import ReachabilityMap

# Load robot
robot = skrobot.models.Panda()

# Get kinematic chain
link_list = robot.rarm.link_list
end_coords = robot.rarm.end_coords

# Create and compute reachability map
rmap = ReachabilityMap(robot, link_list, end_coords, voxel_size=0.05)
rmap.compute(n_samples=500000)

# Check reachability
target = [0.5, -0.3, 0.8]
if rmap.is_reachable(target):
    print("Target is reachable!")
    print(f"Manipulability: {rmap.get_manipulability(target):.4f}")

Key Concepts

Voxel Grid

The workspace is discretized into a 3D voxel grid. Each voxel stores:

• Reachability count: Number of configurations that can reach this voxel

• Reachability index: Ratio of reachable orientations (0.0 to 1.0)

• Manipulability: Average manipulability measure for configurations reaching this voxel

# Create map with 5cm voxel size
rmap = ReachabilityMap(robot, link_list, end_coords, voxel_size=0.05)

# Create map with 10cm voxel size (faster, less detailed)
rmap = ReachabilityMap(robot, link_list, end_coords, voxel_size=0.10)

Sampling Methods

Two sampling methods are available:

Random sampling (default): Uniformly samples joint configurations at random.

rmap.compute(n_samples=500000, sampling='random', seed=42)

Grid sampling: Systematically samples joint space on a regular grid. Total samples = bins_per_joint^n_joints.

# For a 7-DOF robot with 10 bins: 10^7 = 10M samples
rmap.compute(sampling='grid', bins_per_joint=10)

Orientation-Aware Reachability

By default, the reachability map tracks orientation diversity using Fibonacci sphere sampling. The Reachability Index measures what fraction of orientations are reachable at each position:

# Track 50 orientation bins (default)
rmap.compute(n_samples=500000, orientation_bins=50)

# Position-only mode (no orientation tracking)
rmap.compute(n_samples=500000, orientation_bins=0)

Backend Selection

The ReachabilityMap supports two backends:

JAX backend (recommended): Uses JIT compilation for fast batch computation.

# Auto-select (uses JAX if available)
rmap = ReachabilityMap(robot, link_list, end_coords, backend=None)

# Explicitly use JAX
rmap = ReachabilityMap(robot, link_list, end_coords, backend='jax')

NumPy backend: Pure NumPy implementation, no additional dependencies.

rmap = ReachabilityMap(robot, link_list, end_coords, backend='numpy')

Performance comparison (approximate):

• JAX: ~1,000,000 FK/sec (with JIT compilation)

• NumPy: ~100,000 FK/sec

Querying the Reachability Map

Single Position Query

position = [0.5, -0.3, 0.8]

# Check if reachable
if rmap.is_reachable(position):
    print("Reachable!")

# Get reachability score (number of configurations)
score = rmap.get_reachability(position)
print(f"Reachability score: {score}")

# Get manipulability
manip = rmap.get_manipulability(position)
print(f"Manipulability: {manip:.4f}")

Batch Position Query

import numpy as np

# Query multiple positions efficiently
positions = np.array([
    [0.5, -0.3, 0.8],
    [0.6, 0.0, 1.0],
    [0.4, 0.2, 0.6],
])
scores = rmap.get_reachability_at_positions(positions)
print(f"Scores: {scores}")

Filtering Targets

# Filter to only reachable positions
candidate_targets = np.random.uniform(-1, 1, (100, 3))
reachable_targets, indices = rmap.filter_reachable_targets(candidate_targets)
print(f"Reachable: {len(reachable_targets)} / {len(candidate_targets)}")

Finding Nearest Reachable Position

# Find nearest reachable position to an unreachable target
unreachable_target = [2.0, 0.0, 1.5]  # Outside workspace
nearest = rmap.find_nearest_reachable(unreachable_target)
if nearest is not None:
    print(f"Nearest reachable: {nearest}")

Visualization

Get Point Cloud for Visualization

# Get point cloud with colors based on reachability index
positions, colors = rmap.get_point_cloud(
    color_by='reachability_index',  # or 'reachability', 'manipulability'
    max_points=50000
)

Using with ViserViewer

import trimesh as tm
import numpy as np

# Get point cloud
positions, colors = rmap.get_point_cloud(color_by='reachability_index')

# Create viewer
viewer = skrobot.viewers.ViserViewer()
viewer.add(robot)

# Create sphere mesh for visualization
unit_sphere = tm.creation.icosphere(subdivisions=1, radius=1.0)
sphere_vertices = unit_sphere.vertices.astype(np.float32)
sphere_faces = unit_sphere.faces.astype(np.uint32)

# Add batched spheres
n_points = len(positions)
sphere_radius = rmap.voxel_size * 0.4
server = viewer._server
server.scene.add_batched_meshes_simple(
    "reachability_spheres",
    vertices=sphere_vertices,
    faces=sphere_faces,
    batched_wxyzs=np.tile([1.0, 0.0, 0.0, 0.0], (n_points, 1)).astype(np.float32),
    batched_positions=positions.astype(np.float32),
    batched_scales=np.full((n_points, 3), sphere_radius, dtype=np.float32),
    batched_colors=(colors[:, :3] * 255).astype(np.uint8),
    opacity=0.5,
)

viewer.show()

Saving and Loading

Reachability maps can be saved to and loaded from .npz files:

# Save
rmap.compute(n_samples=1000000)
rmap.save('reachability_map.npz')

# Load
rmap2 = ReachabilityMap(robot, link_list, end_coords)
rmap2.load('reachability_map.npz')

# Use loaded map
print(f"Reachable volume: {rmap2.reachable_volume:.3f} m³")

Command Line Interface

The demo script provides a command-line interface for quick experiments:

# Basic usage with PR2 robot
python examples/reachability_map_demo.py

# Use Panda robot with more samples
python examples/reachability_map_demo.py --robot panda --samples 1000000

# Use custom URDF
python examples/reachability_map_demo.py --urdf /path/to/robot.urdf

# Color by manipulability
python examples/reachability_map_demo.py --color manipulability

# Save result
python examples/reachability_map_demo.py --save my_rmap.npz

# Load and visualize
python examples/reachability_map_demo.py --load my_rmap.npz

Available options:

Option	Description	Default
--robot	Built-in robot (pr2, panda, fetch, r8, nextage, tycoon)	pr2
--urdf	Path to custom URDF file	None
--backend	Computation backend (jax, numpy)	auto
--samples	Number of random samples	2000000
--sampling	Sampling method (random, grid)	random
--voxel-size	Voxel size in meters	0.05
--color	Coloring mode (reachability_index, reachability, manipulability)	reachability_index
--orientation-bins	Number of orientation bins (0 for position-only)	50
--save	Save reachability map to file	None
--load	Load reachability map from file	None
--z-slice	Show only points within Z range	None
--alpha	Sphere opacity (0.0-1.0)	0.3

Properties and Statistics

# After computing the map
rmap.compute(n_samples=500000)

# Get statistics
print(f"Reachable volume: {rmap.reachable_volume:.3f} m³")
print(f"Reachable voxels: {rmap.n_reachable_voxels}")
print(f"Backend: {rmap.backend}")
print(f"Voxel size: {rmap.voxel_size} m")

# Workspace bounds
print(f"X range: {rmap.bounds['x']}")
print(f"Y range: {rmap.bounds['y']}")
print(f"Z range: {rmap.bounds['z']}")

# Get reachable points for further analysis
points = rmap.get_reachable_points(min_score=5, max_points=10000)

API Reference

Class: ReachabilityMap

skrobot.kinematics.ReachabilityMap(robot_model, link_list, end_coords, voxel_size=0.05, backend=None)

Constructor Parameters:

• robot_model: Robot model instance

• link_list: List of links in the kinematic chain

• end_coords: End-effector coordinates (CascadedCoords)

• voxel_size: Size of each voxel in meters (default: 0.05)

• backend: Backend to use (‘jax’ or ‘numpy’, default: auto-select)

Methods:

• compute(n_samples, seed, verbose, sampling, bins_per_joint, orientation_bins): Compute the reachability map

• is_reachable(position, threshold): Check if a position is reachable

• get_reachability(position): Get reachability score at a position

• get_manipulability(position): Get average manipulability at a position

• get_reachability_at_positions(positions): Batch query for multiple positions

• filter_reachable_targets(positions, min_score): Filter targets to reachable ones

• find_nearest_reachable(position, min_score, max_distance): Find nearest reachable position

• get_reachable_points(min_score, max_points): Get positions of reachable voxels

• get_point_cloud(color_by, max_points): Get point cloud for visualization

• save(filepath): Save reachability map to .npz file

• load(filepath): Load reachability map from .npz file

Properties:

• reachable_volume: Total reachable volume in cubic meters

• n_reachable_voxels: Number of reachable voxels

• backend: Name of the backend being used

• bounds: Workspace bounds dictionary

• reachability: 3D array of reachability counts

• manipulability: 3D array of average manipulability

• reachability_index: 3D array of orientation diversity ratio (0-1)

Related Documentation

• Batch Inverse Kinematics - Batch inverse kinematics for multiple IK solutions

• Inverse Kinematics - IK tutorial

• Robot Model Tips - Robot model usage tips