Trajectories

shinro.trajectories

Reference path generators for smooth point-to-point motion.

Provides polynomial trajectory generators that compute position, velocity, and acceleration profiles from boundary conditions. All generators support arbitrary N-dimensional positions via numpy broadcasting.

Available generators: CubicPolynomial — 3rd-order, position + velocity continuity QuinticPolynomial — 5th-order, position + velocity + acceleration continuity

---

CubicPolynomial

Bases: TrajectoryGenerator

3rd-order polynomial trajectory generator.

Generates smooth point-to-point trajectories using a cubic polynomial:

\[ p(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 \]

The coefficients are computed in closed form (no matrix solve) from boundary conditions on position and velocity at both ends. Acceleration is continuous but NOT constrained at boundaries (use QuinticPolynomial if acceleration constraints are needed).

Supports arbitrary N-dimensional positions via element-wise operations.

Parameters:

Name	Type	Description	Default
backend	ArrayBackend | None	Array backend. Defaults to NumpyBackend.	None

Source code in src/shinro/trajectories/cubic_polynomial.py

def __init__(self, backend: ArrayBackend | None = None):
    self.bk = backend or NumpyBackend()

generate

generate(start_position, end_position, duration: float, start_vel, end_vel)

Compute cubic polynomial coefficients from boundary conditions.

Closed-form solution:

\[ a_0 &= p_0 \\ a_1 &= v_0 \\ a_2 &= \frac{3\Delta p - T(2v_0 + v_f)}{T^2} \\ a_3 &= \frac{-2\Delta p + T(v_0 + v_f)}{T^3} \]

where \(\Delta p = p_f - p_0\) and \(T =\) duration.

Parameters:

Name	Type	Description	Default
start_position		Initial position vector (N,).	required
end_position		Final position vector (N,).	required
duration	float	Total trajectory time in seconds.	required
start_vel		Initial velocity vector (N,).	required
end_vel		Final velocity vector (N,).	required

Source code in src/shinro/trajectories/cubic_polynomial.py

def generate(
    self,
    start_position,
    end_position,
    duration: float,
    start_vel,
    end_vel,
):
    """Compute cubic polynomial coefficients from boundary conditions.

    Closed-form solution:

    .. math::

        a_0 &= p_0 \\\\
        a_1 &= v_0 \\\\
        a_2 &= \\frac{3\\Delta p - T(2v_0 + v_f)}{T^2} \\\\
        a_3 &= \\frac{-2\\Delta p + T(v_0 + v_f)}{T^3}

    where :math:`\\Delta p = p_f - p_0` and :math:`T =` duration.

    Args:
        start_position: Initial position vector (N,).
        end_position: Final position vector (N,).
        duration: Total trajectory time in seconds.
        start_vel: Initial velocity vector (N,).
        end_vel: Final velocity vector (N,).
    """
    self.a0 = start_position
    self.a1 = start_vel
    a2_numerator = 3 * (end_position - start_position) - duration * (2 * start_vel + end_vel)
    self.a2 = a2_numerator / (duration ** 2)

    a3_numerator = -2 * (end_position - start_position) + duration * (start_vel + end_vel)
    self.a3 = a3_numerator / (duration ** 3)

    self.duration = duration

position_at

position_at(t: float)

Evaluate position, velocity, and acceleration at time t.

Parameters:

Name	Type	Description	Default
t	float	Time in seconds (clipped to [0, duration]).	required

Returns:

Type	Description
	Tuple of (position, velocity, acceleration) arrays, each of
	shape matching the input dimensions (N,).

Source code in src/shinro/trajectories/cubic_polynomial.py

def position_at(self, t: float):
    """Evaluate position, velocity, and acceleration at time t.

    Args:
        t: Time in seconds (clipped to [0, duration]).

    Returns:
        Tuple of (position, velocity, acceleration) arrays, each of
        shape matching the input dimensions (N,).
    """
    t = self.bk.clip(t, 0, self.duration)
    pos = self.a0 + self.a1 * t + self.a2 * t ** 2 + self.a3 * t ** 3
    vel = self.a1 + 2 * self.a2 * t + 3 * self.a3 * t ** 2
    acc = 2 * self.a2 + 6 * self.a3 * t
    return pos, vel, acc

from_config classmethod

from_config(config, backend: ArrayBackend | None = None)

Create a waypoint schedule from a TOML config dict or CubicSegmentsConfig.

Config fields

dt: Time step. segments: List of segment dicts, each with: - duration: Segment duration (s). - start: Start position list. - end: End position list. - start_vel: Optional start velocity (default: zeros). - end_vel: Optional end velocity (default: zeros).

Parameters:

Name	Type	Description	Default
config		TOML config dict or CubicSegmentsConfig.	required
backend	ArrayBackend | None	Array backend. Defaults to NumpyBackend.	None

Returns:

Type	Description
	Array of shape (total_steps, N) with position waypoints.

Source code in src/shinro/trajectories/cubic_polynomial.py

@classmethod
def from_config(cls, config, backend: ArrayBackend | None = None):
    """Create a waypoint schedule from a TOML config dict or :class:`CubicSegmentsConfig`.

    Config fields:
        dt: Time step.
        segments: List of segment dicts, each with:
            - duration: Segment duration (s).
            - start: Start position list.
            - end: End position list.
            - start_vel: Optional start velocity (default: zeros).
            - end_vel: Optional end velocity (default: zeros).

    Args:
        config: TOML config dict or CubicSegmentsConfig.
        backend: Array backend. Defaults to NumpyBackend.

    Returns:
        Array of shape (total_steps, N) with position waypoints.
    """
    bk = backend or NumpyBackend()
    cfg = cls.parse_config(config)
    segs = strict_from_list(CubicSegmentConfig, cfg.segments, "cubic_segments.segment")
    schedule = []
    for seg in segs:
        n_steps = int(np.round(seg.duration / cfg.dt))
        p0 = bk.array(seg.start)
        pf = bk.array(seg.end)
        start_vel = bk.array(seg.start_vel if seg.start_vel is not None else [0.0] * len(seg.start))
        end_vel = bk.array(seg.end_vel if seg.end_vel is not None else [0.0] * len(seg.end))
        traj = cls(backend=bk)
        traj.generate(p0, pf, seg.duration, start_vel, end_vel)
        for k in range(n_steps):
            t = k * cfg.dt
            pos, _, _ = traj.position_at(t)
            schedule.append(pos)
    return bk.array(schedule)

---

QuinticPolynomial

Bases: TrajectoryGenerator

5th-order polynomial trajectory generator.

Generates smooth point-to-point trajectories using a quintic polynomial:

\[ p(t) = a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 t^2 + a_1 t + a_0 \]

Enforces position, velocity, AND acceleration constraints at both start and end (6 boundary conditions → 6 coefficients). When all velocities and accelerations are zero (rest-to-rest), this reduces to the minimum-jerk trajectory:

\[ p(s) = p_0 + (p_f - p_0)(10s^3 - 15s^4 + 6s^5), \quad s = t/T \]

Solves a 6x6 Vandermonde-like linear system via bk.solve(). Supports arbitrary N-dimensional positions — the right-hand side is stacked as (6, N) and solved once.

Parameters:

Name	Type	Description	Default
backend	ArrayBackend | None	Array backend. Defaults to NumpyBackend.	None

Source code in src/shinro/trajectories/quintic_polynomial.py

def __init__(self, backend: ArrayBackend | None = None):
    self.bk = backend or NumpyBackend()

generate

generate(start_position, end_position, duration: float, start_vel=None, end_vel=None, start_acc=None, end_acc=None)

Compute quintic polynomial coefficients by solving the 6x6 system.

Solves \(M c = b\) where:

\[ M = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 1 \\ T^5 & T^4 & T^3 & T^2 & T & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 5T^4 & 4T^3 & 3T^2 & 2T & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 20T^3 & 12T^2 & 6T & 2 & 0 & 0 \end{bmatrix}, \quad b = \begin{bmatrix} p_0 \\ p_f \\ v_0 \\ v_f \\ a_0 \\ a_f \end{bmatrix} \]

Any boundary condition set to None defaults to zero (rest-to-rest).

Parameters:

Name	Type	Description	Default
start_position		Initial position vector (N,).	required
end_position		Final position vector (N,).	required
duration	float	Total trajectory time in seconds.	required
start_vel		Initial velocity vector (N,). Defaults to zeros.	None
end_vel		Final velocity vector (N,). Defaults to zeros.	None
start_acc		Initial acceleration vector (N,). Defaults to zeros.	None
end_acc		Final acceleration vector (N,). Defaults to zeros.	None

Source code in src/shinro/trajectories/quintic_polynomial.py

def generate(
    self,
    start_position,
    end_position,
    duration: float,
    start_vel=None,
    end_vel=None,
    start_acc=None,
    end_acc=None,
):
    """Compute quintic polynomial coefficients by solving the 6x6 system.

    Solves :math:`M c = b` where:

    .. math::

        M = \\begin{bmatrix}
        0 & 0 & 0 & 0 & 0 & 1 \\\\
        T^5 & T^4 & T^3 & T^2 & T & 1 \\\\
        0 & 0 & 0 & 0 & 1 & 0 \\\\
        5T^4 & 4T^3 & 3T^2 & 2T & 1 & 0 \\\\
        0 & 0 & 0 & 2 & 0 & 0 \\\\
        20T^3 & 12T^2 & 6T & 2 & 0 & 0
        \\end{bmatrix}, \\quad
        b = \\begin{bmatrix} p_0 \\\\ p_f \\\\ v_0 \\\\ v_f \\\\ a_0 \\\\ a_f \\end{bmatrix}

    Any boundary condition set to None defaults to zero (rest-to-rest).

    Args:
        start_position: Initial position vector (N,).
        end_position: Final position vector (N,).
        duration: Total trajectory time in seconds.
        start_vel: Initial velocity vector (N,). Defaults to zeros.
        end_vel: Final velocity vector (N,). Defaults to zeros.
        start_acc: Initial acceleration vector (N,). Defaults to zeros.
        end_acc: Final acceleration vector (N,). Defaults to zeros.
    """
    start_vel = self.bk.zeros_like(start_position) if start_vel is None else start_vel
    start_acc = self.bk.zeros_like(start_position) if start_acc is None else start_acc
    end_vel = self.bk.zeros_like(end_position) if end_vel is None else end_vel
    end_acc = self.bk.zeros_like(end_position) if end_acc is None else end_acc

    self.T = duration
    T = duration
    M = self.bk.array([
        [0, 0, 0, 0, 0, 1],
        [T ** 5, T ** 4, T ** 3, T ** 2, T, 1],
        [0, 0, 0, 0, 1, 0],
        [5 * T ** 4, 4 * T ** 3, 3 * T ** 2, 2 * T, 1, 0],
        [0, 0, 0, 2, 0, 0],
        [20 * T ** 3, 12 * T ** 2, 6 * T, 2, 0, 0],
    ])

    b = [start_position, end_position, start_vel, end_vel, start_acc, end_acc]

    coeff_vectors = self.bk.solve(M, b)

    self.A = coeff_vectors[0]
    self.B = coeff_vectors[1]
    self.C = coeff_vectors[2]
    self.D = coeff_vectors[3]
    self.E = coeff_vectors[4]
    self.F = coeff_vectors[5]

position_at

position_at(t: float)

Evaluate position, velocity, and acceleration at time t.

Parameters:

Name	Type	Description	Default
t	float	Time in seconds (clipped to [0, T]).	required

Returns:

Type	Description
	Tuple of (position, velocity, acceleration) arrays, each of
	shape matching the input dimensions (N,).

Source code in src/shinro/trajectories/quintic_polynomial.py

def position_at(self, t: float):
    """Evaluate position, velocity, and acceleration at time t.

    Args:
        t: Time in seconds (clipped to [0, T]).

    Returns:
        Tuple of (position, velocity, acceleration) arrays, each of
        shape matching the input dimensions (N,).
    """
    t = self.bk.clip(t, 0, self.T)
    pos = self.A * t ** 5 + self.B * t ** 4 + self.C * t ** 3 + self.D * t ** 2 + self.E * t + self.F
    vel = 5 * self.A * t ** 4 + 4 * self.B * t ** 3 + 3 * self.C * t ** 2 + 2 * self.D * t + self.E
    acc = 20 * self.A * t ** 3 + 12 * self.B * t ** 2 + 6 * self.C * t + 2 * self.D
    return pos, vel, acc