Skip to content

Trajectory

TimeOptimalParameterizer

TimeOptimalParameterizer(max_velocity: np.ndarray, max_acceleration: np.ndarray)

Time-optimal trajectory parameteriser with fixed joint limits.

Instantiate once per robot (limits are cached), then call :meth:parameterize on every path. The parameteriser itself holds no state between calls — it is safe to reuse across threads so long as the limits do not change.

Parameters:

Name Type Description Default
max_velocity ndarray

(ndof,) per-joint velocity bound, strictly positive. Units must match the path (rad/s for revolute joints, m/s for prismatic).

 required
max_acceleration ndarray

(ndof,) per-joint acceleration bound, strictly positive.

 required
Source code in wheelchair_planning/trajectory/parameterization.py 
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
def __init__(
    self,
    max_velocity: np.ndarray,
    max_acceleration: np.ndarray,
) -> None:
    max_velocity = np.ascontiguousarray(max_velocity, dtype=np.float64).reshape(-1)
    max_acceleration = np.ascontiguousarray(
        max_acceleration, dtype=np.float64
    ).reshape(-1)
    if max_velocity.shape != max_acceleration.shape:
        raise ValueError(
            f"max_velocity {max_velocity.shape} and max_acceleration "
            f"{max_acceleration.shape} must have the same shape"
        )
    if not np.all(max_velocity > 0):
        raise ValueError("max_velocity entries must be strictly positive")
    if not np.all(max_acceleration > 0):
        raise ValueError("max_acceleration entries must be strictly positive")

    self._max_velocity = max_velocity
    self._max_acceleration = max_acceleration

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

Convert a piecewise-linear joint-space path into a time-optimal trajectory.

TOPP-RA runs on a natural cubic spline through the supplied waypoints, so the result passes through every waypoint with continuous velocity and bounded acceleration.

Parameters:

Name Type Description Default
path ndarray

(N, ndof) waypoint array. Must have at least two waypoints and match the parameteriser's DOF.

 required
velocity_scaling float

Scalar in (0, 1] applied to the velocity limit. 1.0 uses the bound passed at construction time.

 1.0
acceleration_scaling float

Scalar in (0, 1] applied to the acceleration limit. Values below 1 trade speed for smoother motion.

 1.0

Returns:

Name Type Description
A Trajectory

class:Trajectory.

Raises:

Type Description
ValueError

If path is malformed, scaling factors are out of range, or TOPP-RA cannot compute a feasible time parameterization.
Source code in wheelchair_planning/trajectory/parameterization.py 
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
def parameterize(
    self,
    path: np.ndarray,
    velocity_scaling: float = 1.0,
    acceleration_scaling: float = 1.0,
) -> Trajectory:
    """Convert a piecewise-linear joint-space path into a time-optimal
    trajectory.

    TOPP-RA runs on a natural cubic spline through the supplied
    waypoints, so the result passes through every waypoint with
    continuous velocity and bounded acceleration.

    Args:
        path: ``(N, ndof)`` waypoint array.  Must have at least two
            waypoints and match the parameteriser's DOF.
        velocity_scaling: Scalar in ``(0, 1]`` applied to the
            velocity limit.  ``1.0`` uses the bound passed at
            construction time.
        acceleration_scaling: Scalar in ``(0, 1]`` applied to the
            acceleration limit.  Values below ``1`` trade speed for
            smoother motion.

    Returns:
        A :class:`Trajectory`.

    Raises:
        ValueError: If ``path`` is malformed, ``scaling`` factors
            are out of range, or TOPP-RA cannot compute a feasible
            time parameterization.
    """
    path = np.ascontiguousarray(path, dtype=np.float64)
    if path.ndim != 2:
        raise ValueError(f"path must be 2D (N, ndof), got shape {path.shape}")
    if path.shape[0] < 2:
        raise ValueError(
            f"path must have at least 2 waypoints, got {path.shape[0]}"
        )
    if path.shape[1] != self.num_dof:
        raise ValueError(
            f"path has {path.shape[1]} DOF, parameteriser was built for "
            f"{self.num_dof} DOF"
        )
    if not (0.0 < velocity_scaling <= 1.0):
        raise ValueError(
            f"velocity_scaling must be in (0, 1], got {velocity_scaling}"
        )
    if not (0.0 < acceleration_scaling <= 1.0):
        raise ValueError(
            f"acceleration_scaling must be in (0, 1], got {acceleration_scaling}"
        )

    # Collapse adjacent duplicate waypoints.  TOPP-RA parameterizes a
    # scalar progress variable along the path; zero-length segments only
    # introduce singular path derivatives without contributing motion.
    path = _deduplicate_waypoints(path)
    if path.shape[0] < 2:
        raise ValueError(
            "path collapsed to fewer than 2 distinct waypoints after "
            "de-duplication"
        )

    handle = compute_toppra_trajectory(
        path,
        self._max_velocity * velocity_scaling,
        self._max_acceleration * acceleration_scaling,
    )
    if handle is None:
        raise ValueError(
            "TOPP-RA failed to parameterize path. Check that the path "
            "is smooth enough for spline interpolation and that the "
            "velocity/acceleration limits are feasible."
        )
    return Trajectory(handle)

