Skip to content

Trajectory

TimeOptimalParameterizer

TimeOptimalParameterizer(max_velocity: np.ndarray, max_acceleration: np.ndarray, max_deviation: float = 0.1, time_step: float = 0.001)

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
max_deviation float

Radial tolerance (same unit as the path) for the circular blend inserted at every interior waypoint. Larger values make cornering faster but deviate further from the original piecewise-linear path. Defaults to 0.1.

 0.1
time_step float

Forward-integration step along the path. Smaller values are more accurate and slower; the MoveIt default of 1e-3 works for most manipulators.

 0.001
Source code in autolife_planning/trajectory/totg.py 
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
def __init__(
    self,
    max_velocity: np.ndarray,
    max_acceleration: np.ndarray,
    max_deviation: float = 0.1,
    time_step: float = 1e-3,
) -> 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")
    if max_deviation <= 0:
        raise ValueError("max_deviation must be strictly positive")
    if time_step <= 0:
        raise ValueError("time_step must be strictly positive")

    self._max_velocity = max_velocity
    self._max_acceleration = max_acceleration
    self._max_deviation = float(max_deviation)
    self._time_step = float(time_step)

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.

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 the TOTG integrator fails (e.g. the path contains a 180-degree reversal).
Source code in autolife_planning/trajectory/totg.py 
 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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
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.

    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 the TOTG integrator fails (e.g.
            the path contains a 180-degree reversal).
    """
    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 — TOTG builds a circular
    # blend per interior corner and zero-length segments short-circuit
    # to degenerate blends that bloat the switching-point list without
    # contributing to the timing.
    path = _deduplicate_waypoints(path)
    if path.shape[0] < 2:
        raise ValueError(
            "path collapsed to fewer than 2 distinct waypoints after "
            "de-duplication"
        )

    from autolife_planning._time_parameterization import compute_trajectory

    handle = compute_trajectory(
        path,
        self._max_velocity * velocity_scaling,
        self._max_acceleration * acceleration_scaling,
        self._max_deviation,
        self._time_step,
    )
    if handle is None:
        raise ValueError(
            "TOTG failed to parameterize path — see stderr for the "
            "underlying reason (common cause: a 180-degree reversal "
            "between three consecutive waypoints)."
        )
    return Trajectory(handle)

Convenience function

parameterize_path(path: np.ndarray, max_velocity: np.ndarray, max_acceleration: np.ndarray, max_deviation: float = 0.1, time_step: float = 0.001) -> 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 autolife_planning/trajectory/totg.py 
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
def parameterize_path(
    path: np.ndarray,
    max_velocity: np.ndarray,
    max_acceleration: np.ndarray,
    max_deviation: float = 0.1,
    time_step: float = 1e-3,
) -> 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, max_deviation, time_step
    ).parameterize(path)

Trajectory

Trajectory(_handle: '_TotgTrajectory') dataclass

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

Instances are immutable handles around a C++ TOTG state machine; 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

TotgTrajectory

Opaque handle to a parameterised trajectory.

Queries are C¹-consistent: :meth:position / :meth:velocity / :meth:acceleration sample a quadratic segment between the algorithm's internal forward-integration grid points.

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 
28
29
30
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 
31
32
33
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 
34
35
36
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 
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
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], max_deviation: float = ..., time_step: float = 0.001) -> TotgTrajectory | None

Build a time-optimal trajectory for a piecewise-linear path.

Returns None if waypoints has fewer than two rows, the velocity/acceleration bounds are infeasible, or the integrator failed (e.g. a 180-degree reversal in the waypoints).

Source code in autolife_planning/_time_parameterization.pyi 
61
62
63
64
65
66
67
68
69
70
71
72
73
74
def compute_trajectory(
    waypoints: NDArray[np.float64],
    max_velocity: NDArray[np.float64],
    max_acceleration: NDArray[np.float64],
    max_deviation: float = ...,
    time_step: float = 1e-3,
) -> TotgTrajectory | None:
    """Build a time-optimal trajectory for a piecewise-linear path.

    Returns ``None`` if ``waypoints`` has fewer than two rows, the
    velocity/acceleration bounds are infeasible, or the integrator
    failed (e.g. a 180-degree reversal in the waypoints).
    """
    ...