Motion planning¶
coordination_oru.motionplanning.abstract_motion_planner
¶
AbstractMotionPlanner: pluggable re-planning interface.
Ported from Java's AbstractMotionPlanner, trimmed to the surface the
coordinator's callOnePathReplan/doReplanning flow actually calls.
No planner implementation is bundled — breakDeadlocksByReplanning is a
no-op unless a concrete subclass is injected via setMotionPlanner.
Pose
dataclass
¶
x
instance-attribute
¶
y
instance-attribute
¶
theta
instance-attribute
¶
z = math.nan
class-attribute
instance-attribute
¶
roll = math.nan
class-attribute
instance-attribute
¶
pitch = math.nan
class-attribute
instance-attribute
¶
is_3d()
¶
distance_xy(other)
¶
getX()
¶
getY()
¶
getTheta()
¶
distanceTo(other)
¶
interpolate(other, ratio)
¶
Linear interpolation towards other; theta via shortest arc.
PoseSteering
dataclass
¶
AbstractMotionPlanner
¶
Bases: ABC
start = None
instance-attribute
¶
goal = ()
instance-attribute
¶
footprintCoords = None
instance-attribute
¶
pathPS = None
instance-attribute
¶
setFootprint(*coords)
¶
setStart(p)
¶
setGoals(*poses)
¶
getPath()
¶
addObstacles(geoms)
¶
clearObstacles()
¶
getObstacles()
¶
doPlanning()
abstractmethod
¶
Populate self.pathPS; return True iff planning succeeded.
plan()
¶
writeDebugImage()
¶
coordination_oru.motionplanning.hybrid_astar_planner
¶
Built-in Hybrid A* motion planner producing Reeds-Shepp-style car paths.
__all__ = ['HybridAStarPlanner']
module-attribute
¶
_TWO_PI = 2.0 * math.pi
module-attribute
¶
_Key = tuple[int, int, int]
module-attribute
¶
_State = tuple[float, float, float, int]
module-attribute
¶
_Sample = tuple[float, float, float, int]
module-attribute
¶
Pose
dataclass
¶
x
instance-attribute
¶
y
instance-attribute
¶
theta
instance-attribute
¶
z = math.nan
class-attribute
instance-attribute
¶
roll = math.nan
class-attribute
instance-attribute
¶
pitch = math.nan
class-attribute
instance-attribute
¶
is_3d()
¶
distance_xy(other)
¶
getX()
¶
getY()
¶
getTheta()
¶
distanceTo(other)
¶
interpolate(other, ratio)
¶
Linear interpolation towards other; theta via shortest arc.
PoseSteering
dataclass
¶
AbstractMotionPlanner
¶
Bases: ABC
start = None
instance-attribute
¶
goal = ()
instance-attribute
¶
footprintCoords = None
instance-attribute
¶
pathPS = None
instance-attribute
¶
setFootprint(*coords)
¶
setStart(p)
¶
setGoals(*poses)
¶
getPath()
¶
addObstacles(geoms)
¶
clearObstacles()
¶
getObstacles()
¶
doPlanning()
abstractmethod
¶
Populate self.pathPS; return True iff planning succeeded.
plan()
¶
writeDebugImage()
¶
OccupancyMap
¶
A y-up occupancy grid with world<->grid transforms and inflation.
image = image
instance-attribute
¶
resolution = resolution
instance-attribute
¶
origin = origin
instance-attribute
¶
occupied = occupied
instance-attribute
¶
height
property
¶
width
property
¶
bounds
property
¶
World-frame (xmin, ymin, xmax, ymax) of the map.
from_yaml(yaml_path, *, unknown_is_occupied=True)
classmethod
¶
Load a ROS map_server YAML descriptor and its image.
Cells with occupancy probability above occupied_thresh are
occupied; the unknown band between free_thresh and
occupied_thresh counts as occupied unless
unknown_is_occupied=False. Only trinary mode and unrotated
maps (origin yaw 0) are supported.
world_to_grid(x, y)
¶
The (row, col) cell containing world point (x, y).
grid_to_world(row, col)
¶
The world (x, y) of the center of cell (row, col).
in_bounds(row, col)
¶
inflated(radius)
¶
The occupancy grid dilated by radius metres (a disk
structuring element). Cached per pixel radius; callers must
.copy() before writing.
to_png_bytes()
¶
The map as a grayscale 8-bit PNG (image orientation, top row first), encoded with the stdlib only.
HybridAStarPlanner
¶
Bases: AbstractMotionPlanner
Hybrid A* over a ROS-style occupancy grid, car-like Reeds-Shepp model.
The robot is a car with minimum turning radius turning_radius that
may drive forward and in reverse. Collision checking uses the robot's
circumcircle (the smallest origin-centered circle containing the
footprint) against the grid inflated by that radius, so validity of a
state is heading-independent. Start and goal theta are honored:
termination is by a collision-checked analytic Reeds-Shepp expansion
exactly onto the goal pose.
Reverse arcs cost reverse_cost x their length plus
gear_switch_cost per direction change (reverse_cost >= 1 keeps
the Reeds-Shepp heuristic admissible). heuristic_inflation > 1
trades optimality for speed. Output poses carry
PoseSteering.steering == 0.0. Planning is fully deterministic: two
identical plan() calls return identical paths. Pure Python — large
maps (millions of cells) will be slow.
turning_radius = turning_radius
instance-attribute
¶
path_step = path_step
instance-attribute
¶
prim_step = prim_step
instance-attribute
¶
reverse_cost = reverse_cost
instance-attribute
¶
gear_switch_cost = gear_switch_cost
instance-attribute
¶
angle_bins = angle_bins
instance-attribute
¶
heuristic_inflation = heuristic_inflation
instance-attribute
¶
max_expansions = max_expansions
instance-attribute
¶
doPlanning()
¶
_wrap(theta)
¶
Normalize an angle to [-pi, pi).
coordination_oru.motionplanning.occupancy_map
¶
ROS-style occupancy-grid maps (YAML + PGM) for motion planning.
OccupancyMap loads a map_server-style YAML descriptor plus its image,
stores the grid y-up (row index increases with world +y), converts
between world and grid coordinates, inflates obstacles by a robot radius
(cached per radius), and exports a PNG for the web viewer.
__all__ = ['OccupancyMap', 'load_bundled_map']
module-attribute
¶
OccupancyMap
¶
A y-up occupancy grid with world<->grid transforms and inflation.
image = image
instance-attribute
¶
resolution = resolution
instance-attribute
¶
origin = origin
instance-attribute
¶
occupied = occupied
instance-attribute
¶
height
property
¶
width
property
¶
bounds
property
¶
World-frame (xmin, ymin, xmax, ymax) of the map.
from_yaml(yaml_path, *, unknown_is_occupied=True)
classmethod
¶
Load a ROS map_server YAML descriptor and its image.
Cells with occupancy probability above occupied_thresh are
occupied; the unknown band between free_thresh and
occupied_thresh counts as occupied unless
unknown_is_occupied=False. Only trinary mode and unrotated
maps (origin yaw 0) are supported.
world_to_grid(x, y)
¶
The (row, col) cell containing world point (x, y).
grid_to_world(row, col)
¶
The world (x, y) of the center of cell (row, col).
in_bounds(row, col)
¶
inflated(radius)
¶
The occupancy grid dilated by radius metres (a disk
structuring element). Cached per pixel radius; callers must
.copy() before writing.
to_png_bytes()
¶
The map as a grayscale 8-bit PNG (image orientation, top row first), encoded with the stdlib only.
_read_pgm(path)
¶
Read a binary (P5) or ASCII (P2) PGM file as a (height, width)
uint8 array. # comments in the header are honored.
load_bundled_map(name='demo.yaml')
¶
Load a map bundled as package data under coordination_oru/data/maps/.
coordination_oru.motionplanning.reeds_shepp
¶
Closed-form Reeds-Shepp curves: solve, sample, lengths.
The 48-word closed-form solution is ported from PythonRobotics
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
(Copyright (c) 2016 Atsushi Sakai, MIT licence), which itself follows the
word families of Reeds & Shepp (1990) as organized in OMPL: CSC,
CCC, CCCC, CCSC and CCSCC base words, each expanded by the
timeflip, reflect and backwards transforms. The math is kept verbatim;
naming and types are adapted to this codebase. Everything is
deterministic and pure Python.
__all__ = ['RSPath', 'solve', 'sample_path', 'reverse_length']
module-attribute
¶
_ZERO = 1e-10
module-attribute
¶
_TWO_PI = 2.0 * math.pi
module-attribute
¶
_Word = tuple[tuple[float, ...], tuple[str, ...]]
module-attribute
¶
RSPath
dataclass
¶
_mod2pi(x)
¶
Wrap x to [-pi, pi] (the PythonRobotics/OMPL convention).
_polar(x, y)
¶
_LpSpLp(x, y, phi)
¶
_LpSpRp(x, y, phi)
¶
_LpRmL(x, y, phi)
¶
_tau_omega(u, v, xi, eta, phi)
¶
_LpRupLumRm(x, y, phi)
¶
_LpRumLumRp(x, y, phi)
¶
_LpRmSmLm(x, y, phi)
¶
_LpRmSmRm(x, y, phi)
¶
_LpRmSLmRp(x, y, phi)
¶
_all_words(x, y, phi)
¶
Every feasible word for the normalized problem, as (lengths, ctypes) in unit-turning-radius units. Timeflip negates (x, phi) and all lengths; reflect negates (y, phi) and swaps L <-> R; backwards mirrors the problem through the goal frame and reverses the segment order.
solve(q0, q1, turning_radius)
¶
Shortest Reeds-Shepp path q0 -> q1 (poses as (x, y, theta)).
_advance(pose, d, ctype, turning_radius)
¶
Pose after driving signed arc length d metres along one segment.
_wrap(theta)
¶
Normalize an angle to [-pi, pi).
sample_path(q0, path, turning_radius, step)
¶
Poses along the path every step metres of arc length, as
(x, y, theta, gear) with gear +1 forward / -1 reverse. Includes the
start pose and the exact endpoint. theta normalized to [-pi, pi).
reverse_length(path)
¶
Sum of abs(length) over segments with negative length.