Motion Planning

Motion Planner

MotionPlanner(robot_name: str, config: PlannerConfig | None = None, pointcloud: np.ndarray | None = None, base_config: np.ndarray | None = None, constraints: list | None = None, costs: list | None = None)

Motion planner using OMPL + VAMP C++ backend.

All internals are private. The public API accepts and returns only numpy arrays.

For subgroup planners the helper methods :meth:extract_config, :meth:embed_config, and :meth:embed_path convert between the reduced DOF space used by the planner and the full 24-DOF body configuration.

Source code in autolife_planning/planning/motion_planner.py

def __init__(
    self,
    robot_name: str,
    config: PlannerConfig | None = None,
    pointcloud: np.ndarray | None = None,
    base_config: np.ndarray | None = None,
    constraints: list | None = None,
    costs: list | None = None,
) -> None:
    from autolife_planning._ompl_vamp import OmplVampPlanner
    from autolife_planning.autolife import (
        HOME_JOINTS,
        PLANNING_SUBGROUPS,
        autolife_robot_config,
    )

    if config is None:
        config = PlannerConfig()

    self._config = config
    self._robot_name = robot_name

    # Frozen 24-DOF joint values for any joint not controlled by
    # this planner.  Defaults to HOME_JOINTS, but the caller can
    # pass any 24-DOF array — e.g. the live config from the env —
    # so the inactive joints are pinned wherever they currently are.
    if base_config is None:
        base_config = HOME_JOINTS
    self._base_config = np.asarray(base_config, dtype=np.float64).copy()
    if self._base_config.shape != HOME_JOINTS.shape:
        raise ValueError(
            f"base_config must have shape {HOME_JOINTS.shape}, "
            f"got {self._base_config.shape}"
        )

    full_names = autolife_robot_config.joint_names
    sg = PLANNING_SUBGROUPS.get(robot_name)

    if sg is None:
        # Full-body planner
        self._planner = OmplVampPlanner()
        self._joint_names = list(full_names)
        self._subgroup_indices = None
    else:
        # Subgroup planner — frozen joints come from the supplied
        # base_config; the C++ checker injects them around the
        # active subset before every collision query.
        sg_joint_names = sg["joints"]
        active_indices = [full_names.index(j) for j in sg_joint_names]
        self._planner = OmplVampPlanner(active_indices, self._base_config.tolist())
        self._joint_names = list(sg_joint_names)
        self._subgroup_indices = np.array(active_indices)

    self._ndof = self._planner.dimension()

    if pointcloud is not None:
        self.add_pointcloud(pointcloud)

    if constraints:
        self._push_constraints(constraints)

    if costs:
        self._push_costs(costs)

bounds: tuple[np.ndarray, np.ndarray] property

(lower, upper) joint limits of the active DOF.

joint_names: list[str] property

Joint names controlled by this planner, in DOF order.

is_subgroup: bool property

True if this planner controls a subset of the full body.

subgroup_indices: np.ndarray | None property

Indices of this planner's joints in the full 24-DOF config.

base_config: np.ndarray property

The 24-DOF stance frozen for joints outside this subgroup.

has_pointcloud: bool property

True if a pointcloud is currently registered.

clear_constraints() -> None

Remove all constraints from the planner.

Source code in autolife_planning/planning/motion_planner.py

def clear_constraints(self) -> None:
    """Remove all constraints from the planner."""
    self._planner.clear_constraints()

set_constraints(constraints: list) -> None

Replace all constraints: clear existing, then push new ones.

Source code in autolife_planning/planning/motion_planner.py

def set_constraints(self, constraints: list) -> None:
    """Replace all constraints: clear existing, then push new ones."""
    self.clear_constraints()
    self._push_constraints(constraints)

clear_costs() -> None

Remove all costs from the planner (falls back to path length).

Source code in autolife_planning/planning/motion_planner.py

def clear_costs(self) -> None:
    """Remove all costs from the planner (falls back to path length)."""
    self._planner.clear_costs()

set_costs(costs: list) -> None

Replace all costs: clear existing, then push new ones.

Source code in autolife_planning/planning/motion_planner.py

def set_costs(self, costs: list) -> None:
    """Replace all costs: clear existing, then push new ones."""
    self.clear_costs()
    self._push_costs(costs)

add_pointcloud(pointcloud: np.ndarray) -> None

Set the scene pointcloud, replacing any previously-registered cloud.

Parameters:

Name	Type	Description	Default
pointcloud	ndarray	(N, 3) array of obstacle positions in world frame. Uses config.point_radius as the per-point inflation radius.	required

Source code in autolife_planning/planning/motion_planner.py

def add_pointcloud(self, pointcloud: np.ndarray) -> None:
    """Set the scene pointcloud, replacing any previously-registered cloud.

    Args:
        pointcloud: ``(N, 3)`` array of obstacle positions in world
            frame.  Uses ``config.point_radius`` as the per-point
            inflation radius.
    """
    self._planner.add_pointcloud(
        np.asarray(pointcloud, dtype=np.float32), self._config.point_radius
    )

remove_pointcloud() -> bool

Drop the currently-registered pointcloud.

Returns False if there was no cloud to remove.

Source code in autolife_planning/planning/motion_planner.py

def remove_pointcloud(self) -> bool:
    """Drop the currently-registered pointcloud.

    Returns ``False`` if there was no cloud to remove.
    """
    return self._planner.remove_pointcloud()

filter_pointcloud(pointcloud: np.ndarray, min_dist: float, max_range: float, origin: np.ndarray | list[float], workspace_min: np.ndarray | list[float], workspace_max: np.ndarray | list[float], cull: bool = True) -> np.ndarray

Spatially downsample a point cloud via Morton-curve sorting.

Keeps one representative point per min_dist neighbourhood and discards points farther than max_range from origin or outside the [workspace_min, workspace_max] bounding box.

Parameters:

Name	Type	Description	Default
pointcloud	ndarray	(N, 3) array of 3-D points.	required
min_dist	float	Minimum distance between two retained points.	required
max_range	float	Maximum distance from origin to keep a point.	required
origin	ndarray | list[float]	(3,) reference position for range culling.	required
workspace_min	ndarray | list[float]	(3,) lower corner of the workspace AABB.	required
workspace_max	ndarray | list[float]	(3,) upper corner of the workspace AABB.	required
cull	bool	If True (default), apply range and AABB culling.	True

