Trajectory

TimeOptimalParameterizer

TimeOptimalParameterizer(max_velocity: np.ndarray, max_acceleration: np.ndarray, knot_spacing: float = 0.1)

Time-optimal path parameteriser with fixed joint limits.

The piecewise-linear path is resampled every knot_spacing and joined by a cubic spline, and TOPP-RA finds the fastest velocity profile along it that starts and ends at rest. The spline passes through every waypoint and deviates from the straight segments by about knot_spacing / 10, rounding the corners.

Parameters:

Name	Type	Description	Default
max_velocity	ndarray	(ndof,) per-joint velocity bound. Units must match the path (rad/s for revolute joints, m/s for prismatic).	required
max_acceleration	ndarray	(ndof,) per-joint acceleration bound.	required
knot_spacing	float	Spline knot spacing along the path (path units). Smaller follows the corners more tightly, at the cost of slower cornering and more computation.	0.1

Source code in nav_planning/trajectory/toppra.py

def __init__(
    self,
    max_velocity: np.ndarray,
    max_acceleration: np.ndarray,
    knot_spacing: float = 0.1,
):
    self.max_velocity = np.asarray(max_velocity, dtype=np.float64).reshape(-1)
    self.max_acceleration = np.asarray(max_acceleration, dtype=np.float64).reshape(
        -1
    )
    self.knot_spacing = float(knot_spacing)

parameterize(path: np.ndarray, velocity_scaling: float = 1.0, acceleration_scaling: float = 1.0) -> Trajectory

Time-parameterize an (N, ndof) joint-space path.

Parameters:

Name	Type	Description	Default
path	ndarray	(N, ndof) waypoints, at least two distinct.	required
velocity_scaling	float	Factor in (0, 1] on the velocity limit.	1.0
acceleration_scaling	float	Factor in (0, 1] on the acceleration limit.	1.0

Raises:

Type	Description
ValueError	If TOPP-RA finds no feasible parameterization.

Source code in nav_planning/trajectory/toppra.py

def parameterize(
    self,
    path: np.ndarray,
    velocity_scaling: float = 1.0,
    acceleration_scaling: float = 1.0,
) -> Trajectory:
    """Time-parameterize an ``(N, ndof)`` joint-space path.

    Args:
        path: ``(N, ndof)`` waypoints, at least two distinct.
        velocity_scaling: Factor in ``(0, 1]`` on the velocity limit.
        acceleration_scaling: Factor in ``(0, 1]`` on the
            acceleration limit.

    Raises:
        ValueError: If TOPP-RA finds no feasible parameterization.
    """
    from nav_planning._time_parameterization import compute_trajectory

    handle = compute_trajectory(
        np.asarray(path, dtype=np.float64),
        self.max_velocity * velocity_scaling,
        self.max_acceleration * acceleration_scaling,
        self.knot_spacing,
    )
    if handle is None:
        raise ValueError("TOPP-RA found no feasible parameterization")
    return Trajectory(handle)

Convenience function

parameterize_path(path: np.ndarray, max_velocity: np.ndarray, max_acceleration: np.ndarray, knot_spacing: float = 0.1) -> Trajectory

One-shot :meth:TimeOptimalParameterizer.parameterize.

Source code in nav_planning/trajectory/toppra.py

def parameterize_path(
    path: np.ndarray,
    max_velocity: np.ndarray,
    max_acceleration: np.ndarray,
    knot_spacing: float = 0.1,
) -> Trajectory:
    """One-shot :meth:`TimeOptimalParameterizer.parameterize`."""
    return TimeOptimalParameterizer(
        max_velocity, max_acceleration, knot_spacing
    ).parameterize(path)

Trajectory

Trajectory(_handle: '_ToppraTrajectory') dataclass

A time-optimal trajectory produced by :class:~nav_planning.trajectory.TimeOptimalParameterizer.

Instances are immutable handles around a C++ TOPP-RA trajectory; query them via :meth:position, :meth:velocity, :meth:acceleration, or one of the batch samplers.

duration: float property

Trajectory duration in seconds.

