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

Source code in fetch_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 fetch_planning._ompl_vamp import OmplVampPlanner
    from fetch_planning.fetch import (
        BASE_REVERSE_ENABLE,
        BASE_TURNING_RADIUS,
        HOME_JOINTS,
        JOINT_GROUPS,
        PLANNING_SUBGROUPS,
        fetch_robot_config,
    )

    if config is None:
        config = PlannerConfig()

    self._config = config
    self._robot_name = robot_name
    self._turning_radius = BASE_TURNING_RADIUS
    self._allow_reverse = BASE_REVERSE_ENABLE

    # Frozen 11-DOF joint values for any joint not controlled by
    # this planner.  Defaults to HOME_JOINTS, but the caller can
    # pass any 11-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 = fetch_robot_config.joint_names
    base_slice = JOINT_GROUPS["base"]
    self._base_joint_names = set(full_names[base_slice])

    active_indices, base_dim = self._resolve_subgroup(
        robot_name, full_names, PLANNING_SUBGROUPS
    )

    self._planner = OmplVampPlanner(
        active_indices,
        self._base_config.tolist(),
        base_dim,
        BASE_TURNING_RADIUS,
        BASE_REVERSE_ENABLE,
    )
    self._apply_subgroup_state(full_names, active_indices, base_dim)

    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 11-DOF config.

base_config: np.ndarray property

The 11-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. While the base is active, base_theta_joint's range is not used: the Dubins / Reeds-Shepp heading is periodic.

Parameters:

Name	Type	Description	Default
limits	Mapping[str, tuple[float, float]] | None	{joint name: (lower, upper)} in metres for base_x_joint, base_y_joint and the torso and radians otherwise — e.g. bound the base to the map instead of the model's ±10 m, 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 fetch_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.
    While the base is active, ``base_theta_joint``'s range is not
    used: the Dubins / Reeds-Shepp heading is periodic.

    Args:
        limits: ``{joint name: (lower, upper)}`` in metres for
            ``base_x_joint``, ``base_y_joint`` and the torso and
            radians otherwise — e.g. bound the base to the map
            instead of the model's ±10 m, 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 fetch_planning._ompl_vamp import OmplVampPlanner
    from fetch_planning.fetch import fetch_robot_config

    names = fetch_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 fetch_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 fetch_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 fetch_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 fetch_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 fetch_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 fetch_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 fetch_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 fetch_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 "fetch" / "fetch_whole_body" for the full 11-DOF body.	required
base_config	ndarray | None	11-DOF frozen config for inactive joints. Defaults to the previously stored base config.	None

Source code in fetch_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
            ``"fetch"`` / ``"fetch_whole_body"`` for the full 11-DOF
            body.
        base_config: 11-DOF frozen config for inactive joints.
            Defaults to the previously stored base config.
    """
    from fetch_planning.fetch import PLANNING_SUBGROUPS, fetch_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 = fetch_robot_config.joint_names
    active_indices, base_dim = self._resolve_subgroup(
        robot_name, full_names, PLANNING_SUBGROUPS
    )
    self._planner.set_subgroup(active_indices, self._base_config.tolist(), base_dim)
    self._apply_subgroup_state(full_names, active_indices, base_dim)

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

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

Source code in fetch_planning/planning/motion_planner.py

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

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

Source code in fetch_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 11-DOF configuration.

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

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

Source code in fetch_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, 11)``.

    ``base_config`` defaults to the planner's stored base — the same
    11-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 fetch_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:fetch_planning.fetch. For subgroups with the mobile base the base follows 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:~fetch_planning.trajectory.KinodynamicTrajectory.base_twist.

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:~fetch_planning.types.KinodynamicResult whose
	KinodynamicResult	trajectory is a
	KinodynamicResult	class:~fetch_planning.trajectory.KinodynamicTrajectory.

Source code in fetch_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:`fetch_planning.fetch`.  For subgroups with the mobile base
    the base follows 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:`~fetch_planning.trajectory.KinodynamicTrajectory.base_twist`.

    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:`~fetch_planning.types.KinodynamicResult` whose
        ``trajectory`` is a
        :class:`~fetch_planning.trajectory.KinodynamicTrajectory`.
    """
    from fetch_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 fetch_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 and on the Dubins / Reeds-Shepp curve for compound base+arm paths. No collision check is performed — the densification only lifts points along the existing 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 fetch_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 and on the Dubins / Reeds-Shepp curve for compound
    base+arm paths.  No collision check is performed — the
    densification only lifts points along the existing 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 fetch_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 fetch_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 fetch_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 = 'fetch', 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. The default "fetch" is an alias for the full whole-body planner (11 DOF, nonholonomic base).	'fetch'