Returns:

Type	Description
ndarray	(M, 3) filtered point cloud with M <= N.

Source code in autolife_planning/planning/motion_planner.py

def filter_pointcloud(
    self,
    pointcloud: np.ndarray,
    min_dist: float,
    max_range: float,
    origin: np.ndarray | list[float],
    workspace_min: np.ndarray | list[float],
    workspace_max: np.ndarray | list[float],
    cull: bool = True,
) -> np.ndarray:
    """Spatially downsample a point cloud via Morton-curve sorting.

    Keeps one representative point per ``min_dist`` neighbourhood and
    discards points farther than ``max_range`` from ``origin`` or
    outside the ``[workspace_min, workspace_max]`` bounding box.

    Args:
        pointcloud: ``(N, 3)`` array of 3-D points.
        min_dist: Minimum distance between two retained points.
        max_range: Maximum distance from ``origin`` to keep a point.
        origin: ``(3,)`` reference position for range culling.
        workspace_min: ``(3,)`` lower corner of the workspace AABB.
        workspace_max: ``(3,)`` upper corner of the workspace AABB.
        cull: If ``True`` (default), apply range and AABB culling.

    Returns:
        ``(M, 3)`` filtered point cloud with ``M <= N``.
    """
    return self._planner.filter_pointcloud(
        np.asarray(pointcloud, dtype=np.float32),
        float(min_dist),
        float(max_range),
        [float(x) for x in origin],
        [float(x) for x in workspace_min],
        [float(x) for x in workspace_max],
        cull,
    )

filter_self_from_pointcloud(pointcloud: np.ndarray, point_radius: float, config: np.ndarray) -> np.ndarray

Remove points that collide with the robot body or environment.

Computes forward kinematics at config, then drops every point whose inflated sphere (radius point_radius) overlaps any robot collision sphere or any registered obstacle.

Parameters:

Name	Type	Description	Default
pointcloud	ndarray	(N, 3) array of 3-D points.	required
point_radius	float	Inflation radius for each point.	required
config	ndarray	Active-DOF configuration (same space as plan).	required

Returns:

Type	Description
ndarray	(M, 3) filtered point cloud with M <= N.

Source code in autolife_planning/planning/motion_planner.py

def filter_self_from_pointcloud(
    self,
    pointcloud: np.ndarray,
    point_radius: float,
    config: np.ndarray,
) -> np.ndarray:
    """Remove points that collide with the robot body or environment.

    Computes forward kinematics at ``config``, then drops every
    point whose inflated sphere (radius ``point_radius``) overlaps
    any robot collision sphere or any registered obstacle.

    Args:
        pointcloud: ``(N, 3)`` array of 3-D points.
        point_radius: Inflation radius for each point.
        config: Active-DOF configuration (same space as ``plan``).

    Returns:
        ``(M, 3)`` filtered point cloud with ``M <= N``.
    """
    return self._planner.filter_self_from_pointcloud(
        np.asarray(pointcloud, dtype=np.float32),
        float(point_radius),
        np.asarray(config, dtype=np.float64),
    )

set_subgroup(robot_name: str, base_config: np.ndarray | None = None) -> None

Switch active joints without rebuilding the collision environment.

Clears all constraints. The pointcloud is preserved.

Parameters:

Name	Type	Description	Default
robot_name	str	Subgroup name from PLANNING_SUBGROUPS, or "autolife" for the full 24-DOF body.	required
base_config	ndarray | None	24-DOF frozen config for inactive joints. Defaults to the previously stored base config.	None

Source code in autolife_planning/planning/motion_planner.py

def set_subgroup(
    self,
    robot_name: str,
    base_config: np.ndarray | None = None,
) -> None:
    """Switch active joints without rebuilding the collision environment.

    Clears all constraints.  The pointcloud is preserved.

    Args:
        robot_name: Subgroup name from ``PLANNING_SUBGROUPS``, or
            ``"autolife"`` for the full 24-DOF body.
        base_config: 24-DOF frozen config for inactive joints.
            Defaults to the previously stored base config.
    """
    from autolife_planning.autolife import (
        PLANNING_SUBGROUPS,
        autolife_robot_config,
    )

    if base_config is not None:
        self._base_config = np.asarray(base_config, dtype=np.float64).copy()
    self._robot_name = robot_name

    full_names = autolife_robot_config.joint_names
    sg = PLANNING_SUBGROUPS.get(robot_name)

    if sg is None:
        self._planner.set_full_body()
        self._joint_names = list(full_names)
        self._subgroup_indices = None
    else:
        sg_joint_names = sg["joints"]
        active_indices = [full_names.index(j) for j in sg_joint_names]
        self._planner.set_subgroup(active_indices, self._base_config.tolist())
        self._joint_names = list(sg_joint_names)
        self._subgroup_indices = np.array(active_indices)

    self._ndof = self._planner.dimension()

extract_config(full_config: np.ndarray) -> np.ndarray

Extract this planner's joints from a full 24-DOF configuration.

Source code in autolife_planning/planning/motion_planner.py

def extract_config(self, full_config: np.ndarray) -> np.ndarray:
    """Extract this planner's joints from a full 24-DOF configuration."""
    full_config = np.asarray(full_config, dtype=np.float64)
    if self._subgroup_indices is None:
        return full_config.copy()
    return full_config[self._subgroup_indices].copy()

embed_config(config: np.ndarray, base_config: np.ndarray | None = None) -> np.ndarray

Embed a subgroup config into a full 24-DOF configuration.

base_config defaults to the planner's stored base — the same 24-DOF values the C++ collision checker injects for inactive joints — so the embedded config matches what was validated.

Source code in autolife_planning/planning/motion_planner.py