position(t: float) -> np.ndarray

Configuration at time t (seconds), clamped to [0, duration].

Source code in nav_planning/trajectory/trajectory.py

def position(self, t: float) -> np.ndarray:
    """Configuration at time ``t`` (seconds), clamped to ``[0, duration]``."""
    return np.asarray(self._handle.position(float(t)), dtype=np.float64)

velocity(t: float) -> np.ndarray

Joint velocity at time t (seconds).

Source code in nav_planning/trajectory/trajectory.py

def velocity(self, t: float) -> np.ndarray:
    """Joint velocity at time ``t`` (seconds)."""
    return np.asarray(self._handle.velocity(float(t)), dtype=np.float64)

acceleration(t: float) -> np.ndarray

Joint acceleration at time t (seconds).

Source code in nav_planning/trajectory/trajectory.py

def acceleration(self, t: float) -> np.ndarray:
    """Joint acceleration at time ``t`` (seconds)."""
    return np.asarray(self._handle.acceleration(float(t)), dtype=np.float64)

sample(times: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]

Sample at the user-supplied times grid.

Parameters:

Name	Type	Description	Default
times	ndarray	(T,) array of sample times in seconds.	required

Returns:

Type	Description
tuple[ndarray, ndarray, ndarray]	(positions, velocities, accelerations) — each (T, ndof).

Source code in nav_planning/trajectory/trajectory.py