config	PlannerConfig | None	Planner configuration (uses defaults if None).	None
pointcloud	ndarray | None	(N, 3) obstacle point cloud (optional).	None
base_config	ndarray | None	11-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 11-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 "fetch" / "fetch_whole_body" planners.	None
constraints	list | None	Optional list of :class:~fetch_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. Not supported for subgroups that include mobile-base joints.	None
costs	list | None	Optional list of :class:~fetch_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. For subgroups with the base the objective keeps the Dubins / Reeds-Shepp path length — user costs add on top, preserving the non-holonomic shaping. 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 fetch_planning/planning/motion_planner.py

def create_planner(
    robot_name: str = "fetch",
    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. The default ``"fetch"`` is an alias for
            the full whole-body planner (11 DOF, nonholonomic base).
        config: Planner configuration (uses defaults if None).
        pointcloud: ``(N, 3)`` obstacle point cloud (optional).
        base_config: 11-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 11-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 ``"fetch"`` / ``"fetch_whole_body"`` planners.
        constraints: Optional list of
            :class:`~fetch_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.
            Not supported for subgroups that include mobile-base joints.
        costs: Optional list of
            :class:`~fetch_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.  For
            subgroups with the base the objective keeps
            the Dubins / Reeds-Shepp path length — user costs add on top,
            preserving the non-holonomic shaping.
            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 fetch_planning/planning/motion_planner.py

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

    return ["fetch"] + 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/fetch_planning/constraints/<sha>/

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

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

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

Holds the CasADi symbolic joint vector q matching the subgroup's active dimension, plus a pinocchio.casadi model for symbolic FK.

Fetch's whole-body URDF encodes the base as three separate 1-DOF virtual joints (prismatic x, prismatic y, revolute z) so pinocchio.nq == 11 and the active→pinocchio mapping is a straight copy (no trig encoding).

Source code in fetch_planning/planning/constraints.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(fetch_robot_config.joint_names)
    if subgroup in ("fetch", "fetch_whole_body"):
        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))

    # Numeric + symbolic pinocchio models — loaded from the whole-body
    # URDF so the 3 base joints are real kinematic DOFs of the tree.
    self.pin_model = pin.buildModelFromUrdf(WHOLE_BODY_URDF_PATH)
    self.pin_data = self.pin_model.createData()
    self.cmodel = cpin.Model(self.pin_model)
    self.cdata = self.cmodel.createData()

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

    # Pre-run symbolic FK so users can access frame transforms.
    cpin.forwardKinematics(self.cmodel, self.cdata, self.q_pin)
    cpin.updateFramePlacements(self.cmodel, self.cdata)

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

Return the symbolic pinocchio SE3 of a URDF link.

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

Source code in fetch_planning/planning/constraints.py

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

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

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

Symbolic 3-vector: link position in world frame.

Source code in fetch_planning/planning/constraints.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 fetch_planning/planning/constraints.py

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

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

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

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

Source code in fetch_planning/planning/constraints.py