def embed_config(
    self,
    config: np.ndarray,
    base_config: np.ndarray | None = None,
) -> np.ndarray:
    """Embed a subgroup config into a full 24-DOF configuration.

    ``base_config`` defaults to the planner's stored base — the same
    24-DOF values the C++ collision checker injects for inactive
    joints — so the embedded config matches what was validated.
    """
    config = np.asarray(config, dtype=np.float64)
    if self._subgroup_indices is None:
        return config.copy()

    if base_config is None:
        base_config = self._base_config
    full = np.array(base_config, dtype=np.float64)
    full[self._subgroup_indices] = config
    return full

embed_path(path: np.ndarray, base_config: np.ndarray | None = None) -> np.ndarray

Convert a subgroup path (N, sub_dof) to (N, 24).

base_config defaults to the planner's stored base — the same 24-DOF values the C++ collision checker injects for inactive joints — so the embedded path matches what was validated.

Source code in autolife_planning/planning/motion_planner.py

def embed_path(
    self,
    path: np.ndarray,
    base_config: np.ndarray | None = None,
) -> np.ndarray:
    """Convert a subgroup path ``(N, sub_dof)`` to ``(N, 24)``.

    ``base_config`` defaults to the planner's stored base — the same
    24-DOF values the C++ collision checker injects for inactive
    joints — so the embedded path matches what was validated.
    """
    path = np.asarray(path, dtype=np.float64)
    if self._subgroup_indices is None:
        return path.copy()

    if base_config is None:
        base_config = self._base_config
    n = path.shape[0]
    full_path = np.tile(np.array(base_config, dtype=np.float64), (n, 1))
    full_path[:, self._subgroup_indices] = path
    return full_path

plan(start: np.ndarray, goal: np.ndarray, time_limit: float | None = None) -> PlanningResult

Plan a collision-free path from start to goal.

Parameters:

Name	Type	Description	Default
start	ndarray	Start configuration (active DOF).	required
goal	ndarray	Goal configuration (active DOF).	required
time_limit	float | None	Optional per-call override for the solver time limit. Defaults to self._config.time_limit.	None

Source code in autolife_planning/planning/motion_planner.py

def plan(
    self,
    start: np.ndarray,
    goal: np.ndarray,
    time_limit: float | None = None,
) -> PlanningResult:
    """Plan a collision-free path from start to goal.

    Args:
        start: Start configuration (active DOF).
        goal: Goal configuration (active DOF).
        time_limit: Optional per-call override for the solver time
            limit.  Defaults to ``self._config.time_limit``.
    """
    start = np.asarray(start, dtype=np.float64)
    goal = np.asarray(goal, dtype=np.float64)

    if len(start) != self._ndof:
        raise ValueError(f"start has {len(start)} DOF, expected {self._ndof}")
    if len(goal) != self._ndof:
        raise ValueError(f"goal has {len(goal)} DOF, expected {self._ndof}")

    if not self._planner.validate(start):
        return PlanningResult(PlanningStatus.INVALID_START, None, 0, float("inf"))
    if not self._planner.validate(goal):
        return PlanningResult(PlanningStatus.INVALID_GOAL, None, 0, float("inf"))

    if time_limit is None:
        time_limit = self._config.time_limit

    result = self._planner.plan(
        start,
        goal,
        self._config.planner_name,
        time_limit,
        self._config.simplify,
        self._config.interpolate,
        self._config.resolution,
    )

    if not result.solved:
        return PlanningResult(
            PlanningStatus.FAILED, None, result.planning_time_ns, float("inf")
        )
    return PlanningResult(
        PlanningStatus.SUCCESS,
        np.array(result.path),
        result.planning_time_ns,
        result.path_cost,
    )

simplify_path(path: np.ndarray, time_limit: float = 1.0) -> np.ndarray

Run OMPL's shortcut-based path simplifier on path.

Same pipeline plan(..., simplify=True) uses internally (reduceVertices + collapseCloseVertices + shortcutPath + B-spline smoothing), but detached so you can apply it to any path you already have — e.g. replay an old plan with a different collision environment.

Shortcuts only consult the motion validator. Custom soft costs (:class:Cost) are ignored; for cost-driven plans, run :meth:plan with simplify=False and leave the path untouched unless you've explicitly decided shortcut shaping is acceptable.

Parameters:

Name	Type	Description	Default
path	ndarray	(N, ndof) array of waypoints in the planner's active DOF space.	required
time_limit	float	Wall-clock budget for the simplifier, seconds.	1.0

Returns:

Type	Description
ndarray	(M, ndof) simplified waypoint array with M <= N.

Source code in autolife_planning/planning/motion_planner.py

def simplify_path(self, path: np.ndarray, time_limit: float = 1.0) -> np.ndarray:
    """Run OMPL's shortcut-based path simplifier on ``path``.

    Same pipeline ``plan(..., simplify=True)`` uses internally
    (``reduceVertices`` + ``collapseCloseVertices`` + ``shortcutPath``
    + B-spline smoothing), but detached so you can apply it to any
    path you already have — e.g. replay an old plan with a
    different collision environment.

    Shortcuts only consult the motion validator.  Custom soft
    costs (:class:`Cost`) are ignored; for cost-driven plans, run
    :meth:`plan` with ``simplify=False`` and leave the path
    untouched unless you've explicitly decided shortcut shaping
    is acceptable.

    Args:
        path: ``(N, ndof)`` array of waypoints in the planner's
            active DOF space.
        time_limit: Wall-clock budget for the simplifier, seconds.

    Returns:
        ``(M, ndof)`` simplified waypoint array with ``M <= N``.
    """
    return self._planner.simplify_path(
        np.asarray(path, dtype=np.float64), float(time_limit)
    )

interpolate_path(path: np.ndarray, count: int = 0, resolution: float = 64.0) -> np.ndarray

Densify path with uniform waypoints along every edge.

Three modes (pick one; the other must be zero):

* ``count > 0``        — exactly that many total waypoints
  distributed proportionally to edge length.
* ``resolution > 0.0`` — ``ceil(edge_length * resolution)``
  waypoints per edge (uniform density in state-space
  distance — the default).
* both ``0``           — OMPL's default longest-valid-segment
  fraction.

Uses StateSpace::interpolate internally, so the inserted states stay on the constraint manifold for projected state spaces as well as flat ones. No collision check is performed — the densification only lifts points along the existing piecewise-linear edges.

Parameters:

Name	Type	Description	Default
path	ndarray	(N, ndof) waypoint array.	required
count	int	Exact total waypoint count if > 0.	0
resolution	float	Waypoints per unit state-space distance if > 0.0.	64.0

Returns:

Type	Description
ndarray	(M, ndof) densified waypoint array with M >= N.

Source code in autolife_planning/planning/motion_planner.py

def interpolate_path(
    self,
    path: np.ndarray,
    count: int = 0,
    resolution: float = 64.0,
) -> np.ndarray:
    """Densify ``path`` with uniform waypoints along every edge.

    Three modes (pick one; the other must be zero):

        * ``count > 0``        — exactly that many total waypoints
          distributed proportionally to edge length.
        * ``resolution > 0.0`` — ``ceil(edge_length * resolution)``
          waypoints per edge (uniform density in state-space
          distance — the default).
        * both ``0``           — OMPL's default longest-valid-segment
          fraction.

    Uses ``StateSpace::interpolate`` internally, so the inserted
    states stay on the constraint manifold for projected state
    spaces as well as flat ones.  No collision check is performed
    — the densification only lifts points along the existing
    piecewise-linear edges.

    Args:
        path: ``(N, ndof)`` waypoint array.
        count: Exact total waypoint count if ``> 0``.
        resolution: Waypoints per unit state-space distance if
            ``> 0.0``.

    Returns:
        ``(M, ndof)`` densified waypoint array with ``M >= N``.
    """
    return self._planner.interpolate_path(
        np.asarray(path, dtype=np.float64), int(count), float(resolution)
    )

validate(configuration: np.ndarray) -> bool

Check if a configuration is collision-free.

Source code in autolife_planning/planning/motion_planner.py

def validate(self, configuration: np.ndarray) -> bool:
    """Check if a configuration is collision-free."""
    return self._planner.validate(np.asarray(configuration, dtype=np.float64))

validate_batch(configurations: np.ndarray) -> np.ndarray

Batched collision check — one SIMD block per rake configs.

Packs rake distinct configurations into a single VAMP ConfigurationBlock<rake> and runs one fkcc<rake> call per block, so N queries cost ceil(N / rake) SIMD sweeps in the common case. When a packed block fails we fall back to per-lane single-state checks for that block only, so the returned array is always exactly one bool per input.

Parameters:

Name	Type	Description	Default
configurations	ndarray	(N, ndof) array of active-DOF configurations.	required

Returns:

Type	Description
ndarray	(N,) boolean array; True at index i iff
ndarray	configurations[i] is collision-free.

Source code in autolife_planning/planning/motion_planner.py

def validate_batch(self, configurations: np.ndarray) -> np.ndarray:
    """Batched collision check — one SIMD block per ``rake`` configs.

    Packs ``rake`` distinct configurations into a single VAMP
    ``ConfigurationBlock<rake>`` and runs one ``fkcc<rake>`` call
    per block, so ``N`` queries cost ``ceil(N / rake)`` SIMD
    sweeps in the common case.  When a packed block fails we fall
    back to per-lane single-state checks for that block only, so
    the returned array is always exactly one bool per input.

    Args:
        configurations: ``(N, ndof)`` array of active-DOF
            configurations.

    Returns:
        ``(N,)`` boolean array; ``True`` at index ``i`` iff
        ``configurations[i]`` is collision-free.
    """
    return self._planner.validate_batch(
        np.asarray(configurations, dtype=np.float64)
    )

sample_valid(max_samples: int = 100000) -> np.ndarray

Sample a random collision-free configuration.

Raises:

Type	Description
RuntimeError	If none of max_samples uniform samples is valid.

Source code in autolife_planning/planning/motion_planner.py

def sample_valid(self, max_samples: int = 100_000) -> np.ndarray:
    """Sample a random collision-free configuration.

    Raises:
        RuntimeError: If none of ``max_samples`` uniform samples is valid.
    """
    lo, hi = self.bounds
    for _ in range(0, max_samples, 256):
        batch = np.random.uniform(lo, hi, (256, self._ndof))
        valid = np.flatnonzero(self.validate_batch(batch))
        if valid.size:
            return batch[valid[0]]
    raise RuntimeError(f"no valid configuration in {max_samples} samples")

Factory Function

create_planner(robot_name: str = 'autolife', config: PlannerConfig | None = None, pointcloud: np.ndarray | None = None, base_config: np.ndarray | None = None, constraints: list | None = None, costs: list | None = None) -> MotionPlanner

Create a motion planner for any robot or subgroup.

Parameters:

Name	Type	Description	Default
robot_name	str	Robot or subgroup name. Use :func:available_robots to list all names.	'autolife'
config	PlannerConfig | None	Planner configuration (uses defaults if None).	None
pointcloud	ndarray | None	(N, 3) obstacle point cloud (optional).	None
base_config	ndarray | None	24-DOF values to inject for joints not controlled by this planner (i.e. the frozen joints of a subgroup). Defaults to HOME_JOINTS. Supply any 24-DOF array — for example the live configuration read from your env — to pin the rest of the body wherever it currently is. Ignored for the full-body "autolife" planner.	None
constraints	list | None	Optional list of :class:~autolife_planning.planning.constraints.Constraint instances (CasADi-backed). When non-empty, the planner switches to ProjectedStateSpace and projects every state onto the constraint manifold. Both start and goal passed to plan(...) must already lie on the manifold.	None
costs	list | None	Optional list of :class:~autolife_planning.planning.costs.Cost instances (CasADi-backed). Soft per-state terms summed with their weights and trapezoidally integrated along every motion — the asymptotically-optimal planners (rrtstar, bitstar, aitstar, …) minimise this objective. Without any costs the planner uses OMPL's default path-length objective.	None

Returns:

Name	Type	Description
A	MotionPlanner	class:MotionPlanner instance.

Source code in autolife_planning/planning/motion_planner.py