Convenience function

parameterize_path(path: np.ndarray, max_velocity: np.ndarray, max_acceleration: np.ndarray) -> Trajectory

One-shot helper that builds a :class:TimeOptimalParameterizer and immediately calls :meth:parameterize on path.

Prefer the class form when you are going to re-use the limits — it avoids revalidating them on every call.

Source code in wheelchair_planning/trajectory/parameterization.py 
141
142
143
144
145
146
147
148
149
150
151
152
def parameterize_path(
    path: np.ndarray,
    max_velocity: np.ndarray,
    max_acceleration: np.ndarray,
) -> Trajectory:
    """One-shot helper that builds a :class:`TimeOptimalParameterizer`
    and immediately calls :meth:`parameterize` on ``path``.

    Prefer the class form when you are going to re-use the limits — it
    avoids revalidating them on every call.
    """
    return TimeOptimalParameterizer(max_velocity, max_acceleration).parameterize(path)

Trajectory

Trajectory(_handle: Any) dataclass

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

Instances are immutable handles around a concrete timing backend; 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 wheelchair_planning/trajectory/trajectory.py 
37
38
39
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 wheelchair_planning/trajectory/trajectory.py 
41
42
43
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 wheelchair_planning/trajectory/trajectory.py 
45
46
47
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 wheelchair_planning/trajectory/trajectory.py 
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
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 wheelchair_planning/trajectory/trajectory.py 
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
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" (rotate in place).

segment_gears: list[int] property

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

position(t: float) -> np.ndarray

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

Source code in wheelchair_planning/trajectory/kinodynamic.py 
86
87
88
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 wheelchair_planning/trajectory/kinodynamic.py 
90
91
92
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 wheelchair_planning/trajectory/kinodynamic.py 
94
95
96
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, omega) at time t.

v is the signed forward speed (negative when reversing) and omega the yaw rate. Zero for arm-only trajectories.

Source code in wheelchair_planning/trajectory/kinodynamic.py 
 98
 99
100
101
102
103
104
def base_twist(self, t: float) -> np.ndarray:
    """Body-frame base command ``(v, omega)`` at time ``t``.

    ``v`` is the signed forward speed (negative when reversing) and
    ``omega`` the yaw rate.  Zero for arm-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 wheelchair_planning/trajectory/kinodynamic.py 
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
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 wheelchair_planning/trajectory/kinodynamic.py 
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
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, omega) at each time, (T, 2).

Source code in wheelchair_planning/trajectory/kinodynamic.py 
152
153
154
155
def sample_base_twist(self, times: np.ndarray) -> np.ndarray:
    """Body-frame base commands ``(v, omega)`` at each time, ``(T, 2)``."""
    times = np.asarray(times, dtype=np.float64).reshape(-1)
    return np.array([self._handle.base_twist(float(t)) for t in times])