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:
        r_min, r_max = self._planner.min_max_radii()
        self._planner.add_pointcloud(
            np.asarray(pointcloud, dtype=np.float32).tolist(),
            r_min,
            r_max,
            config.point_radius,
        )

    if constraints:
        self._push_constraints(constraints)

    if costs:
        self._push_costs(costs)

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.
    """
    r_min, r_max = self._planner.min_max_radii()
    self._planner.add_pointcloud(
        np.asarray(pointcloud, dtype=np.float32).tolist(),
        r_min,
        r_max,
        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``.
    """
    pts = np.asarray(pointcloud, dtype=np.float32).tolist()
    origin = [float(x) for x in origin]
    workspace_min = [float(x) for x in workspace_min]
    workspace_max = [float(x) for x in workspace_max]
    filtered = self._planner.filter_pointcloud(
        pts,
        float(min_dist),
        float(max_range),
        origin,
        workspace_min,
        workspace_max,
        cull,
    )
    return np.asarray(filtered, dtype=np.float32)

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``.
    """
    pts = np.asarray(pointcloud, dtype=np.float32).tolist()
    config = np.asarray(config, dtype=np.float64)
    if len(config) != self._ndof:
        raise ValueError(f"config has {len(config)} DOF, expected {self._ndof}")
    filtered = self._planner.filter_self_from_pointcloud(
        pts,
        float(point_radius),
        config.tolist(),
    )
    return np.asarray(filtered, dtype=np.float32)

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.tolist()):
        return PlanningResult(
            status=PlanningStatus.INVALID_START,
            path=None,
            planning_time_ns=0,
            iterations=0,
            path_cost=float("inf"),
        )
    if not self._planner.validate(goal.tolist()):
        return PlanningResult(
            status=PlanningStatus.INVALID_GOAL,
            path=None,
            planning_time_ns=0,
            iterations=0,
            path_cost=float("inf"),
        )

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

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

    if not result.solved:
        return PlanningResult(
            status=PlanningStatus.FAILED,
            path=None,
            planning_time_ns=result.planning_time_ns,
            iterations=0,
            path_cost=float("inf"),
        )

    path_np = np.array(result.path, dtype=np.float64)

    return PlanningResult(
        status=PlanningStatus.SUCCESS,
        path=path_np,
        planning_time_ns=result.planning_time_ns,
        iterations=0,
        path_cost=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``.
    """
    path = np.asarray(path, dtype=np.float64)
    if path.ndim != 2 or path.shape[1] != self._ndof:
        raise ValueError(
            f"path must have shape (N, {self._ndof}), got {path.shape}"
        )
    simp = self._planner.simplify_path(path.tolist(), float(time_limit))
    return np.array(simp, dtype=np.float64)

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``.
    """
    path = np.asarray(path, dtype=np.float64)
    if path.ndim != 2 or path.shape[1] != self._ndof:
        raise ValueError(
            f"path must have shape (N, {self._ndof}), got {path.shape}"
        )
    dense = self._planner.interpolate_path(
        path.tolist(), int(count), float(resolution)
    )
    return np.array(dense, dtype=np.float64)

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."""
    configuration = np.asarray(configuration, dtype=np.float64)
    return self._planner.validate(configuration.tolist())

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.
    """
    configurations = np.asarray(configurations, dtype=np.float64)
    if configurations.ndim != 2 or configurations.shape[1] != self._ndof:
        raise ValueError(
            f"configurations must have shape (N, {self._ndof}), "
            f"got {configurations.shape}"
        )
    valid = self._planner.validate_batch(configurations.tolist())
    return np.asarray(valid, dtype=bool)

sample_valid() -> np.ndarray

Sample a random collision-free configuration.

Source code in autolife_planning/planning/motion_planner.py

def sample_valid(self) -> np.ndarray:
    """Sample a random collision-free configuration."""
    lo = np.array(self._planner.lower_bounds())
    hi = np.array(self._planner.upper_bounds())
    while True:
        config = np.random.uniform(lo, hi)
        if self._planner.validate(config.tolist()):
            return config

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 triggers (on cold cache): 1. symbolic Jacobian via ca.jacobian(residual, q_sym) 2. C code generation via CasADi 3. compilation to a .so with c++ -O3 -shared -fPIC 4. caching under ~/.cache/autolife_planning/constraints/<sha>/

On a cache hit the whole thing is a single stat + string compare.

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, plus a pinocchio.casadi model for symbolic FK.

The context hides the planar-root encoding (the 24-DOF body vector uses [x, y, theta, j0..j20]; pinocchio uses [x, y, cos(theta), sin(theta), j0..j20]) and the mapping from the active subspace back to the full 24-DOF body via base_config.

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
    full_names = list(autolife_robot_config.joint_names)
    if subgroup == "autolife":
        self.active_indices = list(range(24))
        self.active_names = full_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 = [full_names.index(j) for j in self.active_names]

    self.q = ca.SX.sym("q", len(self.active_indices))
    self._full_names = list(autolife_robot_config.joint_names)
    self._root_link = "Link_Zero_Point"
    self._uses_planar_base = any(
        n in {"Joint_Virtual_X", "Joint_Virtual_Y", "Joint_Virtual_Theta"}
        for n in self.active_names
    )

    self._fk_cache: dict[str, dict[str, object]] = {}

    if pin is not None and cpin is not None:
        self._backend = "pinocchio"
        # Numeric + symbolic pinocchio models with planar root.
        self.pin_model = pin.buildModelFromUrdf(
            autolife_robot_config.urdf_path, pin.JointModelPlanar()
        )
        self.pin_data = self.pin_model.createData()
        self.cmodel = cpin.Model(self.pin_model)
        self.cdata = self.cmodel.createData()

        # Build the symbolic mapping q_active -> pinocchio q (nq=25).
        self.q_pin = self._build_pinocchio_q(self.q)

        # Pre-run symbolic FK so users can access frame transforms.
        cpin.forwardKinematics(self.cmodel, self.cdata, self.q_pin)
        cpin.updateFramePlacements(self.cmodel, self.cdata)
    elif URDFparser is not None:
        if self._uses_planar_base:
            raise RuntimeError(
                "SymbolicContext fallback backend (urdf2casadi) does not support "
                "planar base symbolic joints (Joint_Virtual_X/Y/Theta). "
                "Install pinocchio + pinocchio.casadi for base-enabled groups."
            )
        self._backend = "urdf2casadi"
        self._urdf_parser = URDFparser()
        self._urdf_parser.from_file(autolife_robot_config.urdf_path)
        # Use the same world-like root as PyBullet visualization when available.
        try:
            self._urdf_parser.get_joint_info("Link_Zero_Point", "Link_Zero_Point")
            self._root_link = "Link_Zero_Point"
        except Exception:
            self._root_link = "Link_Ground_Vehicle"
    else:
        raise ModuleNotFoundError(
            "SymbolicContext requires either pinocchio.casadi or urdf2casadi. "
            "Install one of these backends."
        )

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

Return the symbolic pinocchio SE3 of a URDF link.

Pass q_active=self.q (or omit) to get an expression that depends on the active joints symbolically.

Source code in autolife_planning/planning/symbolic.py

def link_pose(self, link_name: str, q_active: ca.SX | None = None):
    """Return the symbolic pinocchio SE3 of a URDF link.

    Pass ``q_active=self.q`` (or omit) to get an expression that
    depends on the active joints symbolically.
    """
    if self._backend == "pinocchio":
        if q_active is None or q_active is self.q:
            frame_id = self.cmodel.getFrameId(link_name)
            return self.cdata.oMf[frame_id]
        # Rebuild with a different symbol.
        cdata = self.cmodel.createData()
        q_pin = self._build_pinocchio_q(q_active)
        cpin.forwardKinematics(self.cmodel, cdata, q_pin)
        cpin.updateFramePlacement(
            self.cmodel, cdata, self.cmodel.getFrameId(link_name)
        )
        return cdata.oMf[self.cmodel.getFrameId(link_name)]

    # urdf2casadi fallback
    q_eval = self.q if q_active is None else q_active
    return self._urdf2casadi_pose(link_name, q_eval)

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).translation

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).rotation

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

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

Uses the numeric pinocchio model, not the symbolic one, so it is fast and has no dependence on CasADi expressions.

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).

    Uses the numeric pinocchio model, not the symbolic one, so it is
    fast and has no dependence on CasADi expressions.
    """
    if self._backend == "pinocchio":
        full = self.base_config.copy()
        for i, idx in enumerate(self.active_indices):
            full[idx] = q_active_numeric[i]
        q = np.empty(int(self.pin_model.nq))
        q[0] = full[0]
        q[1] = full[1]
        q[2] = np.cos(full[2])
        q[3] = np.sin(full[2])
        q[4:] = full[3:]
        pin.forwardKinematics(self.pin_model, self.pin_data, q)
        pin.updateFramePlacement(
            self.pin_model,
            self.pin_data,
            self.pin_model.getFrameId(link_name),
        )
        pose = self.pin_data.oMf[self.pin_model.getFrameId(link_name)]
        M = np.eye(4)
        M[:3, :3] = pose.rotation
        M[:3, 3] = pose.translation
        return M

    # urdf2casadi fallback
    T = self._urdf2casadi_pose(link_name, self.q)
    T_fn = ca.Function("fk_eval", [self.q], [T.rotation, T.translation])
    rot_num, trans_num = T_fn(np.asarray(q_active_numeric, dtype=np.float64))
    M = np.eye(4)
    M[:3, :3] = np.asarray(rot_num, dtype=np.float64)
    M[:3, 3] = np.asarray(trans_num, dtype=np.float64).reshape(-1)
    return M

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 via CasADi
3. compilation to a ``.so`` with ``c++ -O3 -shared -fPIC``
4. caching under ``~/.cache/autolife_planning/costs/<sha>/``

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: Sequence[Sequence[float]], r_min: float, r_max: float, 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	Sequence[Sequence[float]]	(N, 3) array of obstacle positions in world frame.	required
r_min	float	Minimum robot collision-sphere radius (from :meth:min_max_radii).	required
r_max	float	Maximum robot collision-sphere radius (from :meth:min_max_radii).	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: Sequence[Sequence[float]],
    r_min: float,
    r_max: float,
    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)`` array of obstacle positions in world frame.
        r_min: Minimum robot collision-sphere radius (from
            :meth:`min_max_radii`).
        r_max: Maximum robot collision-sphere radius (from
            :meth:`min_max_radii`).
        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: Sequence[float], goal: Sequence[float], planner_name: str = 'rrtc', time_limit: float = 10.0, simplify: bool = True, interpolate: bool = True, interpolate_count: int = 0, resolution: float = 64.0) -> PlanResult

