Motion Planning

Motion Planner

MotionPlanner(robot_name: str, config: PlannerConfig | None = None, pointcloud: np.ndarray | None = None, base_config: np.ndarray | 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 6-DOF body configuration.

Source code in drone_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,
) -> None:
    from drone_planning._ompl_vamp import OmplVampPlanner
    from drone_planning.x500 import HOME_JOINTS

    if config is None:
        config = PlannerConfig()

    self._config = config
    self._robot_name = robot_name

    # Frozen 6-DOF joint values for any joint not controlled by
    # this planner.  Defaults to HOME_JOINTS, but the caller can
    # pass any 6-DOF array — e.g. the live state of the drone — 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}"
        )

    active_indices = self._resolve_subgroup(robot_name)
    self._planner = OmplVampPlanner(active_indices, self._base_config.tolist())
    self._apply_subgroup_state(active_indices)

    if config.joint_limits:
        self.set_joint_limits(config.joint_limits)

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

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

base_config: np.ndarray property

The 6-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. the extent of the map for base_x_joint / base_y_joint / base_z_joint.	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 drone_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. the
            extent of the map for ``base_x_joint`` / ``base_y_joint``
            / ``base_z_joint``.

    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 drone_planning._ompl_vamp import OmplVampPlanner
    from drone_planning.x500 import x500_robot_config

    names = x500_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)

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 drone_planning/planning/motion_planner.py

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

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

remove_pointcloud() -> bool

Drop the currently-registered pointcloud.

Returns False if there was no cloud to remove.

Source code in drone_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 drone_planning/planning/motion_planner.py

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

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

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

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

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

Remove points that collide with the robot body or environment.

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

Parameters:

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

Returns:

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

Source code in drone_planning/planning/motion_planner.py

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

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

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

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

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

Switch active joints without rebuilding the collision environment.

The pointcloud and the joint limits are preserved.