def sample(self, times: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Sample at the user-supplied ``times`` grid.

    Args:
        times: ``(T,)`` array of sample times in seconds.

    Returns:
        ``(positions, velocities, accelerations)`` — each ``(T, ndof)``.
    """
    times = np.ascontiguousarray(times, dtype=np.float64).reshape(-1)
    positions, velocities, accelerations = self._handle.sample(times)
    return (
        np.asarray(positions, dtype=np.float64),
        np.asarray(velocities, dtype=np.float64),
        np.asarray(accelerations, dtype=np.float64),
    )

sample_uniform(dt: float) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]

Uniformly-spaced rollout at step dt.

The returned times always start at 0 and end at :attr:duration, which may make the final step shorter than dt — this matches what a streaming controller expects.

Parameters:

Name	Type	Description	Default
dt	float	Sample interval in seconds (must be > 0).	required

Returns:

Type	Description
ndarray	(times, positions, velocities, accelerations) —
ndarray	times has shape (T,); state arrays have shape
ndarray	(T, ndof).

Source code in nav_planning/trajectory/trajectory.py

def sample_uniform(
    self, dt: float
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """Uniformly-spaced rollout at step ``dt``.

    The returned ``times`` always start at ``0`` and end at
    :attr:`duration`, which may make the final step shorter than
    ``dt`` — this matches what a streaming controller expects.

    Args:
        dt: Sample interval in seconds (must be ``> 0``).

    Returns:
        ``(times, positions, velocities, accelerations)`` —
        ``times`` has shape ``(T,)``; state arrays have shape
        ``(T, ndof)``.
    """
    times, positions, velocities, accelerations = self._handle.sample_uniform(
        float(dt)
    )
    return (
        np.asarray(times, dtype=np.float64),
        np.asarray(positions, dtype=np.float64),
        np.asarray(velocities, dtype=np.float64),
        np.asarray(accelerations, dtype=np.float64),
    )

KinodynamicTrajectory

KinodynamicTrajectory(_handle: '_FlatTrajectory') dataclass

A dynamically feasible trajectory produced by FLASK planning.

Instances are immutable handles around a C++ piecewise-cubic trajectory; query them via :meth:position, :meth:velocity, :meth:acceleration, or one of the batch samplers.

duration: float property

Trajectory duration in seconds.

num_dof: int property

Active DOF of the planner that produced the trajectory.

has_base: bool property

True if the leading three DOF are the mobile base.

num_segments: int property

Number of closed-form segments (after simplification).

knot_times: np.ndarray property

Segment boundary times [0, t_1, ..., duration].

segment_kinds: list[str] property

Per-segment kind: "drive" or "spin" (the LoCoBot rotating in place).

segment_gears: list[int] property

Per-segment LoCoBot gear: 0 forward, 1 reverse.

position(t: float) -> np.ndarray

Configuration at time t (seconds), clamped to [0, duration].

Source code in nav_planning/trajectory/kinodynamic.py

def position(self, t: float) -> np.ndarray:
    """Configuration at time ``t`` (seconds), clamped to ``[0, duration]``."""
    return np.asarray(self._handle.position(float(t)), dtype=np.float64)

velocity(t: float) -> np.ndarray

Active-DOF velocity at time t (world-frame for the base).

Source code in nav_planning/trajectory/kinodynamic.py

def velocity(self, t: float) -> np.ndarray:
    """Active-DOF velocity at time ``t`` (world-frame for the base)."""
    return np.asarray(self._handle.velocity(float(t)), dtype=np.float64)

acceleration(t: float) -> np.ndarray

Active-DOF acceleration at time t (world-frame for the base).

Source code in nav_planning/trajectory/kinodynamic.py

def acceleration(self, t: float) -> np.ndarray:
    """Active-DOF acceleration at time ``t`` (world-frame for the base)."""
    return np.asarray(self._handle.acceleration(float(t)), dtype=np.float64)

base_twist(t: float) -> np.ndarray

Body-frame base command (v_x, v_y, omega) at time t.

v_x is the forward speed (negative when the LoCoBot reverses), v_y the lateral speed (zero for the LoCoBot) and omega the yaw rate. Zero for camera-only trajectories.

Source code in nav_planning/trajectory/kinodynamic.py

def base_twist(self, t: float) -> np.ndarray:
    """Body-frame base command ``(v_x, v_y, omega)`` at time ``t``.

    ``v_x`` is the forward speed (negative when the LoCoBot reverses),
    ``v_y`` the lateral speed (zero for the LoCoBot) and ``omega`` the
    yaw rate.  Zero for camera-only trajectories.
    """
    return np.asarray(self._handle.base_twist(float(t)), dtype=np.float64)

sample(times: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]

Sample at the user-supplied times grid.

Parameters:

Name	Type	Description	Default
times	ndarray	(T,) array of sample times in seconds.	required

Returns:

Type	Description
tuple[ndarray, ndarray, ndarray]	(positions, velocities, accelerations) — each (T, ndof).

Source code in nav_planning/trajectory/kinodynamic.py

def sample(self, times: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Sample at the user-supplied ``times`` grid.

    Args:
        times: ``(T,)`` array of sample times in seconds.

    Returns:
        ``(positions, velocities, accelerations)`` — each ``(T, ndof)``.
    """
    times = np.ascontiguousarray(times, dtype=np.float64).reshape(-1)
    positions, velocities, accelerations = self._handle.sample(times.tolist())
    return (
        np.asarray(positions, dtype=np.float64),
        np.asarray(velocities, dtype=np.float64),
        np.asarray(accelerations, dtype=np.float64),
    )

sample_uniform(dt: float) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]

Uniformly-spaced rollout at step dt.

The returned times always start at 0 and end at :attr:duration, which may make the final step shorter than dt — this matches what a streaming controller expects.

Parameters:

Name	Type	Description	Default
dt	float	Sample interval in seconds (must be > 0).	required

Returns:

Type	Description
ndarray	(times, positions, velocities, accelerations) —
ndarray	times has shape (T,); state arrays have shape
ndarray	(T, ndof).

Source code in nav_planning/trajectory/kinodynamic.py

