Optimization-based Planning Architecture and Library for robotic Manipulators (OPALM)

OPALM: A Cross-Layer Benchmark for Motion Planning

A modular benchmark that isolates trajectory representation, collision geometry, execution path, nonlinear step solving, stopping rules, and final validation under one planning-and-measurement contract.

Overview View figures

Overview

A common contract across the planning stack.

OPALM decomposes optimization-based motion planning into planner, collision, execution-path, and step-solver components while preserving common problem setup, initialization, hierarchical timing, and validation semantics.

  • Five planner adapters
    • CHOMP (Covariant Hamiltonian Optimization for Motion Planning), waypoint-based.
    • TrajOpt (trajectory optimization), waypoint-based.
    • GPMP2 (Gaussian Process Motion Planning 2), waypoint-based.
    • FACTO (Function-space Adaptive Constrained Trajectory Optimization for Robotic Manipulators), coefficient-based.
    • DRAFTO (Decoupled Reduced-space and Adaptive Feasibility-repair Trajectory Optimization for Robotic Manipulators), coefficient-based.
  • Three collision checkers
    • PRIM (geometric primitives).
    • SDF (signed distance fields).
    • CAPT (Collision-Affording Point Tree).
  • Two execution paths
    • Scalar: Pinocchio kinematics.
    • SIMD (single instruction, multiple data): AVX (Advanced Vector Extensions) kinematics, SIMD distance queries, and SoA (structure-of-arrays) data handling.
    Both paths use shared work budgets and dense post-solution validation.
OPALM framework showing collision, kinematics, planner, and step solver modules.
Interchangeable components with a shared experimental contract.

Benchmark Design

A matched stack, not a planner-only comparison.

Each paired run holds the robot, scene, initialization, safety margins, work budget, convergence tolerance, timing contract, and a fixed final verifier.

Planner

Five planner adapters

Waypoint, factor-graph, and coefficient-space formulations are compared through native adapters.

Collision

Three collision checkers

PRIM, SDF, and CAPT return shared distance, gradient, and active-state semantics.

Kinematics

Two execution paths

Scalar Pinocchio is compared with AVX (Advanced Vector Extensions) kinematics, SIMD (single instruction, multiple data) queries, and SoA (structure-of-arrays) data handling.

Step Solver

Common step-solver tracks

GN (Gauss-Newton), LM (Levenberg-Marquardt), and Dogleg isolate step-solver behavior on matched DRAFTO and GPMP models.

Task regimes: S/U single-arm task-unconstrained, S/C single-arm task-constrained, D/U dual-arm task-unconstrained, and D/C dual-arm task-constrained.

Reproducibility

Benchmark protocol and implementation settings.