def evaluate_link_pose(
    self, link_name: str, q_active_numeric: np.ndarray
) -> np.ndarray:
    """Compute a NUMERIC 4x4 link pose (handy for building targets).

    Uses the numeric pinocchio model, not the symbolic one, so it is
    fast and has no dependence on CasADi expressions.
    """
    full = self.base_config.copy()
    for i, idx in enumerate(self.active_indices):
        full[idx] = q_active_numeric[i]
    # Fetch's whole-body URDF uses three separate 1-DOF base joints
    # (prismatic x, prismatic y, revolute z) so pinocchio nq == 11
    # and the mapping is a straight copy — no trig encoding needed.
    pin.forwardKinematics(self.pin_model, self.pin_data, full)
    pin.updateFramePlacement(
        self.pin_model,
        self.pin_data,
        self.pin_model.getFrameId(link_name),
    )
    pose = self.pin_data.oMf[self.pin_model.getFrameId(link_name)]
    M = np.eye(4)
    M[:3, :3] = pose.rotation
    M[:3, 3] = pose.translation
    return M

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 fetch_planning/planning/constraints.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/fetch_planning/costs/<sha>/``

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

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

Low-level C++ binding

OmplVampPlanner(active_indices: Sequence[int], frozen_config: Sequence[float], base_dim: int = 0, turning_radius: float = 0.2, allow_reverse: bool = False)

OMPL planner with VAMP SIMD-accelerated collision checking.

Single construction mode — the Python wrapper decides which joints are active and how many form the nonholonomic base:

• OmplVampPlanner(active_indices, frozen_config, base_dim, turning_radius, allow_reverse)

The C++ collision checker injects frozen_config for every slot in the 11-DOF whole-body state that is not listed in active_indices before running every state or motion validity query.

Create a subgroup planner.

Parameters:

Name	Type	Description	Default
active_indices	Sequence[int]	Positions in the full-body config that this planner will plan over, in DOF order.	required
frozen_config	Sequence[float]	Full-body stance to inject for every joint not in active_indices.	required
base_dim	int	How many leading active indices form the nonholonomic base (0 = arm-only, 3 = SE2 base).	0
turning_radius	float	Minimum turning radius in metres (ignored when base_dim == 0).	0.2
allow_reverse	bool	Select the SE(2) base state space. False (default) picks DubinsStateSpace (forward-only); True picks ReedsSheppStateSpace (reverse permitted).	False

Raises:

Type	Description
ValueError	If frozen_config does not have 11 entries, an index is outside [0, 11), or base_dim is neither 0 nor 3 with the base joints 0, 1, 2 leading active_indices.

Source code in fetch_planning/_ompl_vamp.pyi

def __init__(
    self,
    active_indices: Sequence[int],
    frozen_config: Sequence[float],
    base_dim: int = 0,
    turning_radius: float = 0.2,
    allow_reverse: bool = False,
) -> None:
    """Create a subgroup planner.

    Args:
        active_indices: Positions in the full-body config that this
            planner will plan over, in DOF order.
        frozen_config: Full-body stance to inject for every joint *not*
            in ``active_indices``.
        base_dim: How many leading active indices form the
            nonholonomic base (0 = arm-only, 3 = SE2 base).
        turning_radius: Minimum turning radius in metres (ignored
            when ``base_dim == 0``).
        allow_reverse: Select the SE(2) base state space.  ``False``
            (default) picks ``DubinsStateSpace`` (forward-only); ``True``
            picks ``ReedsSheppStateSpace`` (reverse permitted).

    Raises:
        ValueError: If ``frozen_config`` does not have 11 entries, an
            index is outside ``[0, 11)``, or ``base_dim`` is neither 0
            nor 3 with the base joints 0, 1, 2 leading
            ``active_indices``.
    """
    ...

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

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

Source code in fetch_planning/_ompl_vamp.pyi

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

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

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

Source code in fetch_planning/_ompl_vamp.pyi

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

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

Replace the range of every joint (full-body order).

Each joint needs finite bounds with lower < upper; otherwise ValueError and nothing changes. The ranges bound the active state space (the base x / y range the SE(2) space, whose heading is periodic) and survive :meth:set_subgroup.

Source code in fetch_planning/_ompl_vamp.pyi

def set_joint_limits(self, lower: Sequence[float], upper: Sequence[float]) -> None:
    """Replace the range of every joint (full-body order).

    Each joint needs finite bounds with ``lower < upper``; otherwise
    ``ValueError`` and nothing changes.  The ranges bound the active
    state space (the base x / y range the SE(2) space, whose heading
    is periodic) and survive :meth:`set_subgroup`.
    """
    ...