def sample_uniform(
    self, dt: float
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """Uniformly-spaced rollout at step ``dt``.

    The returned ``times`` always start at ``0`` and end at
    :attr:`duration`, which may make the final step shorter than
    ``dt`` — this matches what a streaming controller expects.

    Args:
        dt: Sample interval in seconds (must be ``> 0``).

    Returns:
        ``(times, positions, velocities, accelerations)`` —
        ``times`` has shape ``(T,)``; state arrays have shape
        ``(T, ndof)``.
    """
    times, positions, velocities, accelerations = self._handle.sample_uniform(
        float(dt)
    )
    return (
        np.asarray(times, dtype=np.float64),
        np.asarray(positions, dtype=np.float64),
        np.asarray(velocities, dtype=np.float64),
        np.asarray(accelerations, dtype=np.float64),
    )

sample_base_twist(times: np.ndarray) -> np.ndarray

Body-frame base commands (v_x, v_y, omega) at each time, (T, 3).

Source code in nav_planning/trajectory/kinodynamic.py

def sample_base_twist(self, times: np.ndarray) -> np.ndarray:
    """Body-frame base commands ``(v_x, v_y, omega)`` at each time, ``(T, 3)``."""
    times = np.asarray(times, dtype=np.float64).reshape(-1)
    return np.array([self._handle.base_twist(float(t)) for t in times])

Low-level C++ binding

ToppraTrajectory

Opaque handle to a parameterised trajectory.

A cubic spline along the waypoint path, timed by TOPP-RA: the path velocity is piecewise linear in time between grid points (constant path acceleration), so joint velocity is continuous.

duration: float property

Total duration in seconds.

position(t: float) -> NDArray[np.float64]

Configuration at time t (seconds).

Source code in nav_planning/_time_parameterization.pyi

def position(self, t: float) -> NDArray[np.float64]:
    """Configuration at time ``t`` (seconds)."""
    ...

velocity(t: float) -> NDArray[np.float64]

Joint velocity at time t (seconds).

Source code in nav_planning/_time_parameterization.pyi

def velocity(self, t: float) -> NDArray[np.float64]:
    """Joint velocity at time ``t`` (seconds)."""
    ...

acceleration(t: float) -> NDArray[np.float64]

Joint acceleration at time t (seconds).

Source code in nav_planning/_time_parameterization.pyi

def acceleration(self, t: float) -> NDArray[np.float64]:
    """Joint acceleration at time ``t`` (seconds)."""
    ...

sample(times: NDArray[np.float64]) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]

Batched sampling at times — returns (T, ndof) matrices of (position, velocity, acceleration).

Source code in nav_planning/_time_parameterization.pyi

def sample(
    self,
    times: NDArray[np.float64],
) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
    """Batched sampling at ``times`` — returns ``(T, ndof)`` matrices
    of (position, velocity, acceleration).
    """
    ...

sample_uniform(dt: float) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]

Uniform rollout with step dt.

Returns (times, positions, velocities, accelerations). times starts at 0 and ends at :attr:duration.

Source code in nav_planning/_time_parameterization.pyi

def sample_uniform(
    self,
    dt: float,
) -> tuple[
    NDArray[np.float64],
    NDArray[np.float64],
    NDArray[np.float64],
    NDArray[np.float64],
]:
    """Uniform rollout with step ``dt``.

    Returns ``(times, positions, velocities, accelerations)``.
    ``times`` starts at ``0`` and ends at :attr:`duration`.
    """
    ...

compute_trajectory(waypoints: NDArray[np.float64], max_velocity: NDArray[np.float64], max_acceleration: NDArray[np.float64], knot_spacing: float = 0.1) -> ToppraTrajectory | None

Time-optimal trajectory along the piecewise-linear path waypoints (N, ndof), starting and ending at rest.

The path is resampled every knot_spacing and splined; the result deviates from it by about knot_spacing / 10. Returns None if TOPP-RA finds no feasible parameterization.

Source code in nav_planning/_time_parameterization.pyi

def compute_trajectory(
    waypoints: NDArray[np.float64],
    max_velocity: NDArray[np.float64],
    max_acceleration: NDArray[np.float64],
    knot_spacing: float = 0.1,
) -> ToppraTrajectory | None:
    """Time-optimal trajectory along the piecewise-linear path
    ``waypoints`` ``(N, ndof)``, starting and ending at rest.

    The path is resampled every ``knot_spacing`` and splined; the result
    deviates from it by about ``knot_spacing / 10``.  Returns ``None`` if
    TOPP-RA finds no feasible parameterization.
    """
    ...