The values below are taken from the scene-crossing runner, active planner YAML (YAML Ain't Markup Language) profiles, collision-sphere model, build configuration, and the manuscript.

Task population

Regime Scenes Tasks per scene Tasks per stack
S/U Bookshelf small, bookshelf tall, bookshelf thin, box, cage, kitchen, table pick, table under pick 100 800
S/C Kitchen, table under pick 100 200
D/U Table dual arm 40 40
D/C Table dual arm 40 40

PRIM excludes the mesh-based kitchen scene, reducing its S/U and S/C populations to 700 and 100 tasks per stack, respectively.

Shared geometry, sampling, and execution

Robot model
Franka Panda; 7 DoF (degrees of freedom) per arm and 14 DoF for dual-arm tasks.
Collision model
26 collision spheres per arm; 52 for the dual-arm model.
Optimization grid
36 intervals: 37 nodes including endpoints and 35 internal collision checkpoints.
Final validation
74 samples for coefficient paths; 109 samples for waypoint paths in the current post-validator.
Safety margin
0.05 m (meters) for environment and self-collision costs.
SDF resolution
0.01 m grid-cell size.
CAPT sampling
0.015 m surface-sample spacing; 0.005 m point radius and deduplication resolution.
Iteration budget
200 outer iterations for every planner.
Timing repetitions
One measured execution per task and stack; no dedicated warm-up pass in the benchmark runner.
Benchmark host
Ubuntu 24.04.4, Intel Core Ultra 7 265K, 62 GiB (gibibytes) RAM (random-access memory); one application process per task.
Compiler and flags
GNU (GNU's Not Unix) C++20 toolchain, Release build with optimization level 3, disabled debug assertions, automatic link-time optimization, and OpenMP (Open Multi-Processing) parallelism: -O3 -DNDEBUG -flto=auto -fopenmp; SIMD units add AVX2 (Advanced Vector Extensions 2) and FMA (fused multiply-add) instructions: -mavx2 -mfma.

Objective weights and stopping tolerances

Planner Objective settings Stopping tolerances Iteration budget
FACTO Smoothness 5e-3; obstacle 1.0 Step 2.5e-3; function 1e-6; feasibility 1e-5 200
DRAFTO Smoothness 5e-3; obstacle 1.0 Step 2.5e-3; function 1e-6. LM/Dogleg overlays: step 1e-3, relative decrease 1e-6 200
CHOMP Smoothness 0.05; obstacle 1.0 Small update 2e-4 200
TrajOpt Smoothness 0.05; obstacle 1.0 Approximate improvement 1e-6; constraint 0.035 200
GPMP2 Obstacle sigma 0.0025; Gaussian-process model 1.0; configuration/velocity priors 1e-4 Relative 1e-4; step 2.5e-3 200

Planner adapters

Planner Decision representation Initialization Native step Common-step track
CHOMPWaypoint configurationsQuintic splineCovariant functional GD (gradient descent)GD
TrajOptWaypoint configurationsLinear interpolationSequential convex procedureSQP (sequential quadratic programming)
GPMP2GP (Gaussian process) support statesConstant-velocity GP priorSparse nonlinear least squaresGN / LM / Dogleg
FACTOFourier sine coefficientsBoundary lift with constant velocityConstrained reduced QP (quadratic program)SQP
DRAFTOFourier sine coefficientsBoundary lift with constant velocityDense nonlinear least squaresGN / LM / Dogleg

Key Results

What the controlled comparisons show.

>92% / >90%

Coefficient planners lead success

FACTO remains above 92% and DRAFTO above 90% across all task regimes and both execution paths.

14.5x

Kernel-level SIMD gain

Median forward-kinematics speedup per iteration; end-to-end gains depend on the dominant pipeline bottleneck.

GN / LM

Solver choice changes outcomes

GN is DRAFTO's fastest default, while LM is more reliable for GPMP and for some dual-arm sampled-distance cases.

Results Figures

Evidence by study.

Cross-scene comparisons, solver ablations, and hierarchical timing inspections from the revised evaluation.

Success rate heatmap for five planners across scalar and SIMD execution paths and four task regimes.
Success rateValidated success across planner, backend, and task regime.SVG (Scalable Vector Graphics)Spec (specification)CSV (comma-separated values)

Planning Scenes

Motion planning scenes.

Franka Panda arm in the bookshelf small single-arm planning scene.
Bookshelf small · S/U
Franka Panda arm in the bookshelf tall single-arm planning scene.
Bookshelf tall · S/U
Franka Panda arm in the bookshelf thin single-arm planning scene.
Bookshelf thin · S/U
Franka Panda arm in the box single-arm planning scene.
Box · S/U
Franka Panda arm in the cage single-arm planning scene.
Cage · S/U
Franka Panda arm in the kitchen constrained planning scene.
Kitchen · S/U, S/C
Franka Panda arm in the table pick planning scene.
Table pick · S/U
Franka Panda arm in the table-under-pick planning scene.
Table under pick · S/U, S/C
Dual Franka Panda arms in the table-under-pick planning scene.
Table dual arm · D/U, D/C