def create_planner(
    robot_name: str = "autolife",
    config: PlannerConfig | None = None,
    pointcloud: np.ndarray | None = None,
    base_config: np.ndarray | None = None,
    constraints: list | None = None,
    costs: list | None = None,
) -> MotionPlanner:
    """Create a motion planner for any robot or subgroup.

    Args:
        robot_name: Robot or subgroup name. Use :func:`available_robots`
            to list all names.
        config: Planner configuration (uses defaults if None).
        pointcloud: ``(N, 3)`` obstacle point cloud (optional).
        base_config: 24-DOF values to inject for joints not controlled
            by this planner (i.e. the frozen joints of a subgroup).
            Defaults to ``HOME_JOINTS``.  Supply any 24-DOF array — for
            example the live configuration read from your env — to pin
            the rest of the body wherever it currently is.  Ignored for
            the full-body ``"autolife"`` planner.
        constraints: Optional list of
            :class:`~autolife_planning.planning.constraints.Constraint`
            instances (CasADi-backed).  When non-empty, the planner
            switches to ``ProjectedStateSpace`` and projects every state
            onto the constraint manifold.  Both ``start`` and ``goal``
            passed to ``plan(...)`` must already lie on the manifold.
        costs: Optional list of
            :class:`~autolife_planning.planning.costs.Cost` instances
            (CasADi-backed).  Soft per-state terms summed with their
            weights and trapezoidally integrated along every motion —
            the asymptotically-optimal planners (``rrtstar``,
            ``bitstar``, ``aitstar``, …) minimise this objective.
            Without any costs the planner uses OMPL's default
            path-length objective.

    Returns:
        A :class:`MotionPlanner` instance.
    """
    return MotionPlanner(
        robot_name, config, pointcloud, base_config, constraints, costs
    )

Available Robots

available_robots() -> list[str]

Return all available robot names for planning.

Source code in autolife_planning/planning/motion_planner.py

def available_robots() -> list[str]:
    """Return all available robot names for planning."""
    from autolife_planning.autolife import PLANNING_SUBGROUPS

    return ["autolife"] + sorted(PLANNING_SUBGROUPS.keys())

Constraint

Constraint(residual: ca.SX, q_sym: ca.SX, name: str = 'constraint') dataclass

A user-defined holonomic constraint, JIT-compiled via CasADi.

Constructing this class takes the symbolic Jacobian via ca.jacobian(residual, q_sym), generates C and compiles it to a cached shared library (see :func:compile_function).

SymbolicContext(subgroup: str, base_config: np.ndarray | None = None)

CasADi-friendly view of the planner's active subgroup.

Holds the CasADi symbolic joint vector q matching the subgroup's active dimension. Joints outside the subgroup are frozen at base_config, the same 24-DOF body vector ([x, y, theta, j0..j20]) the planner uses.

Source code in autolife_planning/planning/symbolic.py

def __init__(
    self,
    subgroup: str,
    base_config: np.ndarray | None = None,
) -> None:
    if base_config is None:
        base_config = HOME_JOINTS
    self.base_config = np.asarray(base_config, dtype=np.float64).copy()
    if self.base_config.shape != HOME_JOINTS.shape:
        raise ValueError(
            f"base_config must have shape {HOME_JOINTS.shape}, "
            f"got {self.base_config.shape}"
        )

    self.subgroup_name = subgroup
    self._names = list(autolife_robot_config.joint_names)
    if subgroup == "autolife":
        self.active_names = list(self._names)
    else:
        sg = PLANNING_SUBGROUPS.get(subgroup)
        if sg is None:
            raise ValueError(f"Unknown subgroup: {subgroup!r}")
        self.active_names = list(sg["joints"])
    self.active_indices = [self._names.index(j) for j in self.active_names]

    self.q = ca.SX.sym("q", len(self.active_indices))
    self._joints = _parse_joints(autolife_robot_config.urdf_path)
    self._poses: dict[str, ca.SX] = {}
    self._pose_fns: dict[str, ca.Function] = {}

link_pose(link_name: str, q_active: ca.SX | None = None) -> ca.SX

Symbolic 4x4 world pose of a URDF link.

Uses self.q unless another active-dimension symbol is given.

Source code in autolife_planning/planning/symbolic.py

def link_pose(self, link_name: str, q_active: ca.SX | None = None) -> ca.SX:
    """Symbolic 4x4 world pose of a URDF link.

    Uses ``self.q`` unless another active-dimension symbol is given.
    """
    if q_active is not None and q_active is not self.q:
        return self._pose(link_name, q_active)
    if link_name not in self._poses:
        self._poses[link_name] = self._pose(link_name, self.q)
    return self._poses[link_name]

link_translation(link_name: str, q_active: ca.SX | None = None) -> ca.SX

Symbolic 3-vector: link position in world frame.

Source code in autolife_planning/planning/symbolic.py

def link_translation(self, link_name: str, q_active: ca.SX | None = None) -> ca.SX:
    """Symbolic 3-vector: link position in world frame."""
    return self.link_pose(link_name, q_active)[:3, 3]

link_rotation(link_name: str, q_active: ca.SX | None = None) -> ca.SX

Symbolic 3x3 rotation matrix of the link.

Source code in autolife_planning/planning/symbolic.py

def link_rotation(self, link_name: str, q_active: ca.SX | None = None) -> ca.SX:
    """Symbolic 3x3 rotation matrix of the link."""
    return self.link_pose(link_name, q_active)[:3, :3]

evaluate_link_pose(link_name: str, q_active_numeric: np.ndarray) -> np.ndarray

Compute a NUMERIC 4x4 link pose (handy for building targets).

Source code in autolife_planning/planning/symbolic.py

def evaluate_link_pose(
    self, link_name: str, q_active_numeric: np.ndarray
) -> np.ndarray:
    """Compute a NUMERIC 4x4 link pose (handy for building targets)."""
    if link_name not in self._pose_fns:
        self._pose_fns[link_name] = ca.Function(
            "fk", [self.q], [self.link_pose(link_name)]
        )
    return np.asarray(self._pose_fns[link_name](q_active_numeric), dtype=np.float64)

project(q_init: np.ndarray, residual: ca.SX, tol: float = 1e-08, max_iters: int = 100) -> np.ndarray

Project a joint configuration onto the manifold residual(q) = 0.

Runs damped Gauss-Newton on the CasADi Jacobian — the same iteration OMPL's ProjectedStateSpace runs internally — so the returned config will pass the planner's tolerance check and can be used directly as a start or goal state.