Parameters:

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

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

    The pointcloud and the joint limits are preserved.

    Args:
        robot_name: Subgroup name from ``PLANNING_SUBGROUPS``, or
            ``"x500"`` for the full 6-DOF body.
        base_config: 6-DOF frozen config for inactive joints.
            Defaults to the previously stored base config.
    """
    if base_config is not None:
        self._base_config = np.asarray(base_config, dtype=np.float64).copy()
    self._robot_name = robot_name
    active_indices = self._resolve_subgroup(robot_name)
    self._planner.set_subgroup(active_indices, self._base_config.tolist())
    self._apply_subgroup_state(active_indices)

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

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

Source code in drone_planning/planning/motion_planner.py

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

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

Source code in drone_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 6-DOF configuration.

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

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

Source code in drone_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, 6)``.

    ``base_config`` defaults to the planner's stored base — the same
    6-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 drone_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 not self._planner.validate(start):
        return PlanningResult(PlanningStatus.INVALID_START, None, 0, float("inf"))
    if not self._planner.validate(goal):
        return PlanningResult(PlanningStatus.INVALID_GOAL, None, 0, float("inf"))

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

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

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

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 velocity / acceleration limits from :mod:drone_planning.x500 — the norm of the horizontal velocity and acceleration, the climb and descent rates, the yaw rate and the gimbal rates. The output is the velocity setpoint stream a PX4 offboard controller tracks.

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. Defaults to rest.	None
config	KinodynamicConfig | None	Kinodynamic planner parameters.	None

Returns:

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

Source code in drone_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 velocity /
    acceleration limits from :mod:`drone_planning.x500` — the norm of
    the horizontal velocity and acceleration, the climb and descent
    rates, the yaw rate and the gimbal rates.  The output is the
    velocity setpoint stream a PX4 offboard controller tracks.

    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.  Defaults to rest.
        config: Kinodynamic planner parameters.

    Returns:
        A :class:`~drone_planning.types.KinodynamicResult` whose
        ``trajectory`` is a
        :class:`~drone_planning.trajectory.KinodynamicTrajectory`.
    """
    from drone_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 = np.zeros(0) if start_velocity is None else start_velocity

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

    r = self._planner.plan_kinodynamic(
        start,
        np.asarray(velocity, dtype=np.float64),
        goal,
        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.

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

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

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

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

Densify path with uniform waypoints along every edge.

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

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

No collision check is performed — the densification only lifts points along the existing (straight) 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 drone_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.

    No collision check is performed — the densification only lifts
    points along the existing (straight) edges.

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

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

validate(configuration: np.ndarray) -> bool

Check if a configuration is collision-free.

Source code in drone_planning/planning/motion_planner.py

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

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

Batched collision check — one SIMD block per rake configs.

Packs rake distinct configurations into a single VAMP ConfigurationBlock<rake> and runs one fkcc<rake> call per block, so N queries cost ceil(N / rake) SIMD sweeps in the common case. When a packed block fails, that block's configurations are re-checked one by one, 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 drone_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, that
    block's configurations are re-checked one by one, so the
    returned array is always exactly one bool per input.

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

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

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

Sample a random collision-free configuration.

Raises:

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

Source code in drone_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 = 'x500', config: PlannerConfig | None = None, pointcloud: np.ndarray | None = None, base_config: np.ndarray | 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 "x500" is an alias for the full whole-body planner (6 DOF: body + gimbal).	'x500'
config	PlannerConfig | None	Planner configuration (uses defaults if None).	None
pointcloud	ndarray | None	(N, 3) obstacle point cloud (optional).	None
base_config	ndarray | None	6-DOF values to inject for joints not controlled by this planner (i.e. the frozen joints of a subgroup — the gimbal for "x500_base"). Defaults to HOME_JOINTS.	None

Returns:

Name	Type	Description
A	MotionPlanner	class:MotionPlanner instance.

Source code in drone_planning/planning/motion_planner.py

def create_planner(
    robot_name: str = "x500",
    config: PlannerConfig | None = None,
    pointcloud: np.ndarray | None = None,
    base_config: np.ndarray | 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 ``"x500"`` is an alias for
            the full whole-body planner (6 DOF: body + gimbal).
        config: Planner configuration (uses defaults if None).
        pointcloud: ``(N, 3)`` obstacle point cloud (optional).
        base_config: 6-DOF values to inject for joints not controlled
            by this planner (i.e. the frozen joints of a subgroup — the
            gimbal for ``"x500_base"``).  Defaults to ``HOME_JOINTS``.

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

Available Robots

available_robots() -> list[str]

Return all available robot names for planning.

Source code in drone_planning/planning/motion_planner.py

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

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

Low-level C++ binding

OmplVampPlanner(active_indices: Sequence[int] = ..., frozen_config: Sequence[float] = ...)

OMPL planner with VAMP SIMD-accelerated collision checking.

Two construction modes:

• OmplVampPlanner() — full body, 6 DOF (x, y, z, yaw, gimbal pan, gimbal tilt).
• OmplVampPlanner(active_indices, frozen_config) — subgroup over the listed joints; the C++ collision checker injects frozen_config for every other joint before every query.

Source code in drone_planning/_ompl_vamp.pyi

def __init__(
    self,
    active_indices: Sequence[int] = ...,
    frozen_config: Sequence[float] = ...,
) -> None: ...

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

Set the scene point cloud, replacing any previous one.

Parameters:

Name	Type	Description	Default
points	NDArray[float32]	(N, 3) float32 obstacle positions in world frame.	required
point_radius	float	Inflation radius of every point (0 checks them as points: the robot's spheres already cover its meshes).	required

Source code in drone_planning/_ompl_vamp.pyi

def add_pointcloud(self, points: NDArray[np.float32], point_radius: float) -> None:
    """Set the scene point cloud, replacing any previous one.

    Args:
        points: ``(N, 3)`` float32 obstacle positions in world frame.
        point_radius: Inflation radius of every point (0 checks them
            as points: the robot's spheres already cover its meshes).
    """
    ...

remove_pointcloud() -> bool

Drop the point cloud. Returns False if there was none.

Source code in drone_planning/_ompl_vamp.pyi

def remove_pointcloud(self) -> bool:
    """Drop the point cloud.  Returns ``False`` if there was none."""
    ...

has_pointcloud() -> bool

True if a point cloud is registered.

Source code in drone_planning/_ompl_vamp.pyi

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

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

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

Source code in drone_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 drone_planning/_ompl_vamp.pyi

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

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

Plan a collision-free path from start to goal.

Parameters:

Name	Type	Description	Default
start	NDArray[float64]	Active-DOF start configuration (length :meth:dimension).	required
goal	NDArray[float64]	Active-DOF goal configuration (length :meth:dimension).	required
planner_name	str	OMPL planner identifier (e.g. "rrtc", "rrtstar", "prm", "bitstar").	'rrtc'
time_limit	float	Solver time limit in seconds.	10.0
simplify	bool	If true, run SimpleSetup::simplifySolution on the returned path.	True
interpolate	bool	If true, densify the simplified path: ceil(d * resolution) waypoints per edge of length d when resolution > 0, else OMPL's default.	True
resolution	float	Waypoints per unit of state-space distance.	64.0

Source code in drone_planning/_ompl_vamp.pyi

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

    Args:
        start: Active-DOF start configuration (length :meth:`dimension`).
        goal: Active-DOF goal configuration (length :meth:`dimension`).
        planner_name: OMPL planner identifier (e.g. ``"rrtc"``,
            ``"rrtstar"``, ``"prm"``, ``"bitstar"``).
        time_limit: Solver time limit in seconds.
        simplify: If true, run ``SimpleSetup::simplifySolution`` on the
            returned path.
        interpolate: If true, densify the simplified path:
            ``ceil(d * resolution)`` waypoints per edge of length ``d``
            when ``resolution > 0``, else OMPL's default.
        resolution: Waypoints per unit of state-space distance.
    """
    ...

plan_kinodynamic(start: NDArray[np.float64], start_velocity: NDArray[np.float64], goal: NDArray[np.float64], time_limit: float, settings: KinodynamicSettings) -> KinodynamicResult

FLASK kinodynamic planning in the flat output space.

Parameters:

Name	Type	Description	Default
start	NDArray[float64]	Active-DOF start configuration.	required
start_velocity	NDArray[float64]	Active-DOF start velocity, or empty for rest.	required
goal	NDArray[float64]	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 drone_planning/_ompl_vamp.pyi

def plan_kinodynamic(
    self,
    start: NDArray[np.float64],
    start_velocity: NDArray[np.float64],
    goal: NDArray[np.float64],
    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.
        goal: Active-DOF goal configuration, reached at rest.
        time_limit: Solver time limit in seconds.
        settings: Limits and planner parameters.
    """
    ...

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

Run OMPL's shortcut simplifier on an (N, dimension()) path against the current collision environment.

Source code in drone_planning/_ompl_vamp.pyi

def simplify_path(
    self, path: NDArray[np.float64], time_limit: float = 1.0
) -> NDArray[np.float64]:
    """Run OMPL's shortcut simplifier on an ``(N, dimension())`` path
    against the current collision environment."""
    ...

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

Densify an (N, dimension()) path along its straight edges (count total waypoints, or resolution per unit distance); no collision check.

Source code in drone_planning/_ompl_vamp.pyi

def interpolate_path(
    self, path: NDArray[np.float64], count: int = 0, resolution: float = 64.0
) -> NDArray[np.float64]:
    """Densify an ``(N, dimension())`` path along its straight edges
    (``count`` total waypoints, or ``resolution`` per unit distance);
    no collision check."""
    ...

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

True if the active-DOF config is collision-free.

Source code in drone_planning/_ompl_vamp.pyi

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

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

Per-configuration collision check of (N, dimension()) configurations, packed rake at a time into one SIMD sweep.

Source code in drone_planning/_ompl_vamp.pyi

def validate_batch(self, configs: NDArray[np.float64]) -> NDArray[np.bool_]:
    """Per-configuration collision check of ``(N, dimension())``
    configurations, packed ``rake`` at a time into one SIMD sweep."""
    ...

dimension() -> int

Number of active joints — 6 for the full body.

Source code in drone_planning/_ompl_vamp.pyi

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

lower_bounds() -> NDArray[np.float64]

Lower joint limits of the active DOF.

Source code in drone_planning/_ompl_vamp.pyi

def lower_bounds(self) -> NDArray[np.float64]:
    """Lower joint limits of the active DOF."""
    ...

upper_bounds() -> NDArray[np.float64]

Upper joint limits of the active DOF.

Source code in drone_planning/_ompl_vamp.pyi

def upper_bounds(self) -> NDArray[np.float64]:
    """Upper joint limits of the active DOF."""
    ...

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

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

Source code in drone_planning/_ompl_vamp.pyi

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

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

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

Source code in drone_planning/_ompl_vamp.pyi

def joint_limits(self) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
    """``(lower, upper)`` limits in effect, full-body order."""
    ...

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

Replace the limits of every joint (full-body order; finite, lower < upper). They survive :meth:set_subgroup.

Source code in drone_planning/_ompl_vamp.pyi

def set_joint_limits(
    self, lower: NDArray[np.float64], upper: NDArray[np.float64]
) -> None:
    """Replace the limits of every joint (full-body order; finite,
    ``lower < upper``).  They survive :meth:`set_subgroup`."""
    ...

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

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

Source code in drone_planning/_ompl_vamp.pyi

def filter_pointcloud(
    self,
    points: NDArray[np.float32],
    min_dist: float,
    max_range: float,
    origin: Sequence[float],
    workspace_min: Sequence[float],
    workspace_max: Sequence[float],
    cull: bool = True,
) -> NDArray[np.float32]:
    """Morton-curve spatial down-sampling of a point cloud."""
    ...

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

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

Source code in drone_planning/_ompl_vamp.pyi

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

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

Switch active joints without rebuilding the collision environment.

Parameters:

Name	Type	Description	Default
active_indices	Sequence[int]	Positions in the full-body config that this planner plans over, in DOF order.	required
frozen_config	Sequence[float]	Full-body stance (6 values) for every joint not in active_indices.	required

Raises:

Type	Description
ValueError	For an index outside [0, 6) or a frozen_config without 6 entries (the subgroup constructor raises the same).

Source code in drone_planning/_ompl_vamp.pyi

def set_subgroup(
    self, active_indices: Sequence[int], frozen_config: Sequence[float]
) -> None:
    """Switch active joints without rebuilding the collision environment.

    Args:
        active_indices: Positions in the full-body config that this
            planner plans over, in DOF order.
        frozen_config: Full-body stance (6 values) for every joint
            *not* in ``active_indices``.

    Raises:
        ValueError: For an index outside ``[0, 6)`` or a
            ``frozen_config`` without 6 entries (the subgroup
            constructor raises the same).
    """
    ...

KinodynamicSettings()

Parameters of OmplVampPlanner.plan_kinodynamic (FLASK).

See ext/ompl_vamp/plan_kinodynamic.hpp for field semantics; :class:drone_planning.types.KinodynamicConfig fills them from the robot limits in :mod:drone_planning.x500. min_* / max_* hold one value per active DOF; the horizontal pair (base_x_joint, base_y_joint) is also bounded in norm when both are active.

Source code in drone_planning/_ompl_vamp.pyi

def __init__(self) -> None: ...