OMPL examples¶
OMPL finds collision-free paths by sampling. From Python you choose the planners, how
long they optimize and the objective that decides when a path is good enough. On a sweep
through six cubes, a Python objective with a cost threshold ends the optimize loop at the
first path that meets it: in one run of 10 seeds the whole plan took 1.27 to 2.26 s, against
12.11 to 13.83 s for the default objective, which optimizes until planning_time runs out. On this
scene the first path OMPL finds usually meets the threshold already.
All examples install as console scripts:
tesseract_freespace_clearance_objective_example
# or as a module
pixi run python -m tesseract_robotics.examples.freespace_clearance_objective_example
Freespace OMPL¶
freespace_ompl_example, tesseract_robotics 0.35.0.9.dev554
Basic OMPL planning between joint configurations using the KUKA LBR iiwa 14 R820. The arm swings
joint_a1 from -0.4 to 0.4 rad around one obstacle, a 0.15 m sphere at (0.5, 0, 0.55) m, as in
tesseract_planning's C++ freespace_ompl_example.cpp. The six cubes are the scene of
the cost-threshold example below.
See the source at
freespace_ompl_example.py.
Hybrid planning (OMPL + TrajOpt)¶
freespace_hybrid_example, tesseract_robotics 0.35.0.9.dev554
OMPL finds a feasible path, TrajOpt smooths and optimizes it:
See the source at
freespace_hybrid_example.py.
Ending the optimize loop on cost¶
The KUKA IIWA sweeps joint_a1 from -1.2 to 1.2 rad, a 1.25 m arc of the tool, through six
cubes of different sizes and orientations. The straight joint-space sweep hits them from
5 % to 92 % of the way. The example plans the sweep twice with FreespaceIfoptPipeline
(two RRTConnect planners, then TrajOptIfopt), with no solution-count exit:
- Default objective: path length,
optimize = True. Nothing but the time cap ends the optimize loop, so OMPL runs for the wholeplanning_time. - Python objective: an
OMPLRealVectorMoveProfilesubclass builds aMultiOptimizationObjective(path length x 1 + max-min clearance x 0.5) with a cost threshold. The loop ends at the first path whose cost meets it. Here that is usually the first path OMPL finds. With one planner such a path is the oneoptimize = Falsereturns; with the example's two, see the note below.
The viewer shows case 2: the TCP path and, for each cube, a red line between the nearest points of the robot and the cube, labelled with the distance.
flowchart TD
A[planning_time starts] --> B[RRTConnect finds a path]
B --> C{"objective.isSatisfied(cost)?"}
C -->|"yes: cost <= threshold"| D[OMPL returns the path]
C -->|no| E{time left?}
E -->|yes| B
E -->|no| D
D --> F[TrajOptIfopt smooths it]
classDef runtime fill:#ffaa00,stroke:#b37700,color:#000
class C,E runtime
The highlighted checks run on every path OMPL finds. With the default objective there is
no threshold, so isSatisfied stays false and only the time check ends the loop.
The objective, from Python¶
class LengthAndClearanceProfile(OMPLRealVectorMoveProfile):
"""OMPL move profile: path length x 1 + max-min clearance x w, with a cost threshold.
Args:
cost_threshold: The optimize loop ends at the first path with a cost at or below it.
"""
def __init__(self, cost_threshold: float) -> None:
super().__init__()
self.cost_threshold = cost_threshold
self.objective_calls = 0
def createOptimizationObjective(self, si, env, manip):
"""Build the weighted objective the planner's optimize loop checks.
Args:
si: The problem's OMPL `SpaceInformation`.
env: The planning environment.
manip: The planning `JointGroup`.
Returns:
The `MultiOptimizationObjective`.
"""
self.objective_calls += 1
objective = MultiOptimizationObjective(si)
objective.addObjective(PathLengthOptimizationObjective(si), 1.0)
objective.addObjective(MaximizeMinClearanceObjective(si), CLEARANCE_WEIGHT)
objective.setCostThreshold(Cost(self.cost_threshold))
return objective
The planner calls createOptimizationObjective once per solve, with the problem's
SpaceInformation. Objectives are bound but cannot be subclassed in Python: a profile
composes the bound ones. See OMPL optimization objectives
for the bound API.
The example checks itself¶
run() raises a named error when one of its claims fails, so the test only runs it:
# Case 1 has no exit but the time cap; case 2's threshold ends it, so its OMPL node returns
# inside the planning_time that case 1's OMPL node uses up.
check_loop_exits(cases["default"]["result"], cases["threshold"]["result"], planning_time_s)
cases["clearances"] = closest_approaches(
cases["threshold"]["robot"], cases["threshold"]["result"]
)
for clearance in cases["clearances"]:
print(f"{clearance.cube}: {clearance.distance_m * MM_PER_M:.1f} mm from {clearance.link}")
touching = [c.cube for c in cases["clearances"] if c.distance_m <= 0]
if touching:
raise CubeContactError(f"case 2's path touches {touching}")
def check_loop_exits(
default: PlanningResult, threshold: PlanningResult, planning_time_s: float
) -> None:
"""Check each case's optimize loop against its cap, from the OMPL node's own time.
The loop's cap is wall clock, so a node that returned inside `planning_time_s` stopped before
the cap, whatever the machine's load. Load only lengthens a node's time: it cannot raise
`DefaultEndedEarlyError`, and raises `ThresholdDidNotEndLoopError` only if it pushes case 2's
OMPL node itself past the cap. The rest of the pipeline does not count.
A default-case pass is only a lower bound: the node's time also holds setup and whatever load
adds, so a loop that stopped early, plus setup or load reaching the cap, passes as well.
Args:
default: Case 1's plan; nothing but the cap ends its loop.
threshold: Case 2's plan; its cost threshold ends the loop.
planning_time_s: OMPL `planning_time` of both cases (s).
Raises:
NodeInfoMissingError: a plan has not exactly one OMPL node info.
DefaultEndedEarlyError: case 1's OMPL node took less than `planning_time_s`.
ThresholdDidNotEndLoopError: case 2's OMPL node took `planning_time_s` or longer.
"""
default_s = node_info(default, OMPL_NODE).elapsed_time
if default_s < planning_time_s:
raise DefaultEndedEarlyError(
f"default case's OMPL node took {default_s:.2f} s < planning_time {planning_time_s} s"
)
threshold_s = node_info(threshold, OMPL_NODE).elapsed_time
if threshold_s >= planning_time_s:
raise ThresholdDidNotEndLoopError(
f"threshold case's OMPL node took {threshold_s:.2f} s "
f">= planning_time {planning_time_s} s"
)
| Error | Raised when |
|---|---|
PlanningFailedError |
a case did not plan |
ObjectiveNotUsedError |
the Python objective was not built exactly once |
DefaultEndedEarlyError |
case 1's OMPL node took less than planning_time |
ThresholdDidNotEndLoopError |
case 2's OMPL node took planning_time or longer |
CubeContactError |
case 2's path touches a cube |
The clearances come from a scan of case 2's path in 0.005 rad joint steps
(closest_approaches). The viewer draws them with draw_clearances, which the demo
recording calls too:
class LabelPlacementError(RuntimeError):
"""A label still overlaps another after rising `LABEL_MAX_RAISE_M`."""
@dataclass(frozen=True)
class LabelView:
"""A pinhole view along which labels must not overlap, world frame (m, rad), z up."""
position_m: tuple[float, float, float]
look_at_m: tuple[float, float, float]
fov_rad: float
def image_xy(self, point_m: np.ndarray) -> np.ndarray:
"""`point_m` on the image plane at unit depth: (right, up), in units of depth."""
forward = np.subtract(self.look_at_m, self.position_m)
forward = forward / np.linalg.norm(forward)
right = np.cross(forward, (0.0, 0.0, 1.0))
right = right / np.linalg.norm(right)
up = np.cross(right, forward)
ray = np.asarray(point_m) - np.asarray(self.position_m)
depth = ray @ forward
return np.array([ray @ right, ray @ up]) / depth
def label_size(self) -> np.ndarray:
"""A label's (width, height) on the unit-depth image plane."""
view_height = 2.0 * np.tan(self.fov_rad / 2.0)
return view_height * np.array([LABEL_WIDTH_FRACTION, LABEL_HEIGHT_FRACTION])
def overlap(self, a_m: np.ndarray, b_m: np.ndarray) -> bool:
"""Whether labels anchored at `a_m` and `b_m` overlap in this view."""
gap = np.abs(self.image_xy(a_m) - self.image_xy(b_m))
return bool(np.all(gap < self.label_size()))
DEFAULT_VIEW = LabelView(VIEW_POSITION_M, VIEW_LOOK_AT_M, VIEW_FOV_RAD)
def label_anchors(clearances: list[Clearance], view: LabelView = DEFAULT_VIEW) -> list[np.ndarray]:
"""Where each distance label goes: at its cube's nearest point, raised clear of the others.
Labels keep their pixel size, so whether two overlap depends on the view, not on their
distance in space: cubes behind one another along the line of sight put their labels on
top of each other however far apart they are. In `clearances` order, a label starts at its
cube's nearest point and rises in `LABEL_RAISE_STEP_M` steps until it overlaps no label
placed before it in `view`. Rising keeps it above the end of its own line.
Args:
clearances: From `closest_approaches`.
view: The view to keep the labels apart in.
Returns:
One world-frame anchor (m) per clearance, in the same order.
Raises:
LabelPlacementError: a label overlaps another after rising `LABEL_MAX_RAISE_M`.
"""
step = np.array([0.0, 0.0, LABEL_RAISE_STEP_M])
anchors: list[np.ndarray] = []
for clearance in clearances:
start = np.array(clearance.cube_point_m, dtype=np.float64)
anchor = start
while any(view.overlap(anchor, placed) for placed in anchors):
anchor = anchor + step
if anchor[2] - start[2] > LABEL_MAX_RAISE_M:
raise LabelPlacementError(
f"{clearance.cube}: label still overlaps after rising {LABEL_MAX_RAISE_M} m"
)
anchors.append(anchor)
return anchors
def draw_clearances(viewer, clearances: list[Clearance]) -> None:
"""Mark each cube's closest approach: a line between the nearest points, labelled in mm at the cube.
Labels that would overlap in `DEFAULT_VIEW` are stacked apart (`label_anchors`).
Args:
viewer: The `TesseractViewer` showing the scene.
clearances: From `closest_approaches`.
"""
for clearance, anchor in zip(clearances, label_anchors(clearances), strict=True):
segment = np.array([[clearance.robot_point_m, clearance.cube_point_m]])
viewer.server.scene.add_line_segments(
f"/clearance/{clearance.cube}",
segment,
CLEARANCE_COLOR_RGB,
thickness=CLEARANCE_LINE_THICKNESS_M,
)
viewer.server.scene.add_label(
f"/clearance/{clearance.cube}/label",
f"{clearance.distance_m * MM_PER_M:.0f} mm",
position=anchor,
)
Evidence¶
| Claim | Evidence | Source |
|---|---|---|
| Default objective runs to the time cap | 12.11 to 13.83 s end to end, OMPL 10.81 to 11.98 s, at planning_time = 10 (one run of 10 seeds) |
Per seed table |
| The threshold ends the loop early | Same run, on a loaded machine: OMPL 0.04 to 0.07 s against 10.81 to 11.98 s; end to end 1.27 to 2.26 s against 12.11 to 13.83 s | Per seed table |
| OMPL's first path usually meets 4.0 rad already | One planner, 5 seeds, identical over 3 runs: first path 3.57 to 4.21 rad. The 4 that meet the threshold come back unchanged (same length and state count as optimize = False, 0.04 to 0.05 s in those runs). Seed 2's first path is 4.21 rad; re-solving found a 3.95 rad path in 0.24 s |
pixi run ompl-first-path-probe |
| OMPL is a small part of case 2's time | OMPL took 0.04 to 0.07 s of the 1.27 to 2.26 s in that run. The rest is the pipeline's other tasks, among them TrajOptIfopt, MinLengthTask and DiscreteContactCheckTask; none has its own timing here |
Per seed table |
| The straight sweep is blocked | collides from 5 % to 92 % of the way (41 samples) | scene probe when the cubes were placed |
| Case 2's final path clears every cube | Smallest distance to any cube 31.0 to 51.0 mm (threshold rows, one run of 10 seeds). One direct run(): 35 to 151 mm per cube, each cube closest to link_7 |
Per seed table; run() printout; CubeContactError check in run() |
The clearance term adds nothing
tesseract's OMPL state validators do not implement clearance(), so OMPL reports 0 for
every state and MaximizeMinClearanceObjective adds a constant. The objective reduces
to path length. The red lines in the viewer are measured afterwards by the contact
manager; the planner does not see them.
Under verification
The recording above awaits review and is not yet published.
The threshold rarely has to wait for a shorter path here
With the example's two planners OMPL stopped after 0.04 to 0.07 s on all 10 seeds of the
per-seed run, because a first path usually meets 4.0 rad already. With one
planner, which reproduces on a seed, 4 of 5 seeds end at the first solution, the path
optimize = False returns; on seed 2 the first path is 4.21 rad, and the planner re-solved
and found a 3.95 rad path, which ended the loop. With the two planners (seeds 1 to 3, both modes) 1 of 42 runs
differed from its seed's usual path: one optimize = False run on seed 3 was longer,
3.64 rad against 3.57 rad. The planners' race is the inferred cause; 36 repeats did not
reproduce it. pixi run ompl-first-path-probe plans with one planner, so it does not
cover this case.
Per seed¶
In one run of 10 seeds, the threshold ended OMPL in 0.04 to 0.07 s against 10.81 to
11.98 s for the default objective, and TrajOptIfopt brought both cases' final paths to
26.7 to 51.0 mm from the cubes. pixi run ompl-seed-report plans both cases once per seed, each seed in its own
process, and writes JSON and this table to build/ompl_seed_report/.
It plans with FreespaceIfoptOMPLOutputPipeline (scripts/ompl_seed_report_task_composer.yaml).
That is stock FreespaceIfoptPipeline except for one key: OMPL hands its program to
TrajOptIfopt under ompl_output_data instead of output_data, which TrajOptIfopt overwrote,
so one run yields both stages' paths. A test parses both configs: in the two plugins the variant
copies, only that key and the task names differ, and the header matches except search_paths.
On a fixed seed with one planner its final program is bit-identical to stock's
(test_ompl_output_variant_is_bit_identical_to_stock).
flowchart LR
M[MinLengthTask] -->|output_data| O[OMPLMotionPlannerTask]
O -->|"ompl_output_data (stock: output_data)"| T[TrajOptIfoptMotionPlannerTask]
T -->|output_data| C[DiscreteContactCheckTask]
O -.->|"read after the run: OMPL wall time, OMPL path"| R[ompl_seed_report.py]
T -.->|"final path, SQP iterations"| R
classDef runtime fill:#ffaa00,stroke:#b37700,color:#000
class O,T runtime
Both cases plan OMPL at OMPL_CONTACT_MARGIN_M (10 mm), so they differ only in the
objective. Clearances are each path's smallest distance to any cube, from the example's
0.005 rad scan; the final length is joint-space (sum of the per-step L2 norms). The table
below is build/ompl_seed_report/table.md, pasted verbatim apart from its header line's
commit, from Apple M1 Max.
Timings are load-sensitive evidence: the machine was shared with other timing probes, and
uptime read load averages 11.81 / 46.39 / 47.36 (1 / 5 / 15 min) at the start of the run
and 19.33 / 35.55 / 42.70 at the end (2:47 wall).
The run was on an uncommitted tree from before this change landed, so no commit reproduces it exactly. Since that run the example's checks and comments have changed; its planning settings have not.
N = 10 seeds, Darwin arm64, Python 3.14.10, pipeline FreespaceIfoptOMPLOutputPipeline.
| seed | case | OMPL (s) | OMPL min clearance (mm) | TrajOptIfopt iterations | TrajOptIfopt status | final min clearance (mm) | final length (rad) | total (s) |
|---|---|---|---|---|---|---|---|---|
| 1 | default | 11.42 | 10.0 | 2 | Found valid solution | 51.0 | 2.81 | 13.48 |
| 1 | threshold | 0.05 | 10.0 | 5 | Found valid solution | 33.0 | 2.75 | 1.99 |
| 2 | default | 11.98 | 10.1 | 2 | Found valid solution | 39.9 | 2.88 | 13.74 |
| 2 | threshold | 0.06 | 10.0 | 3 | Found valid solution | 51.0 | 2.89 | 1.74 |
| 3 | default | 11.02 | 10.0 | 2 | Found valid solution | 26.7 | 2.79 | 12.34 |
| 3 | threshold | 0.06 | 10.0 | 6 | Found valid solution | 33.6 | 2.76 | 1.69 |
| 4 | default | 11.57 | 10.0 | 4 | Found valid solution | 29.8 | 2.76 | 13.83 |
| 4 | threshold | 0.06 | 10.0 | 4 | Found valid solution | 43.3 | 2.85 | 2.26 |
| 5 | default | 10.97 | 10.1 | 2 | Found valid solution | 51.0 | 2.84 | 12.30 |
| 5 | threshold | 0.05 | 10.0 | 4 | Found valid solution | 35.6 | 2.76 | 1.67 |
| 6 | default | 11.65 | 10.0 | 5 | Found valid solution | 29.1 | 2.82 | 13.16 |
| 6 | threshold | 0.04 | 10.0 | 2 | Found valid solution | 51.0 | 2.81 | 1.69 |
| 7 | default | 10.84 | 10.0 | 5 | Found valid solution | 39.6 | 2.91 | 12.15 |
| 7 | threshold | 0.07 | 10.0 | 6 | Found valid solution | 40.2 | 2.79 | 1.60 |
| 8 | default | 10.95 | 10.0 | 4 | Found valid solution | 43.1 | 2.79 | 12.64 |
| 8 | threshold | 0.05 | 10.0 | 4 | Found valid solution | 45.0 | 2.81 | 1.57 |
| 9 | default | 10.81 | 10.0 | 5 | Found valid solution | 30.1 | 2.74 | 12.11 |
| 9 | threshold | 0.07 | 10.0 | 3 | Found valid solution | 31.0 | 2.72 | 1.42 |
| 10 | default | 10.95 | 10.0 | 5 | Found valid solution | 32.2 | 2.75 | 12.28 |
| 10 | threshold | 0.06 | 10.0 | 3 | Found valid solution | 51.0 | 2.85 | 1.27 |
| Claim | Evidence | Source |
|---|---|---|
| The default objective's OMPL stage runs to the time cap | 10.81 to 11.98 s at planning_time = 10, all 10 seeds of this run |
table, OMPL (s) |
| The threshold ends OMPL early | 0.04 to 0.07 s, all 10 seeds of this run | table, OMPL (s) |
| TrajOptIfopt took 2 to 6 SQP iterations in this run | default 2 to 5, threshold 2 to 6; highest overall_iteration its callback saw |
table |
| Both cases ended 26.7 to 51.0 mm from the cubes in this run | final min clearance; final length 2.72 to 2.91 rad | table |
| Both cases' OMPL paths sit at the 10 mm collision margin | default 10.0 to 10.1 mm, threshold 10.0 mm (0.0007 to 0.094 mm before the margin fix) | table, OMPL min clearance |
| The report's config differs from stock only in the hand-off | in the two plugins it copies, parsed configs differ in OMPL's output key, TrajOptIfopt's input key and the task names; the header matches except search_paths |
tests/scripts/test_ompl_seed_report.py |
| A run of the report's pipeline keeps the OMPL program | a run keeps an OMPL program under the key, one state per final state, and stock keeps none | tests/scripts/test_ompl_seed_report.py |
| The report's pipeline plans what stock plans | final program bit-identical to stock FreespaceIfoptPipeline's on seed 1, one planner; stock reproduces itself on the seed |
tests/scripts/test_ompl_seed_report.py |
One run is not a bound
A second run of 10 seeds minutes earlier, not pasted here, went outside these ranges: threshold OMPL up to 0.09 s and total up to 2.83 s; default total up to 14.51 s and up to 7 SQP iterations. Final clearance on a seed also moved between the runs (threshold seed 1: 33.0 mm against 37.0 mm).
The table does not say why OMPL's loop ended
tesseract's planner reports only "Found valid solution". The OMPL time column is what tells the two exits apart.
Further reading¶
- Optimal planning: OMPL's overview of optimizing planners and termination conditions.
- Optimization objectives tutorial: defining objectives, combining them, and cost thresholds.
OptimizationObjective:isSatisfiedandsetCostThreshold.MultiOptimizationObjective: the weighted sum the example builds.PathLengthOptimizationObjectiveandMaximizeMinClearanceObjective: its two terms.