Source code in autolife_planning/planning/symbolic.py

def project(
    self,
    q_init: np.ndarray,
    residual: ca.SX,
    tol: float = 1e-8,
    max_iters: int = 100,
) -> np.ndarray:
    """Project a joint configuration onto the manifold ``residual(q) = 0``.

    Runs damped Gauss-Newton on the CasADi Jacobian — the same
    iteration OMPL's ``ProjectedStateSpace`` runs internally — so
    the returned config will pass the planner's tolerance check
    and can be used directly as a start or goal state.
    """
    res_fn = ca.Function("proj_res", [self.q], [ca.reshape(residual, -1, 1)])
    jac_fn = ca.Function("proj_jac", [self.q], [ca.jacobian(residual, self.q)])
    q = np.asarray(q_init, dtype=np.float64).copy()
    for _ in range(max_iters):
        r = np.asarray(res_fn(q)).flatten()
        if np.linalg.norm(r) < tol:
            return q
        J = np.asarray(jac_fn(q))
        JJt = J @ J.T + 1e-10 * np.eye(J.shape[0])
        q -= J.T @ np.linalg.solve(JJt, r)
    raise RuntimeError(
        f"SymbolicContext.project failed to converge: "
        f"|residual|={np.linalg.norm(r):.2e} after {max_iters} iters"
    )

Cost

Cost(expression: ca.SX, q_sym: ca.SX, name: str = 'cost', weight: float = 1.0) dataclass

A user-defined soft path cost, JIT-compiled via CasADi.

Pass a scalar CasADi expression in the planner's active joint vector. Construction triggers (on cold cache):

1. symbolic gradient via ``ca.gradient(expression, q_sym)``
2. C code generation and compilation to a cached shared library
   (see :func:`~autolife_planning.planning.symbolic.compile_function`)

The C++ planner wraps the loaded function in an OMPL StateCostIntegralObjective — so per-state values are trapezoidally integrated along every motion, which is what RRT*-family optimal planners expect.

The expression must be non-negative (OMPL objectives accumulate with operator+ and the optimal-planner tooling assumes the zero cost is the minimum). We don't enforce this at runtime because that would require evaluating the symbolic expression, but violating it produces nonsensical RRT* solutions.

Low-level C++ binding

OmplVampPlanner

OMPL planner with VAMP SIMD-accelerated collision checking.

Two construction modes:

• OmplVampPlanner() — full body, 24 DOF (3 base + 21 joints).
• OmplVampPlanner(active_indices, frozen_config) — subgroup planner over the joints listed in active_indices; the C++ collision checker injects frozen_config for every other slot in the 24-DOF body on every state and motion validity query.

add_pointcloud(points: NDArray[np.float32], point_radius: float) -> None

Set the scene pointcloud.

The planner holds at most one cloud; calling this replaces any previously-registered cloud.

Parameters:

Name	Type	Description	Default
points	NDArray[float32]	(N, 3) float32 obstacle positions in world frame.	required
point_radius	float	Inflation radius applied to every cloud point.	required

Source code in autolife_planning/_ompl_vamp.pyi

def add_pointcloud(
    self,
    points: NDArray[np.float32],
    point_radius: float,
) -> None:
    """Set the scene pointcloud.

    The planner holds at most one cloud; calling this replaces any
    previously-registered cloud.

    Args:
        points: ``(N, 3)`` float32 obstacle positions in world frame.
        point_radius: Inflation radius applied to every cloud point.
    """
    ...

remove_pointcloud() -> bool

Drop the currently-registered pointcloud.

Returns False if there was no cloud to remove.

Source code in autolife_planning/_ompl_vamp.pyi

def remove_pointcloud(self) -> bool:
    """Drop the currently-registered pointcloud.

    Returns ``False`` if there was no cloud to remove.
    """
    ...

has_pointcloud() -> bool

True if a pointcloud is currently registered.

Source code in autolife_planning/_ompl_vamp.pyi

def has_pointcloud(self) -> bool:
    """``True`` if a pointcloud is currently registered."""
    ...

add_sphere(center: Sequence[float], radius: float) -> None

Add a single sphere obstacle (centre + radius) to the environment.

Source code in autolife_planning/_ompl_vamp.pyi

def add_sphere(self, center: Sequence[float], radius: float) -> None:
    """Add a single sphere obstacle (centre + radius) to the environment."""
    ...

clear_environment() -> None

Remove all obstacles from the collision environment.

Source code in autolife_planning/_ompl_vamp.pyi

def clear_environment(self) -> None:
    """Remove all obstacles from the collision environment."""
    ...

add_compiled_constraint(so_path: str, symbol_name: str, ambient_dim: int, co_dim: int) -> None

Load a CasADi-generated shared library as an OMPL constraint.

Parameters:

Name	Type	Description	Default
so_path	str	Path to the compiled .so file.	required
symbol_name	str	CasADi function symbol name inside the library.	required
ambient_dim	int	Dimension of the joint space (must match :meth:dimension).	required
co_dim	int	Number of constraint equations (rows of the residual).	required

Source code in autolife_planning/_ompl_vamp.pyi

def add_compiled_constraint(
    self,
    so_path: str,
    symbol_name: str,
    ambient_dim: int,
    co_dim: int,
) -> None:
    """Load a CasADi-generated shared library as an OMPL constraint.

    Args:
        so_path: Path to the compiled ``.so`` file.
        symbol_name: CasADi function symbol name inside the library.
        ambient_dim: Dimension of the joint space (must match
            :meth:`dimension`).
        co_dim: Number of constraint equations (rows of the residual).
    """
    ...

clear_constraints() -> None

Drop every accumulated constraint.

Source code in autolife_planning/_ompl_vamp.pyi

def clear_constraints(self) -> None:
    """Drop every accumulated constraint."""
    ...

num_constraints() -> int

Number of constraints currently registered.

Source code in autolife_planning/_ompl_vamp.pyi

def num_constraints(self) -> int:
    """Number of constraints currently registered."""
    ...

add_compiled_cost(so_path: str, symbol_name: str, ambient_dim: int, weight: float = 1.0) -> None

