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 10-DOF body configuration.

Source code in wheelchair_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 wheelchair_planning._ompl_vamp import OmplVampPlanner
    from wheelchair_planning.wheelchair import (
        HOME_JOINTS,
        PLANNING_SUBGROUPS,
        wheelchair_robot_config,
    )

    if robot_name not in available_robots():
        raise ValueError(
            f"Unknown robot name '{robot_name}'. Use one of: {available_robots()}"
        )

    if config is None:
        config = PlannerConfig()

    self._config = config
    self._robot_name = robot_name

    # Frozen 10-DOF joint values for any joint not controlled by
    # this planner.  Defaults to HOME_JOINTS, but the caller can
    # pass any 10-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 = wheelchair_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 10-DOF config.

base_config: np.ndarray property

The 10-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. narrow Joint_Virtual_X / Joint_Virtual_Y around the task's workspace (the model's base spans ±10 m).	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 wheelchair_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. narrow
            ``Joint_Virtual_X`` / ``Joint_Virtual_Y`` around the task's
            workspace (the model's base spans ±10 m).

    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 wheelchair_planning._ompl_vamp import OmplVampPlanner
    from wheelchair_planning.wheelchair import wheelchair_robot_config

    names = wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 and costs. The pointcloud is preserved.

Parameters:

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

Source code in wheelchair_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 and costs.  The pointcloud is preserved.

    Args:
        robot_name: Subgroup name from ``PLANNING_SUBGROUPS``, or
            ``"wheelchair"`` for the full 10-DOF body.
        base_config: 10-DOF frozen config for inactive joints.
            Defaults to the previously stored base config.
    """
    from wheelchair_planning.wheelchair import (
        PLANNING_SUBGROUPS,
        wheelchair_robot_config,
    )

    if robot_name not in available_robots():
        raise ValueError(
            f"Unknown robot name '{robot_name}'. Use one of: {available_robots()}"
        )
    if base_config is not None:
        self._base_config = np.asarray(base_config, dtype=np.float64).copy()
    self._robot_name = robot_name

    full_names = wheelchair_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 10-DOF configuration.

Source code in wheelchair_planning/planning/motion_planner.py

def extract_config(self, full_config: np.ndarray) -> np.ndarray:
    """Extract this planner's joints from a full 10-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 10-DOF configuration.

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

Source code in wheelchair_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 10-DOF configuration.

    ``base_config`` defaults to the planner's stored base — the same
    10-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, 10).

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

Source code in wheelchair_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, 10)``.

    ``base_config`` defaults to the planner's stored base — the same
    10-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 wheelchair_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(PlanningStatus.INVALID_START, None, 0, float("inf"))
    if not self._planner.validate(goal.tolist()):
        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.tolist(),
        goal.tolist(),
        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, dtype=np.float64),
        result.planning_time_ns,
        result.path_cost,
    )

plan_kinodynamic(start: np.ndarray, goal: np.ndarray, start_velocity: np.ndarray | None = None, config: KinodynamicConfig | None = None) -> KinodynamicResult

Plan a dynamically feasible, time-parameterised trajectory (FLASK).

Kinodynamic RRT-Connect in the differentially flat output space (Duong et al., T-RO 2026): every tree edge is the closed-form minimum-effort cubic between two (position, velocity) states, validated with SIMD-batched collision checks and the joint / base velocity and acceleration limits from :mod:wheelchair_planning.wheelchair. For subgroups with the base, the wheelchair follows diff-drive (unicycle) kinematics — heading tangent to the path, forward or reverse, rotate-in-place allowed while parked — so the output can be tracked directly with feed-forward + PID; see :meth:~wheelchair_planning.trajectory.KinodynamicTrajectory.base_twist.

Base positions are sampled inside the planner's joint limits, so narrow Joint_Virtual_X / Joint_Virtual_Y around the task's workspace with :meth:set_joint_limits first. Collision constraints and soft costs registered on this planner are not used by the kinodynamic planner.

Parameters:

Name	Type	Description	Default
start	ndarray	Start configuration (active DOF).	required
goal	ndarray	Goal configuration (active DOF). Reached at rest.	required
start_velocity	ndarray | None	Optional active-DOF velocity at start — e.g. traj.velocity(t) of the trajectory being executed, for replanning on the fly. Base entries are world-frame (x_dot, y_dot, theta_dot); the planar part is projected onto the heading. Defaults to rest.	None
config	KinodynamicConfig | None	Kinodynamic planner parameters.	None

Returns:

Name	Type	Description
A	KinodynamicResult	class:~wheelchair_planning.types.KinodynamicResult whose
	KinodynamicResult	trajectory is a
	KinodynamicResult	class:~wheelchair_planning.trajectory.KinodynamicTrajectory.

Source code in wheelchair_planning/planning/motion_planner.py

def plan_kinodynamic(
    self,
    start: np.ndarray,
    goal: np.ndarray,
    start_velocity: np.ndarray | None = None,
    config: KinodynamicConfig | None = None,
) -> KinodynamicResult:
    """Plan a dynamically feasible, time-parameterised trajectory (FLASK).

    Kinodynamic RRT-Connect in the differentially flat output space
    (Duong et al., T-RO 2026): every tree edge is the closed-form
    minimum-effort cubic between two (position, velocity) states,
    validated with SIMD-batched collision checks and the joint /
    base velocity and acceleration limits from
    :mod:`wheelchair_planning.wheelchair`.  For subgroups with the
    base, the wheelchair follows diff-drive (unicycle) kinematics —
    heading tangent to the path, forward or reverse, rotate-in-place
    allowed while parked — so the output can be tracked directly
    with feed-forward + PID; see
    :meth:`~wheelchair_planning.trajectory.KinodynamicTrajectory.base_twist`.

    Base positions are sampled inside the planner's joint limits, so
    narrow ``Joint_Virtual_X`` / ``Joint_Virtual_Y`` around the task's
    workspace with :meth:`set_joint_limits` first.
    Collision constraints and soft costs registered on this planner
    are not used by the kinodynamic planner.

    Args:
        start: Start configuration (active DOF).
        goal: Goal configuration (active DOF).  Reached at rest.
        start_velocity: Optional active-DOF velocity at ``start`` —
            e.g. ``traj.velocity(t)`` of the trajectory being
            executed, for replanning on the fly.  Base entries are
            world-frame ``(x_dot, y_dot, theta_dot)``; the planar
            part is projected onto the heading.  Defaults to rest.
        config: Kinodynamic planner parameters.

    Returns:
        A :class:`~wheelchair_planning.types.KinodynamicResult` whose
        ``trajectory`` is a
        :class:`~wheelchair_planning.trajectory.KinodynamicTrajectory`.
    """
    from wheelchair_planning.trajectory import KinodynamicTrajectory

    if config is None:
        config = KinodynamicConfig()
    start = np.asarray(start, dtype=np.float64)
    goal = np.asarray(goal, dtype=np.float64)
    velocity = [] if start_velocity is None else np.asarray(start_velocity).tolist()

    for q, status in (
        (start, PlanningStatus.INVALID_START),
        (goal, PlanningStatus.INVALID_GOAL),
    ):
        if not self._planner.validate(q.tolist()):
            return KinodynamicResult(status, None, 0, 0, float("inf"))

    r = self._planner.plan_kinodynamic(
        start.tolist(),
        velocity,
        goal.tolist(),
        config.time_limit,
        self._kinodynamic_settings(config),
    )
    return KinodynamicResult(
        status=PlanningStatus.SUCCESS if r.solved else PlanningStatus.FAILED,
        trajectory=KinodynamicTrajectory(r.trajectory) if r.solved else None,
        planning_time_ns=r.planning_time_ns,
        iterations=r.iterations,
        cost=r.cost,
        simplify_time_ns=r.simplify_time_ns,
        start_tree_size=r.start_tree_size,
        goal_tree_size=r.goal_tree_size,
        edges_checked=r.edges_checked,
    )

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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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(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 wheelchair_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 = 'wheelchair', 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.	'wheelchair'
config	PlannerConfig | None	Planner configuration (uses defaults if None).	None
pointcloud	ndarray | None	(N, 3) obstacle point cloud (optional).	None
base_config	ndarray | None	10-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 10-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 "wheelchair" planner.	None
constraints	list | None	Optional list of :class:~wheelchair_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:~wheelchair_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 wheelchair_planning/planning/motion_planner.py

def create_planner(
    robot_name: str = "wheelchair",
    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: 10-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 10-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 ``"wheelchair"`` planner.
        constraints: Optional list of
            :class:`~wheelchair_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:`~wheelchair_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 wheelchair_planning/planning/motion_planner.py

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

    return ["wheelchair"] + 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/wheelchair_planning/constraints/<sha[:2]>/<sha[2:]>/

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 and builds symbolic link poses from the robot URDF.

The context hides the planar-root encoding (the 10-DOF body vector uses [x, y, theta, j0..j6], with the base applied as a planar joint at the URDF root link) and the mapping from the active subspace back to the full 10-DOF body via base_config.

Source code in wheelchair_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(wheelchair_robot_config.joint_names)
    if subgroup == "wheelchair":
        self.active_indices = list(range(len(full_names)))
        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 = full_names
    self._joints, self._root_link = _parse_urdf(wheelchair_robot_config.urdf_path)
    self._fk = self._build_fk(self.q)
    self._numeric_fk: dict[str, ca.Function] = {}

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

Return the symbolic world pose of a URDF link.

The result exposes translation (3x1) and rotation (3x3) CasADi expressions. Pass q_active=self.q (or omit) to get an expression that depends on the active joints symbolically.

Source code in wheelchair_planning/planning/symbolic.py

def link_pose(self, link_name: str, q_active: ca.SX | None = None) -> _Pose:
    """Return the symbolic world pose of a URDF link.

    The result exposes ``translation`` (3x1) and ``rotation`` (3x3)
    CasADi expressions.  Pass ``q_active=self.q`` (or omit) to get
    an expression that depends on the active joints symbolically.
    """
    if q_active is None or q_active is self.q:
        return self._fk.pose(link_name)
    return self._build_fk(q_active).pose(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 wheelchair_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 wheelchair_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).

Source code in wheelchair_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)."""
    fn = self._numeric_fk.get(link_name)
    if fn is None:
        fn = ca.Function(
            f"fk_{link_name}", [self.q], [self.link_pose(link_name).homogeneous]
        )
        self._numeric_fk[link_name] = fn
    q = np.asarray(q_active_numeric, dtype=np.float64).reshape(-1)
    return np.asarray(fn(q), 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 wheelchair_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/wheelchair_planning/costs/<sha[:2]>/<sha[2:]>/``

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, 10 DOF (3 base + 7 arm 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 10-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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_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 wheelchair_planning/_ompl_vamp.pyi

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

num_constraints() -> int

Number of constraints currently registered.

Source code in wheelchair_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	Non-negative scalar multiplier applied to the cost.	1.0

Source code in wheelchair_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: Non-negative scalar multiplier applied to the cost.
    """
    ...

clear_costs() -> None

Drop every accumulated cost.

Source code in wheelchair_planning/_ompl_vamp.pyi

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

num_costs() -> int

Number of costs currently registered.

Source code in wheelchair_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, 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 by resolution.	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 wheelchair_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,
    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 by
            ``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.
    """
    ...

plan_kinodynamic(start: Sequence[float], start_velocity: Sequence[float], goal: Sequence[float], time_limit: float, settings: KinodynamicSettings) -> KinodynamicResult

FLASK kinodynamic planning in the flat output space.

Parameters:

Name	Type	Description	Default
start	Sequence[float]	Active-DOF start configuration.	required
start_velocity	Sequence[float]	Active-DOF start velocity, or empty for rest. Base entries are world-frame (x_dot, y_dot, theta_dot).	required
goal	Sequence[float]	Active-DOF goal configuration, reached at rest.	required
time_limit	float	Solver time limit in seconds.	required
settings	KinodynamicSettings	Limits and planner parameters.	required

Source code in wheelchair_planning/_ompl_vamp.pyi

def plan_kinodynamic(
    self,
    start: Sequence[float],
    start_velocity: Sequence[float],
    goal: Sequence[float],
    time_limit: float,
    settings: KinodynamicSettings,
) -> KinodynamicResult:
    """FLASK kinodynamic planning in the flat output space.

    Args:
        start: Active-DOF start configuration.
        start_velocity: Active-DOF start velocity, or empty for rest.
            Base entries are world-frame ``(x_dot, y_dot, theta_dot)``.
        goal: Active-DOF goal configuration, reached at rest.
        time_limit: Solver time limit in seconds.
        settings: Limits and planner parameters.
    """
    ...

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 wheelchair_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 wheelchair_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 10-DOF state with the stored frozen_config before checking.

Source code in wheelchair_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 10-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 10-DOF state with the stored frozen_config before packing.

Source code in wheelchair_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 10-DOF state with
    the stored ``frozen_config`` before packing.
    """
    ...

dimension() -> int

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

Source code in wheelchair_planning/_ompl_vamp.pyi

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

lower_bounds() -> list[float]

Per-joint lower bounds for the active DOFs.

Source code in wheelchair_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 wheelchair_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 wheelchair_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.
    """
    ...

default_joint_limits() -> tuple[list[float], list[float]] staticmethod

The (lower, upper) limits compiled into the robot model, in full-body order.

Source code in wheelchair_planning/_ompl_vamp.pyi

@staticmethod
def default_joint_limits() -> tuple[list[float], list[float]]:
    """The ``(lower, upper)`` limits compiled into the robot model, in
    full-body order."""
    ...

joint_limits() -> tuple[list[float], list[float]]

The (lower, upper) limits in effect, in full-body order.

Source code in wheelchair_planning/_ompl_vamp.pyi

def joint_limits(self) -> tuple[list[float], list[float]]:
    """The ``(lower, upper)`` limits in effect, in full-body order."""
    ...

set_joint_limits(lower: Sequence[float], upper: Sequence[float]) -> None

Replace the limits of every joint (full-body order); each needs finite bounds with lower < upper. Persists across :meth:set_subgroup / :meth:set_full_body.

Source code in wheelchair_planning/_ompl_vamp.pyi

def set_joint_limits(self, lower: Sequence[float], upper: Sequence[float]) -> None:
    """Replace the limits of every joint (full-body order); each needs
    finite bounds with ``lower < upper``.  Persists across
    :meth:`set_subgroup` / :meth:`set_full_body`."""
    ...

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

Spatially downsample points via Morton-curve sorting.

Keeps one point per min_dist neighbourhood and, with cull, drops points farther than max_range from origin or outside [workspace_min, workspace_max].

Source code in wheelchair_planning/_ompl_vamp.pyi

def filter_pointcloud(
    self,
    points: Sequence[Sequence[float]],
    min_dist: float,
    max_range: float,
    origin: Sequence[float],
    workspace_min: Sequence[float],
    workspace_max: Sequence[float],
    cull: bool = True,
) -> list[list[float]]:
    """Spatially downsample ``points`` via Morton-curve sorting.

    Keeps one point per ``min_dist`` neighbourhood and, with
    ``cull``, drops points farther than ``max_range`` from ``origin``
    or outside ``[workspace_min, workspace_max]``.
    """
    ...

filter_self_from_pointcloud(points: Sequence[Sequence[float]], point_radius: float, config: Sequence[float]) -> list[list[float]]

Drop the points whose sphere of radius point_radius overlaps a robot collision sphere at config (active DOF) or an obstacle of the environment.

Source code in wheelchair_planning/_ompl_vamp.pyi

def filter_self_from_pointcloud(
    self,
    points: Sequence[Sequence[float]],
    point_radius: float,
    config: Sequence[float],
) -> list[list[float]]:
    """Drop the points whose sphere of radius ``point_radius`` overlaps
    a robot collision sphere at ``config`` (active DOF) or an obstacle
    of the environment."""
    ...

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

Switch to a different subgroup without rebuilding the environment.

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

Parameters:

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

Raises:

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

Source code in wheelchair_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 and costs.  The pointcloud and collision
    geometry are preserved.

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

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

set_full_body() -> None

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

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

Source code in wheelchair_planning/_ompl_vamp.pyi

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

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