"""Point-to-point joint trajectories: cubic, quintic and trapezoidal velocity. Every function takes a time array t (s) and returns (position, velocity, acceleration) arrays of the same shape. Times before 0 hold the start, times after the end hold the goal. Contents -------- cubic_coeffs, cubic third-order polynomial, position + velocity at both ends quintic_coeffs, quintic fifth-order polynomial, adds acceleration at both ends trapezoid_duration, trapezoid constant-acceleration / cruise / deceleration profile sample sample a trajectory at a fixed controller rate Run this file to print peak velocities and accelerations for one move: python trajectory.py """ from __future__ import annotations from typing import Callable import numpy as np from numpy.typing import NDArray FloatArray = NDArray[np.float64] Profile = tuple[FloatArray, FloatArray, FloatArray] def _poly_eval(coeffs: FloatArray, tau: FloatArray) -> Profile: """Evaluate p(tau) = sum c_i tau^i and its first two derivatives.""" p = np.polynomial.polynomial c = np.asarray(coeffs, dtype=float) return p.polyval(tau, c), p.polyval(tau, p.polyder(c)), p.polyval(tau, p.polyder(c, 2)) def cubic_coeffs(q0: float, qf: float, T: float, v0: float = 0.0, vf: float = 0.0) -> FloatArray: """Coefficients a0..a3 of q(t) = a0 + a1 t + a2 t^2 + a3 t^3.""" dq = qf - q0 a2 = (3.0 * dq - (2.0 * v0 + vf) * T) / T**2 a3 = (-2.0 * dq + (v0 + vf) * T) / T**3 return np.array([q0, v0, a2, a3]) def cubic(q0: float, qf: float, T: float, t: FloatArray, v0: float = 0.0, vf: float = 0.0) -> Profile: t = np.asarray(t, dtype=float) tc = np.clip(t, 0.0, T) pos, vel, acc = _poly_eval(cubic_coeffs(q0, qf, T, v0, vf), tc) outside = (t < 0.0) | (t > T) vel = np.where(outside, np.where(t < 0.0, v0, vf), vel) acc = np.where(outside, 0.0, acc) return pos, vel, acc def quintic_coeffs(q0: float, qf: float, T: float, v0: float = 0.0, vf: float = 0.0, a0: float = 0.0, af: float = 0.0) -> FloatArray: """Coefficients c0..c5 that meet position, velocity and acceleration at t = 0 and t = T.""" m = np.array([ [1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 2, 0, 0, 0], [1, T, T**2, T**3, T**4, T**5], [0, 1, 2 * T, 3 * T**2, 4 * T**3, 5 * T**4], [0, 0, 2, 6 * T, 12 * T**2, 20 * T**3], ], dtype=float) return np.linalg.solve(m, np.array([q0, v0, a0, qf, vf, af], dtype=float)) def quintic(q0: float, qf: float, T: float, t: FloatArray, v0: float = 0.0, vf: float = 0.0, a0: float = 0.0, af: float = 0.0) -> Profile: t = np.asarray(t, dtype=float) tc = np.clip(t, 0.0, T) pos, vel, acc = _poly_eval(quintic_coeffs(q0, qf, T, v0, vf, a0, af), tc) vel = np.where(t < 0.0, v0, np.where(t > T, vf, vel)) acc = np.where(t < 0.0, a0, np.where(t > T, af, acc)) return pos, vel, acc def trapezoid_duration(distance: float, v_max: float, a_max: float) -> tuple[float, float, float]: """Return (T, t_acc, v_peak) for a rest-to-rest move of |distance|. If the distance is too short to reach v_max, the profile is triangular and v_peak = sqrt(distance * a_max) < v_max. """ d = abs(distance) if v_max <= 0.0 or a_max <= 0.0: raise ValueError("v_max and a_max must be positive") if d * a_max >= v_max**2: # reaches cruise speed t_acc = v_max / a_max t_cruise = (d - v_max * t_acc) / v_max return 2.0 * t_acc + t_cruise, t_acc, v_max v_peak = float(np.sqrt(d * a_max)) # triangular t_acc = v_peak / a_max return 2.0 * t_acc, t_acc, v_peak def trapezoid(q0: float, qf: float, v_max: float, a_max: float, t: FloatArray) -> Profile: """Trapezoidal velocity profile from rest at q0 to rest at qf.""" t = np.asarray(t, dtype=float) dist = qf - q0 sgn = 1.0 if dist >= 0.0 else -1.0 T, ta, vp = trapezoid_duration(dist, v_max, a_max) a = vp / ta if ta > 0.0 else 0.0 tc = np.clip(t, 0.0, T) s = np.where( tc < ta, 0.5 * a * tc**2, np.where(tc <= T - ta, 0.5 * a * ta**2 + vp * (tc - ta), abs(dist) - 0.5 * a * (T - tc) ** 2)) v = np.where(tc < ta, a * tc, np.where(tc <= T - ta, vp, a * (T - tc))) acc = np.where(tc < ta, a, np.where(tc <= T - ta, 0.0, -a)) outside = (t <= 0.0) | (t >= T) v = np.where(outside, 0.0, v) acc = np.where(outside, 0.0, acc) return q0 + sgn * s, sgn * v, sgn * acc def trapezoid_for_duration(q0: float, qf: float, T: float, accel_fraction: float = 1.0 / 3.0 ) -> tuple[float, float]: """(v_max, a_max) for a trapezoid of total time T that accelerates for accel_fraction * T. With 1/3 the profile has equal accel, cruise and decel phases.""" d = abs(qf - q0) ta = accel_fraction * T v = d / (T - ta) return v, v / ta def sample(profile: Callable[[FloatArray], Profile], T: float, rate_hz: float) -> FloatArray: """Positions sampled at rate_hz from t = 0 to t = T inclusive (goal positions for a servo).""" n = int(np.ceil(T * rate_hz)) t = np.minimum(np.arange(n + 1) / rate_hz, T) return profile(t)[0] def main() -> None: q0, qf, T = 0.0, 1.0, 1.0 t = np.linspace(0.0, T, 10001) _, vc, ac = cubic(q0, qf, T, t) _, vq, aq = quintic(q0, qf, T, t) v_max, a_max = trapezoid_for_duration(q0, qf, T) _, vt, at = trapezoid(q0, qf, v_max, a_max, t) print(f"Move {qf - q0:.1f} rad in {T:.1f} s") for name, v, a in (("cubic", vc, ac), ("quintic", vq, aq), ("trapezoid", vt, at)): print(f"{name:10s} peak velocity {np.max(np.abs(v)):.3f} rad/s, " f"peak acceleration {np.max(np.abs(a)):.3f} rad/s^2") if __name__ == "__main__": main()