Skip to content

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 autolife_planning/trajectory/toppra.py 
36
37
38
39
40
41
42
43
44
45
46
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 autolife_planning/trajectory/toppra.py 
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
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 autolife_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 autolife_planning/trajectory/toppra.py 
82
83
84
85
86
87
88
89
90
91
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:~autolife_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 autolife_planning/trajectory/trajectory.py 
42
43
44
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 autolife_planning/trajectory/trajectory.py 
46
47
48
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 autolife_planning/trajectory/trajectory.py 
50
51
52
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 autolife_planning/trajectory/trajectory.py 
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
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 autolife_planning/trajectory/trajectory.py 
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
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),
    )

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 autolife_planning/_time_parameterization.pyi 
25
26
27
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 autolife_planning/_time_parameterization.pyi 
29
30
31
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 autolife_planning/_time_parameterization.pyi 
33
34
35
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 autolife_planning/_time_parameterization.pyi 
37
38
39
40
41
42
43
44
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 autolife_planning/_time_parameterization.pyi 
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
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; raises ValueError if the limits do not match the path's DOF or the path has no length.

Source code in autolife_planning/_time_parameterization.pyi 
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
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; raises ``ValueError`` if the
    limits do not match the path's DOF or the path has no length.
    """
    ...