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 config.joint_limits:
        self.set_joint_limits(config.joint_limits)

    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_limits: dict[str, tuple[float, float]] property

{joint name: (lower, upper)} the planner samples in, for every joint of the full body. :attr:bounds gives the active joints' ranges as arrays.

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.

set_joint_limits(limits: Mapping[str, tuple[float, float]] | None = None) -> None

Set the joint ranges the planner samples in, by joint name.

Joints left out of limits take the range compiled into the robot model, so None restores every compiled range. The ranges cover the full body and are kept across :meth:set_subgroup. Collision checking does not read them.

Parameters:

Name	Type	Description	Default
limits	Mapping[str, tuple[float, float]] | None	{joint name: (lower, upper)} in metres for the virtual base joints and radians otherwise — e.g. widen Joint_Virtual_X beyond the model's ±10 m for a larger map, or narrow any joint to keep plans in a smaller range.	None

Raises:

Type	Description
ValueError	For an unknown joint name, or bounds that are not finite with lower < upper. The ranges in effect are then left unchanged.

Source code in autolife_planning/planning/motion_planner.py

def set_joint_limits(
    self, limits: Mapping[str, tuple[float, float]] | None = None
) -> None:
    """Set the joint ranges the planner samples in, by joint name.

    Joints left out of ``limits`` take the range compiled into the
    robot model, so ``None`` restores every compiled range.  The
    ranges cover the full body and are kept across
    :meth:`set_subgroup`.  Collision checking does not read them.

    Args:
        limits: ``{joint name: (lower, upper)}`` in metres for the
            virtual base joints and radians otherwise — e.g. widen
            ``Joint_Virtual_X`` beyond the model's ±10 m for a larger
            map, or narrow any joint to keep plans in a smaller range.

    Raises:
        ValueError: For an unknown joint name, or bounds that are not
            finite with ``lower < upper``.  The ranges in effect are
            then left unchanged.
    """
    from autolife_planning._ompl_vamp import OmplVampPlanner
    from autolife_planning.autolife import autolife_robot_config

    names = autolife_robot_config.joint_names
    lower, upper = OmplVampPlanner.default_joint_limits()
    for name, (lo, hi) in (limits or {}).items():
        if name not in names:
            raise ValueError(
                f"Unknown joint {name!r}; the joints are {', '.join(names)}"
            )
        index = names.index(name)
        lower[index], upper[index] = lo, hi
    self._planner.set_joint_limits(lower, upper)

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

default_joint_limits() -> tuple[NDArray[np.float64], NDArray[np.float64]] staticmethod

(lower, upper) limits compiled into the robot model, for all 24 joints in full-body order.

Source code in autolife_planning/_ompl_vamp.pyi

@staticmethod
def default_joint_limits() -> tuple[NDArray[np.float64], NDArray[np.float64]]:
    """``(lower, upper)`` limits compiled into the robot model, for all
    24 joints in full-body order.
    """
    ...

joint_limits() -> tuple[NDArray[np.float64], NDArray[np.float64]]

(lower, upper) limits in effect for all 24 joints, in full-body order. :meth:lower_bounds and :meth:upper_bounds give the same limits for the active joints only.

Source code in autolife_planning/_ompl_vamp.pyi

def joint_limits(self) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
    """``(lower, upper)`` limits in effect for all 24 joints, in
    full-body order.  :meth:`lower_bounds` and :meth:`upper_bounds`
    give the same limits for the active joints only.
    """
    ...

set_joint_limits(lower: NDArray[np.float64], upper: NDArray[np.float64]) -> None

Replace the limits of all 24 joints (full-body order).

The active state space takes its bounds from these limits, now and after every :meth:set_subgroup or :meth:set_full_body. Collision checking does not read them.

Raises:

Type	Description
ValueError	Unless both arrays have 24 finite entries with lower < upper; the limits in effect are then unchanged.

Source code in autolife_planning/_ompl_vamp.pyi

def set_joint_limits(
    self,
    lower: NDArray[np.float64],
    upper: NDArray[np.float64],
) -> None:
    """Replace the limits of all 24 joints (full-body order).

    The active state space takes its bounds from these limits, now
    and after every :meth:`set_subgroup` or :meth:`set_full_body`.
    Collision checking does not read them.

    Raises:
        ValueError: Unless both arrays have 24 finite entries with
            ``lower < upper``; the limits in effect are then unchanged.
    """
    ...

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

Raises:

Type	Description
ValueError	If frozen_config does not have 24 entries or an index is outside [0, 24).

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

    Raises:
        ValueError: If ``frozen_config`` does not have 24 entries or
            an index is outside ``[0, 24)``.
    """
    ...

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