tesseract_robotics.motion_planners_ompl¶
OMPL (Open Motion Planning Library) planner bindings.
Overview¶
OMPL provides sampling-based planners for complex environments where optimization-based planners may get stuck in local minima.
from tesseract_robotics.motion_planners_ompl import (
OMPLMotionPlanner,
OMPLMoveProfile, OMPLRealVectorMoveProfile,
RRTConnectConfigurator, RRTstarConfigurator, SBLConfigurator,
)
OMPLMotionPlanner¶
The OMPL motion planner. It derives from MotionPlanner
(motion_planners), so it is accepted wherever a MotionPlanner is.
from tesseract_robotics.motion_planners_ompl import OMPLMotionPlanner
from tesseract_robotics.motion_planners import PlannerRequest
planner = OMPLMotionPlanner("OMPLMotionPlannerTask") # the namespace of its profiles
request = PlannerRequest()
request.env = env
request.instructions = program
request.profiles = profiles
response = planner.solve(request)
Profiles¶
OMPLRealVectorMoveProfile¶
Configure OMPL planning behavior. The profile has three sub-configs:
solver_config, collision_check_config, and contact_manager_config.
from tesseract_robotics.motion_planners_ompl import (
OMPLRealVectorMoveProfile, RRTConnectConfigurator,
)
profile = OMPLRealVectorMoveProfile()
# Solver settings
profile.solver_config.planning_time = 5.0 # seconds
profile.solver_config.simplify = True # simplify path after planning
profile.solver_config.optimize = True # run path optimization
# Add planner configurator(s)
profile.solver_config.addPlanner(RRTConnectConfigurator())
# State-validation resolution (for collision checking)
profile.collision_check_config.longest_valid_segment_length = 0.01
| Attribute | Config | Description |
|---|---|---|
solver_config.planning_time |
solver | Max planning time (seconds) |
solver_config.simplify |
solver | Simplify path after planning |
solver_config.optimize |
solver | Run path optimization |
solver_config.max_solutions |
solver | Cap on returned paths |
collision_check_config.longest_valid_segment_length |
collision | State-validation resolution |
collision_check_config.type |
collision | CollisionEvaluatorType (discrete vs continuous) |
Planner Configurators¶
Each OMPLPlannerType has a configurator class, <type>Configurator. Each
one default-constructs with OMPL's defaults; getType() returns its
OMPLPlannerType. A range of 0 lets OMPL pick the step size.
OMPLPlannerType |
Configurator | Fields |
|---|---|---|
SBL |
SBLConfigurator |
range |
EST |
ESTConfigurator |
range, goal_bias |
LBKPIECE1 |
LBKPIECE1Configurator |
range, border_fraction, min_valid_path_fraction |
BKPIECE1 |
BKPIECE1Configurator |
range, border_fraction, failed_expansion_score_factor, min_valid_path_fraction |
KPIECE1 |
KPIECE1Configurator |
range, goal_bias, border_fraction, failed_expansion_score_factor, min_valid_path_fraction |
BiTRRT |
BiTRRTConfigurator |
range, temp_change_factor, cost_threshold, init_temperature, frontier_threshold, frontier_node_ratio |
RRT |
RRTConfigurator |
range, goal_bias |
RRTConnect |
RRTConnectConfigurator |
range |
RRTstar |
RRTstarConfigurator |
range, goal_bias, delay_collision_checking |
TRRT |
TRRTConfigurator |
range, goal_bias, temp_change_factor, init_temperature, frontier_threshold, frontier_node_ratio |
PRM |
PRMConfigurator |
max_nearest_neighbors |
PRMstar |
PRMstarConfigurator |
none |
LazyPRMstar |
LazyPRMstarConfigurator |
none |
SPARS |
SPARSConfigurator |
max_failures, dense_delta_fraction, sparse_delta_fraction, stretch_factor |
RRTConnect is the default and usually the fastest. The PRM family and the
asymptotically optimal planners (RRTstar, PRMstar) use the whole
planning_time.
from tesseract_robotics.motion_planners_ompl import (
PRMstarConfigurator, RRTstarConfigurator,
)
config = RRTstarConfigurator()
config.goal_bias = 0.1
profile.solver_config.planners = [config, PRMstarConfigurator()]
planning.profiles.create_ompl_planner_configurators builds them by type
name and sets shared fields on the configurators that have them:
from tesseract_robotics.planning.profiles import create_ompl_planner_configurators
profile.solver_config.planners = create_ompl_planner_configurators(
planners=["RRTConnect", "KPIECE1"], num_planners=2, range=0.1
)
create (which builds the OMPL planner from an ompl::base::SpaceInformation)
is not bound: it takes and returns OMPL types.
OMPLSolverConfig.planners¶
solver_config.planners is the list of configurators, one parallel planner
each. The default profile holds two RRTConnectConfigurator. The list is
copied on access; its configurators are shared with the config.
cfg.planners.append(x) changes the copy, not cfg, but
cfg.planners[0].range = 0.5 changes the configurator cfg holds. Assign a
new list, or use addPlanner / clearPlanners, which change it in place. len(cfg.planners) replaces the removed getNumPlanners().
cfg = profile.solver_config
cfg.planners = [RRTConnectConfigurator()] # replace
cfg.addPlanner(SBLConfigurator()) # append in place
assert len(cfg.planners) == 2
OMPLSolverConfig and OMPLRealVectorMoveProfile compare by value and are
unhashable. Upstream compares planners by count only, so two configs that
hold different configurator types of the same number compare equal.
profile.createSolverConfig() returns the solver config the planner uses for
that profile (a copy of solver_config).
Adding Profiles¶
from tesseract_robotics.command_language import ProfileDictionary
from tesseract_robotics.motion_planners_ompl import (
OMPLRealVectorMoveProfile,
RRTConnectConfigurator
)
# Create profile
profile = OMPLRealVectorMoveProfile()
profile.solver_config.planning_time = 10.0
profile.solver_config.addPlanner(RRTConnectConfigurator())
# Add to dictionary (dict, namespace, profile_name, profile)
profiles = ProfileDictionary()
profiles.addProfile("OMPLMotionPlannerTask", "DEFAULT", profile)
# Use with planner request
request.profiles = profiles
ProfileDictionary.hasProfile / getProfile take a profile key.
OMPLMoveProfile.createKey(), inherited by every OMPL move profile, returns
it without building a profile:
assert profiles.hasProfile(
OMPLRealVectorMoveProfile.createKey(), "OMPLMotionPlannerTask", "DEFAULT"
)
Complete Example¶
from tesseract_robotics.motion_planners_ompl import (
OMPLMotionPlanner, OMPLRealVectorMoveProfile,
RRTConnectConfigurator,
)
from tesseract_robotics.motion_planners import PlannerRequest
from tesseract_robotics.command_language import ProfileDictionary
# Configure OMPL profile
profile = OMPLRealVectorMoveProfile()
profile.solver_config.planning_time = 5.0
profile.solver_config.simplify = True
profile.solver_config.addPlanner(RRTConnectConfigurator())
profiles = ProfileDictionary()
profiles.addProfile("OMPLMotionPlannerTask", "DEFAULT", profile)
# Create request
request = PlannerRequest()
request.env = env
request.instructions = program
request.profiles = profiles
# Solve
planner = OMPLMotionPlanner("OMPLMotionPlannerTask") # the namespace of its profiles
response = planner.solve(request)
if response.successful:
trajectory = response.results
print(f"Found path with {len(trajectory)} waypoints")
else:
print(f"Planning failed: {response.message}")
Tips¶
- RRTConnect is usually the best starting point - fast and reliable
- Increase planning_time for complex environments
- Use simplify=True to reduce waypoints
- Lower collision resolution (
longest_valid_segment_length) for narrow passages - OMPL finds paths, not smooth trajectories - use TrajOpt for smoothing
Reference¶
tesseract_motion_planners_ompl Python bindings
OMPLObjectiveReturnTypeError
¶
Bases: TypeError
OMPLStateCostError
¶
Bases: TypeError
OMPLStateSpaceTypeError
¶
Bases: TypeError
SpaceInformation
¶
The planning problem's OMPL space information (opaque). The objectives are built on it.
Cost
¶
OptimizationObjective
¶
Base of the OMPL optimization objectives. The planner's optimize loop ends when isSatisfied(cost of the best path) is true, or at planning_time.
PathLengthOptimizationObjective
¶
MaximizeMinClearanceObjective
¶
Bases: OptimizationObjective
Maximise the smallest clearance the state validity checker reports along the path. tesseract's OMPL state validators do not report clearance (OMPL's default, 0, for every state), so on this stack the term is constant.
MechanicalWorkOptimizationObjective
¶
StateCostIntegralObjective
¶
Bases: OptimizationObjective
Integral of a per-state cost along the path. Subclass it and override stateCost(self, values) -> float, where values holds the state's joint positions (rad or m per joint); the planner's thread takes the GIL for every call. Without an override the cost of every state is 1, so the integral is path length.
MultiOptimizationObjective
¶
OMPLSolverConfig
¶
planning_time
property
writable
¶
Max planning time allowed in seconds (default: 5.0)
max_solutions
property
writable
¶
Max number of solutions to find before exiting (default: 10)
simplify
property
writable
¶
Simplify trajectory (default: false). Ignores n_output_states if true.
simplify_time
property
writable
¶
Max time in seconds to spend simplifying when simplify is true (default: 0.0). <= 0 runs simplification to completion (unbounded); > 0 time-bounds it.
optimize
property
writable
¶
Use all planning time to optimize trajectory (default: true)
planners
property
writable
¶
Planner configurators, one parallel planner each. The list is copied on access; its configurators are shared with the config, so editing one edits the config. Assign a new list (or use addPlanner/clearPlanners) to change which planners it holds.
addPlanner
¶
Add a planner configurator (each runs in parallel)
OMPLPlannerType
¶
Bases: Enum
SBLConfigurator
¶
Bases: OMPLPlannerConfigurator
ESTConfigurator
¶
Bases: OMPLPlannerConfigurator
LBKPIECE1Configurator
¶
Bases: OMPLPlannerConfigurator
BKPIECE1Configurator
¶
Bases: OMPLPlannerConfigurator
KPIECE1Configurator
¶
Bases: OMPLPlannerConfigurator
BiTRRTConfigurator
¶
Bases: OMPLPlannerConfigurator
RRTConfigurator
¶
Bases: OMPLPlannerConfigurator
RRTConnectConfigurator
¶
Bases: OMPLPlannerConfigurator
RRTstarConfigurator
¶
Bases: OMPLPlannerConfigurator
TRRTConfigurator
¶
Bases: OMPLPlannerConfigurator
PRMConfigurator
¶
Bases: OMPLPlannerConfigurator
PRMstarConfigurator
¶
Bases: OMPLPlannerConfigurator
LazyPRMstarConfigurator
¶
Bases: OMPLPlannerConfigurator
SPARSConfigurator
¶
Bases: OMPLPlannerConfigurator
OMPLMoveProfile
¶
OMPLRealVectorMoveProfile
¶
Bases: OMPLMoveProfile
solver_config
property
writable
¶
The OMPL parallel planner solver config
contact_manager_config
property
writable
¶
The contact manager configuration (collision margins)
collision_check_config
property
writable
¶
The collision check configuration
from_yaml
staticmethod
¶
Construct the profile from its YAML config text; malformed YAML raises ValueError.
Nested sub-configs are replaced, not merged: a key naming one rebuilds it from that struct's own defaults, so fields left out lose the profile's values. This is per struct; here solver_config: starts with no planners (OMPLSolverConfig() holds none).
OMPLMotionPlanner
¶
Bases: MotionPlanner
RNG_setSeed
¶
Seed OMPL's global RNG seed generator for reproducible planning.
Samplers created after this call draw deterministic seeds, so repeated solve() calls produce identical paths. Re-seeding after planning has started logs an OMPL error (harmless: only pre-existing RNG instances stay nondeterministic). A seed of 0 is rejected once generation has started.
RNG_getSeed
¶
Get the first seed used by OMPL's RNG seed generator.
Log this value to make a stochastic planning failure reproducible by passing it to RNG_setSeed in a subsequent run.