add_pointcloud(points: Sequence[Sequence[float]], r_min: float, r_max: float, point_radius: float) -> None

Set the scene point cloud, replacing any registered one.

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 fetch_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 point cloud, replacing any registered one.

    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 registered pointcloud.

Returns False if there was none to remove.

Source code in fetch_planning/_ompl_vamp.pyi

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

    Returns ``False`` if there was none to remove.
    """
    ...

has_pointcloud() -> bool

True if a pointcloud is currently registered.

Source code in fetch_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 fetch_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 fetch_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 fetch_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 fetch_planning/_ompl_vamp.pyi

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

num_constraints() -> int

Number of constraints currently registered.

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

For whole-body / base-including subgroups the cost is combined with the Dubins / Reeds-Shepp path-length term (which lives in the state-space distance) — non-holonomic shaping is preserved.

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

    For whole-body / base-including subgroups the cost is combined
    with the Dubins / Reeds-Shepp path-length term (which lives in
    the state-space distance) — non-holonomic shaping is preserved.

    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 fetch_planning/_ompl_vamp.pyi

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

num_costs() -> int

Number of costs currently registered.

Source code in fetch_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 when it is > 0, otherwise by OMPL's default longest-valid-segment fraction.	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 fetch_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`` when it is ``> 0``, otherwise by OMPL's
            default longest-valid-segment fraction.
        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 fetch_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 fetch_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, so the inserted waypoints stay on the Dubins / Reeds-Shepp curve for compound base+arm paths.

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 fetch_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, so the inserted waypoints stay on the
    Dubins / Reeds-Shepp curve for compound base+arm paths.

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

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

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

dimension() -> int

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

Source code in fetch_planning/_ompl_vamp.pyi

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

lower_bounds() -> list[float]

Per-joint lower bounds for the active DOFs (the joint limits).

Source code in fetch_planning/_ompl_vamp.pyi

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

upper_bounds() -> list[float]

Per-joint upper bounds for the active DOFs.

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

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]]

Morton-curve spatial down-sampling of a point cloud.

Keeps one representative point per min_dist neighbourhood and discards points outside the AABB / range envelope.

Source code in fetch_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]]:
    """Morton-curve spatial down-sampling of a point cloud.

    Keeps one representative point per ``min_dist`` neighbourhood
    and discards points outside the AABB / range envelope.
    """
    ...

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

Remove points that collide with the robot body at config or with the registered environment.

Source code in fetch_planning/_ompl_vamp.pyi

def filter_self_from_pointcloud(
    self,
    points: Sequence[Sequence[float]],
    point_radius: float,
    config: Sequence[float],
) -> list[list[float]]:
    """Remove points that collide with the robot body at ``config``
    or with the registered environment."""
    ...

set_subgroup(active_indices: Sequence[int], frozen_config: Sequence[float], base_dim: int = 0) -> 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-body config that this planner will plan over, in DOF order.	required
frozen_config	Sequence[float]	Full-body stance to inject for every joint not in active_indices.	required
base_dim	int	How many leading active indices form the nonholonomic base (0 = arm-only, 3 = SE2 base).	0

Raises:

Type	Description
ValueError	If frozen_config does not have 11 entries, an index is outside [0, 11), or base_dim is neither 0 nor 3 with the base joints 0, 1, 2 leading active_indices.

Source code in fetch_planning/_ompl_vamp.pyi

def set_subgroup(
    self,
    active_indices: Sequence[int],
    frozen_config: Sequence[float],
    base_dim: int = 0,
) -> 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-body config that this
            planner will plan over, in DOF order.
        frozen_config: Full-body stance to inject for every joint *not*
            in ``active_indices``.
        base_dim: How many leading active indices form the
            nonholonomic base (0 = arm-only, 3 = SE2 base).

    Raises:
        ValueError: If ``frozen_config`` does not have 11 entries, an
            index is outside ``[0, 11)``, or ``base_dim`` is neither 0
            nor 3 with the base joints 0, 1, 2 leading
            ``active_indices``.
    """
    ...