Load a CasADi-generated shared library as a soft path cost.

The cost is wrapped as an ompl::StateCostIntegralObjective — trapezoidally integrated along every motion — and drives the search of asymptotically-optimal planners (RRT, BIT, AIT*, …). Multiple costs are summed with their weight.

Parameters:

Name	Type	Description	Default
so_path	str	Path to the compiled .so file.	required
symbol_name	str	CasADi function symbol name inside the library.	required
ambient_dim	int	Dimension of the joint space (must match :meth:dimension).	required
weight	float	Positive scalar multiplier applied to the cost.	1.0

Source code in autolife_planning/_ompl_vamp.pyi

def add_compiled_cost(
    self,
    so_path: str,
    symbol_name: str,
    ambient_dim: int,
    weight: float = 1.0,
) -> None:
    """Load a CasADi-generated shared library as a soft path cost.

    The cost is wrapped as an ``ompl::StateCostIntegralObjective``
    — trapezoidally integrated along every motion — and drives
    the search of asymptotically-optimal planners (RRT*, BIT*,
    AIT*, …).  Multiple costs are summed with their ``weight``.

    Args:
        so_path: Path to the compiled ``.so`` file.
        symbol_name: CasADi function symbol name inside the library.
        ambient_dim: Dimension of the joint space (must match
            :meth:`dimension`).
        weight: Positive scalar multiplier applied to the cost.
    """
    ...

clear_costs() -> None

Drop every accumulated cost.

Source code in autolife_planning/_ompl_vamp.pyi

def clear_costs(self) -> None:
    """Drop every accumulated cost."""
    ...

num_costs() -> int

Number of costs currently registered.

Source code in autolife_planning/_ompl_vamp.pyi

def num_costs(self) -> int:
    """Number of costs currently registered."""
    ...

plan(start: NDArray[np.float64], goal: NDArray[np.float64], planner_name: str = 'rrtc', time_limit: float = 10.0, simplify: bool = True, interpolate: bool = True, resolution: float = 64.0) -> PlanResult

Plan a collision-free path from start to goal.

Parameters:

Name	Type	Description	Default
start	NDArray[float64]	Active-DOF start configuration (length :meth:dimension).	required
goal	NDArray[float64]	Active-DOF goal configuration (length :meth:dimension).	required
planner_name	str	OMPL planner identifier (e.g. "rrtc", "rrtstar", "prm", "bitstar").	'rrtc'
time_limit	float	Solver time limit in seconds.	10.0
simplify	bool	If true, run SimpleSetup::simplifySolution on the returned path.	True
interpolate	bool	If true, densify the simplified path.	True
resolution	float	Waypoints per unit of state-space distance. Each edge of length d is split into ceil(d * resolution) equal segments, so higher values give denser paths. Default 64.0. Set to 0.0 to fall back to OMPL's default interpolator.	64.0

Source code in autolife_planning/_ompl_vamp.pyi

def plan(
    self,
    start: NDArray[np.float64],
    goal: NDArray[np.float64],
    planner_name: str = "rrtc",
    time_limit: float = 10.0,
    simplify: bool = True,
    interpolate: bool = True,
    resolution: float = 64.0,
) -> PlanResult:
    """Plan a collision-free path from ``start`` to ``goal``.

    Args:
        start: Active-DOF start configuration (length :meth:`dimension`).
        goal: Active-DOF goal configuration (length :meth:`dimension`).
        planner_name: OMPL planner identifier (e.g. ``"rrtc"``,
            ``"rrtstar"``, ``"prm"``, ``"bitstar"``).
        time_limit: Solver time limit in seconds.
        simplify: If true, run ``SimpleSetup::simplifySolution`` on the
            returned path.
        interpolate: If true, densify the simplified path.
        resolution: Waypoints per unit of state-space distance.  Each
            edge of length ``d`` is split into ``ceil(d * resolution)``
            equal segments, so higher values give denser paths.
            Default ``64.0``.  Set to ``0.0`` to fall back to OMPL's
            default interpolator.
    """
    ...

simplify_path(path: NDArray[np.float64], time_limit: float = 1.0) -> NDArray[np.float64]

Run OMPL's shortcut simplifier on a waypoint list.

Reuses the current collision environment and constraints. Shortcuts only consult the motion validator, so custom soft costs are ignored. Returns the simplified path as waypoints.

Parameters:

Name	Type	Description	Default
path	NDArray[float64]	(N, dimension()) waypoint list.	required
time_limit	float	Simplifier wall-clock budget, seconds.	1.0

Source code in autolife_planning/_ompl_vamp.pyi

def simplify_path(
    self,
    path: NDArray[np.float64],
    time_limit: float = 1.0,
) -> NDArray[np.float64]:
    """Run OMPL's shortcut simplifier on a waypoint list.

    Reuses the current collision environment and constraints.
    Shortcuts only consult the motion validator, so custom soft
    costs are ignored.  Returns the simplified path as waypoints.

    Args:
        path: ``(N, dimension())`` waypoint list.
        time_limit: Simplifier wall-clock budget, seconds.
    """
    ...

interpolate_path(path: NDArray[np.float64], count: int = 0, resolution: float = 64.0) -> NDArray[np.float64]

Densify a waypoint list along its existing edges.

Pass at most one of count (exact total waypoints, distributed by edge length) or resolution (waypoints per unit of state-space distance). Both zero falls back to OMPL's default longest-valid-segment fraction. No collision check is performed — densification only calls StateSpace::interpolate on the existing edges.

Parameters:

Name	Type	Description	Default
path	NDArray[float64]	(N, dimension()) waypoint list.	required
count	int	Total waypoint count if > 0.	0
resolution	float	Waypoints per unit distance if > 0.0.	64.0

Source code in autolife_planning/_ompl_vamp.pyi

def interpolate_path(
    self,
    path: NDArray[np.float64],
    count: int = 0,
    resolution: float = 64.0,
) -> NDArray[np.float64]:
    """Densify a waypoint list along its existing edges.

    Pass at most one of ``count`` (exact total waypoints,
    distributed by edge length) or ``resolution`` (waypoints per
    unit of state-space distance).  Both zero falls back to OMPL's
    default longest-valid-segment fraction.  No collision check is
    performed — densification only calls ``StateSpace::interpolate``
    on the existing edges.

    Args:
        path: ``(N, dimension())`` waypoint list.
        count: Total waypoint count if > 0.
        resolution: Waypoints per unit distance if > 0.0.
    """
    ...