Plan a collision-free path from start to goal.

Parameters:

Name	Type	Description	Default
start	Sequence[float]	Active-DOF start configuration (length :meth:dimension).	required
goal	Sequence[float]	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. Density is picked by interpolate_count when it is > 0, otherwise by resolution when it is > 0, otherwise by OMPL's default longest-valid-segment fraction.	True
interpolate_count	int	Target total waypoint count for the whole path. OMPL distributes states across edges proportionally to their length. 0 disables this knob. Cannot be combined with resolution.	0
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. Cannot be combined with interpolate_count.	64.0

Source code in autolife_planning/_ompl_vamp.pyi

def plan(
    self,
    start: Sequence[float],
    goal: Sequence[float],
    planner_name: str = "rrtc",
    time_limit: float = 10.0,
    simplify: bool = True,
    interpolate: bool = True,
    interpolate_count: int = 0,
    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.  Density
            is picked by ``interpolate_count`` when it is ``> 0``,
            otherwise by ``resolution`` when it is ``> 0``, otherwise
            by OMPL's default longest-valid-segment fraction.
        interpolate_count: Target total waypoint count for the whole
            path.  OMPL distributes states across edges proportionally
            to their length.  ``0`` disables this knob.  Cannot be
            combined with ``resolution``.
        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.  Cannot be combined with
            ``interpolate_count``.
    """
    ...

simplify_path(path: Sequence[Sequence[float]], time_limit: float = 1.0) -> list[list[float]]

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	Sequence[Sequence[float]]	(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: Sequence[Sequence[float]],
    time_limit: float = 1.0,
) -> list[list[float]]:
    """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: Sequence[Sequence[float]], count: int = 0, resolution: float = 64.0) -> list[list[float]]

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	Sequence[Sequence[float]]	(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: Sequence[Sequence[float]],
    count: int = 0,
    resolution: float = 64.0,
) -> list[list[float]]:
    """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: Sequence[float]) -> 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: Sequence[float]) -> 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: Sequence[Sequence[float]]) -> list[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.

Each configs[i] must have length :meth: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: Sequence[Sequence[float]],
) -> list[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.

    Each ``configs[i]`` must have length :meth:`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() -> list[float]

Per-joint lower bounds for the active DOFs.

Source code in autolife_planning/_ompl_vamp.pyi

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

upper_bounds() -> list[float]

Per-joint upper bounds for the active DOFs.

Source code in autolife_planning/_ompl_vamp.pyi

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

min_max_radii() -> tuple[float, float]

(min_radius, max_radius) of the robot's collision spheres.

Pass these to :meth:add_pointcloud so VAMP can index its broadphase correctly.

Source code in autolife_planning/_ompl_vamp.pyi

def min_max_radii(self) -> tuple[float, float]:
    """``(min_radius, max_radius)`` of the robot's collision spheres.

    Pass these to :meth:`add_pointcloud` so VAMP can index its
    broadphase correctly.
    """
    ...

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.
    """
    ...