validate(config: NDArray[np.float64]) -> bool

Return True if config is collision-free.

config must have length :meth:dimension. Subgroup planners expand it to a full 24-DOF state with the stored frozen_config before checking.

Source code in autolife_planning/_ompl_vamp.pyi

def validate(self, config: NDArray[np.float64]) -> bool:
    """Return ``True`` if ``config`` is collision-free.

    ``config`` must have length :meth:`dimension`.  Subgroup
    planners expand it to a full 24-DOF state with the stored
    ``frozen_config`` before checking.
    """
    ...

validate_batch(configs: NDArray[np.float64]) -> NDArray[np.bool_]

Batched collision check — deep SIMD, per-config result.

Packs up to rake distinct configurations into a single VAMP ConfigurationBlock<rake> so one Robot::fkcc<rake> call sphere-FKs and collision-checks all of them simultaneously — the same SIMD primitive the motion-edge validator uses for interpolated samples, fed independent configs per lane instead.

Returns one bool per input config in input order. In the common case (most configs valid) costs ceil(N / rake) SIMD calls; when a packed block fails, only that block falls back to per-lane single-state checks.

configs has shape (N, dimension()). Subgroup planners expand each to a full 24-DOF state with the stored frozen_config before packing.

Source code in autolife_planning/_ompl_vamp.pyi

def validate_batch(
    self,
    configs: NDArray[np.float64],
) -> NDArray[np.bool_]:
    """Batched collision check — deep SIMD, per-config result.

    Packs up to ``rake`` distinct configurations into a single
    VAMP ``ConfigurationBlock<rake>`` so one ``Robot::fkcc<rake>``
    call sphere-FKs and collision-checks all of them
    simultaneously — the same SIMD primitive the motion-edge
    validator uses for interpolated samples, fed independent
    configs per lane instead.

    Returns one bool per input config in input order.  In the
    common case (most configs valid) costs ``ceil(N / rake)``
    SIMD calls; when a packed block fails, only that block falls
    back to per-lane single-state checks.

    ``configs`` has shape ``(N, dimension())``.
    Subgroup planners expand each to a full 24-DOF state with
    the stored ``frozen_config`` before packing.
    """
    ...

dimension() -> int

Number of active joints — 24 for the full body, smaller for subgroups.

Source code in autolife_planning/_ompl_vamp.pyi

def dimension(self) -> int:
    """Number of active joints — 24 for the full body, smaller for subgroups."""
    ...

lower_bounds() -> NDArray[np.float64]

Per-joint lower bounds for the active DOFs.

Source code in autolife_planning/_ompl_vamp.pyi

def lower_bounds(self) -> NDArray[np.float64]:
    """Per-joint lower bounds for the active DOFs."""
    ...

upper_bounds() -> NDArray[np.float64]

Per-joint upper bounds for the active DOFs.

Source code in autolife_planning/_ompl_vamp.pyi

def upper_bounds(self) -> NDArray[np.float64]:
    """Per-joint upper bounds for the active DOFs."""
    ...

filter_pointcloud(points: NDArray[np.float32], min_dist: float, max_range: float, origin: Sequence[float], workspace_min: Sequence[float], workspace_max: Sequence[float], cull: bool = True) -> NDArray[np.float32]

Morton-curve downsampling of an (N, 3) cloud, optionally culled to max_range from origin and to the workspace box.

Source code in autolife_planning/_ompl_vamp.pyi

def filter_pointcloud(
    self,
    points: NDArray[np.float32],
    min_dist: float,
    max_range: float,
    origin: Sequence[float],
    workspace_min: Sequence[float],
    workspace_max: Sequence[float],
    cull: bool = True,
) -> NDArray[np.float32]:
    """Morton-curve downsampling of an ``(N, 3)`` cloud, optionally
    culled to ``max_range`` from ``origin`` and to the workspace box.
    """
    ...

filter_self_from_pointcloud(points: NDArray[np.float32], point_radius: float, config: NDArray[np.float64]) -> NDArray[np.float32]

Drop the points of an (N, 3) cloud that overlap the robot's spheres at config (active DOF) or the current environment.

Source code in autolife_planning/_ompl_vamp.pyi

def filter_self_from_pointcloud(
    self,
    points: NDArray[np.float32],
    point_radius: float,
    config: NDArray[np.float64],
) -> NDArray[np.float32]:
    """Drop the points of an ``(N, 3)`` cloud that overlap the robot's
    spheres at ``config`` (active DOF) or the current environment.
    """
    ...

set_subgroup(active_indices: Sequence[int], frozen_config: Sequence[float]) -> None

Switch to a different subgroup without rebuilding the environment.

Clears all constraints. The pointcloud and collision geometry are preserved.

Parameters:

Name	Type	Description	Default
active_indices	Sequence[int]	Positions in the full 24-DOF body that this planner will plan over, in DOF order.	required
frozen_config	Sequence[float]	24-DOF stance to inject for every joint not in active_indices.	required

Source code in autolife_planning/_ompl_vamp.pyi

def set_subgroup(
    self,
    active_indices: Sequence[int],
    frozen_config: Sequence[float],
) -> None:
    """Switch to a different subgroup without rebuilding the environment.

    Clears all constraints.  The pointcloud and collision geometry
    are preserved.

    Args:
        active_indices: Positions in the full 24-DOF body that this
            planner will plan over, in DOF order.
        frozen_config: 24-DOF stance to inject for every joint *not*
            in ``active_indices``.
    """
    ...

set_full_body() -> None

Switch back to full-body planning (24 DOF).

Clears all constraints. The pointcloud and collision geometry are preserved.

Source code in autolife_planning/_ompl_vamp.pyi

def set_full_body(self) -> None:
    """Switch back to full-body planning (24 DOF).

    Clears all constraints.  The pointcloud and collision geometry
    are preserved.
    """
    ...