This document provides comprehensive technical documentation for all trajectory planning and interpolation algorithms implemented in InterpolatePy. The library is designed for robotics, animation, and scientific computing applications requiring smooth trajectory generation with precise control over position, velocity, acceleration, and jerk profiles.
InterpolatePy implements state-of-the-art trajectory planning algorithms based on mathematical optimization and approximation theory. All algorithms provide:
Throughout this document, we use the following conventions:
InterpolatePy organizes algorithms into five main categories:
Advanced piecewise polynomial methods with global smoothness constraints.
Classical trajectory profiles optimized for robotics and automation.
Direct polynomial fitting with boundary conditions.
Specialized methods for smooth rotation interpolation.
Path-following, frame computation, and geometric utilities.
Spline methods construct piecewise polynomials that maintain continuity across segment boundaries. InterpolatePy implements cubic splines (C² continuous) and B-splines with various constraint handling approaches.
File: cubic_spline.py
Class: CubicSpline
Implements natural cubic spline interpolation with configurable boundary conditions. Given waypoints {(tᵢ, qᵢ)}ᵢ₌₀ⁿ, constructs piecewise cubic polynomials:
qₖ(t) = aₖ₀ + aₖ₁(t-tₖ) + aₖ₂(t-tₖ)² + aₖ₃(t-tₖ)³
for t ∈ [tₖ, tₖ₊₁]. The spline satisfies:
Mathematical Derivation:
qₖ(t) = Aₖ + Bₖ(t-tₖ) + (Cₖ/6)(t-tₖ)³ + (Dₖ/6)(t-tₖ)²
where Cₖ = ωₖ, Dₖ = ωₖ₊₁ - ωₖ
Aᵢᵢ = (Tᵢ₋₁ + Tᵢ)/3, Aᵢ,ᵢ₋₁ = Tᵢ₋₁/6, Aᵢ,ᵢ₊₁ = Tᵢ/6
cᵢ = (qᵢ₊₁ - qᵢ)/Tᵢ - (qᵢ - qᵢ₋₁)/Tᵢ₋₁
with Tᵢ = tᵢ₊₁ - tᵢ being the time intervals.
class CubicSpline:
def __init__(self, t_points: list[float], q_points: list[float],
v0: float = 0.0, vn: float = 0.0,
a0: float | None = None, an: float | None = None)
Parameters:
t_points: Time waypoints [t₀, t₁, …, tₙ] (strictly increasing)q_points: Position waypoints [q₀, q₁, …, qₙ] (same length as t_points)v0: Initial velocity constraint (default: 0.0)vn: Final velocity constraint (default: 0.0)a0: Initial acceleration constraint (default: None → natural spline)an: Final acceleration constraint (default: None → natural spline)Key Methods:
def evaluate(self, t: float | np.ndarray) -> float | np.ndarray
Evaluate position q(t) at time(s) t.
def evaluate_velocity(self, t: float | np.ndarray) -> float | np.ndarray
Evaluate velocity q̇(t) at time(s) t.
def evaluate_acceleration(self, t: float | np.ndarray) -> float | np.ndarray
Evaluate acceleration q̈(t) at time(s) t.
def plot(self, num_points: int = 1000) -> None
Generate visualization with position, velocity, and acceleration profiles.
import numpy as np
from interpolatepy import CubicSpline
# Define waypoints
t_points = [0, 1, 2, 3, 4]
q_points = [0, 1, 0, -1, 0]
# Create spline with velocity constraints
spline = CubicSpline(t_points, q_points, v0=0.0, vn=0.0)
# Evaluate at specific time
position = spline.evaluate(1.5)
velocity = spline.evaluate_velocity(1.5)
acceleration = spline.evaluate_acceleration(1.5)
# Generate trajectory
t_eval = np.linspace(0, 4, 100)
trajectory = spline.evaluate(t_eval)
# Visualize
spline.plot()
File: c_s_smoothing.py
Class: CubicSmoothingSpline
Implements cubic smoothing splines that balance interpolation accuracy with smoothness. Instead of exact interpolation, minimizes the objective function:
J(q) = μ∑ᵢ(qᵢ - q̂ᵢ)² + ∫[t₀,tₙ] (q̈(t))² dt
where:
Special cases:
The solution involves solving a modified tridiagonal system where the diagonal is augmented by the smoothing parameter.
class CubicSmoothingSpline:
def __init__(self, t_points: list[float], q_points: list[float],
mu: float, v0: float = 0.0, vn: float = 0.0)
Parameters:
t_points: Time waypointsq_points: Target position waypointsmu: Smoothing parameter (≥ 0)v0, vn: Boundary velocity conditionsKey Methods: Same as CubicSpline
from interpolatepy import CubicSmoothingSpline
# Noisy data points
t_points = [0, 0.5, 1.0, 1.5, 2.0]
q_noisy = [0.1, 1.05, -0.02, -0.98, 0.05] # Noisy sine wave
# Smooth the data
spline = CubicSmoothingSpline(t_points, q_noisy, mu=0.1)
spline.plot()
File: c_s_smoot_search.py
Function: smoothing_spline_with_tolerance
Automatically determines the optimal smoothing parameter μ using binary search to achieve a specified tolerance. The algorithm iteratively adjusts μ until:
max|qᵢ - q̂ᵢ| ≤ tolerance
This provides an objective way to balance smoothness and fidelity without manual parameter tuning.
def smoothing_spline_with_tolerance(
t_points: np.ndarray,
q_points: np.ndarray,
tolerance: float,
config: SplineConfig,
) -> tuple[CubicSmoothingSpline, float, float, int]
Parameters:
t_points: Time points as numpy arrayq_points: Position points as numpy arraytolerance: Maximum allowed deviation from waypointsconfig: SplineConfig with search parametersfrom interpolatepy import smoothing_spline_with_tolerance, SplineConfig
import numpy as np
# Automatically find optimal smoothing
config = SplineConfig(max_iterations=50)
spline, mu, error, iterations = smoothing_spline_with_tolerance(
np.array(t_points), np.array(q_noisy), tolerance=0.01, config=config
)
print(f"Optimal μ = {mu:.6f}, error = {error:.6f}")
File: c_s_with_acc1.py
Class: CubicSplineWithAcceleration1
Extends cubic spline interpolation to handle both velocity AND acceleration boundary conditions simultaneously. This requires adding extra degrees of freedom to the system.
Method 1 Approach:
The positions of virtual points are computed using:
q₁ = q₀ + T₀·v₀ + (T₀²/3)·a₀ + (T₀²/6)·ω₁
qₙ₋₁ = qₙ - Tₙ₋₁·vₙ + (Tₙ₋₁²/3)·aₙ + (Tₙ₋₁²/6)·ωₙ₋₁
This creates a larger tridiagonal system that can satisfy 4 boundary conditions total.
class CubicSplineWithAcceleration1:
def __init__(self, t_points: list[float], q_points: list[float],
v0: float = 0.0, vn: float = 0.0,
a0: float = 0.0, an: float = 0.0)
Parameters:
t_points: Original time waypointsq_points: Original position waypointsv0, vn: Initial and final velocitya0, an: Initial and final accelerationKey Methods: Same as CubicSpline, plus:
@property
def original_indices(self) -> list[int]
Returns indices of original waypoints in expanded arrays (useful for plotting).
from interpolatepy import CubicSplineWithAcceleration1
# Trajectory with specific boundary conditions
spline = CubicSplineWithAcceleration1(
t_points=[0, 1, 2, 3],
q_points=[0, 1, 0, 1],
v0=1.0, vn=-0.5, # Velocity constraints
a0=0.0, an=0.0 # Acceleration constraints
)
spline.plot() # Shows original vs virtual waypoints
File: c_s_with_acc2.py
Class: CubicSplineWithAcceleration2
Alternative approach to handling acceleration boundary conditions using quintic (5th-order) polynomials for the first and last segments while maintaining cubic segments in between.
Method 2 Approach:
This approach provides more flexibility in satisfying boundary conditions while maintaining computational efficiency.
class CubicSplineWithAcceleration2:
def __init__(self, t_points: list[float], q_points: list[float],
v0: float = 0.0, vn: float = 0.0,
a0: float = 0.0, an: float = 0.0)
Parameters: Same as Method 1
Key Methods: Same as CubicSpline
Files: b_spline.py, b_spline_*.py
Classes: BSpline, BSplineApproximation, BSplineInterpolator, etc.
B-splines provide a more general framework for curve construction using basis functions. A B-spline curve of degree p is defined as:
q(u) = ∑ᵢ₌₀ⁿ Pᵢ Nᵢ,ₚ(u)
where:
Key Properties:
Variants Implemented:
class BSplineInterpolator:
def __init__(self, degree: int, points: np.ndarray,
times: np.ndarray | None = None,
initial_velocity: np.ndarray | None = None,
final_velocity: np.ndarray | None = None)
Parameters:
degree: B-spline degree (3, 4, or 5)points: Control/waypoints (N×d array for d-dimensional curves)times: Time values (if None, uses uniform parameterization)initial_velocity, final_velocity: Boundary conditionsMotion profiles are classical trajectory planning methods optimized for robotics applications. They focus on bounded acceleration/jerk and smooth velocity transitions.
File: double_s.py
Class: DoubleSTrajectory
Implements double-S (jerk-bounded) trajectories that limit the rate of acceleration change. The velocity profile follows an S-curve shape with up to 7 distinct phases:
Mathematical Model:
The trajectory is computed by solving the time-optimal control problem:
minimize T
subject to: |q̇(t)| ≤ vₘₐₓ, |q̈(t)| ≤ aₘₐₓ, |q⃛(t)| ≤ jₘₐₓ
q(0) = q₀, q(T) = q₁, q̇(0) = v₀, q̇(T) = v₁
Phase Duration Calculation:
Kinematic Equations: For each phase i with jerk jᵢ, acceleration a₀ᵢ, velocity v₀ᵢ:
q̈ᵢ(t) = a₀ᵢ + jᵢt
q̇ᵢ(t) = v₀ᵢ + a₀ᵢt + ½jᵢt²
qᵢ(t) = q₀ᵢ + v₀ᵢt + ½a₀ᵢt² + ⅙jᵢt³
Constraints:
| Jerk limit: | q⃛(t) | ≤ jₘₐₓ |
| Acceleration limit: | q̈(t) | ≤ aₘₐₓ |
| Velocity limit: | q̇(t) | ≤ vₘₐₓ |
@dataclass
class StateParams:
q_0: float = 0.0 # Initial position
q_1: float = 1.0 # Final position
v_0: float = 0.0 # Initial velocity
v_1: float = 0.0 # Final velocity
@dataclass
class TrajectoryBounds:
v_bound: float = 1.0 # Velocity bound
a_bound: float = 1.0 # Acceleration bound
j_bound: float = 1.0 # Jerk bound
class DoubleSTrajectory:
def __init__(self, state_params: StateParams, bounds: TrajectoryBounds)
Key Methods:
def evaluate(self, t: float) -> float
def evaluate_velocity(self, t: float) -> float
def evaluate_acceleration(self, t: float) -> float
def evaluate_jerk(self, t: float) -> float
def get_duration(self) -> float
def plot(self) -> None
from interpolatepy import DoubleSTrajectory, StateParams, TrajectoryBounds
# Define trajectory parameters
state = StateParams(q_0=0, q_1=10, v_0=0, v_1=0)
bounds = TrajectoryBounds(v_bound=2.0, a_bound=1.0, j_bound=0.5)
# Create trajectory
traj = DoubleSTrajectory(state, bounds)
# Evaluate
t_eval = np.linspace(0, traj.get_duration(), 100)
positions = [traj.evaluate(t) for t in t_eval]
velocities = [traj.evaluate_velocity(t) for t in t_eval]
# Visualize all profiles
traj.plot()
File: trapezoidal.py
Class: TrapezoidalTrajectory
Classical trapezoidal velocity profiles consist of three phases:
Two trajectory types are supported:
The algorithm can operate in two modes:
Key Equations: For velocity-constrained mode:
vᵥ = min(vₘₐₓ, √(h·aₘₐₓ + (v₀² + v₁²)/2))
where h = q₁ - q₀ is the displacement.
@dataclass
class TrajectoryParams:
q0: float # Initial position
q1: float # Final position
t0: float = 0.0 # Start time
v0: float = 0.0 # Initial velocity
v1: float = 0.0 # Final velocity
amax: float | None = None # Maximum acceleration
vmax: float | None = None # Maximum velocity
duration: float | None = None # Total duration
class TrapezoidalTrajectory:
@staticmethod
def generate_trajectory(params: TrajectoryParams) ->
tuple[Callable[[float], tuple[float, float, float]], float]
Returns: (trajectory_function, duration)
The trajectory function takes time t and returns (position, velocity, acceleration).
from interpolatepy import TrapezoidalTrajectory, TrajectoryParams
# Velocity-constrained trajectory
params = TrajectoryParams(
q0=0, q1=5, v0=0, v1=0,
amax=2.0, vmax=1.5
)
traj_func, duration = TrapezoidalTrajectory.generate_trajectory(params)
# Evaluate trajectory
t = 1.0
pos, vel, acc = traj_func(t)
print(f"At t={t}: pos={pos:.2f}, vel={vel:.2f}, acc={acc:.2f}")
File: lin_poly_parabolic.py
Class: ParabolicBlendTrajectory
Constructs trajectories from linear segments connected by parabolic blends at via points. This approach:
Each blend has duration dt_blend[i] with acceleration:
aᵢ = (v_after[i] - v_before[i]) / dt_blend[i]
The total trajectory duration is extended by:
T_total = T_nominal + (dt_blend[0] + dt_blend[-1]) / 2
class ParabolicBlendTrajectory:
def __init__(self, q: list | np.ndarray, t: list | np.ndarray,
dt_blend: list | np.ndarray, dt: float = 0.01)
def generate(self) -> tuple[Callable[[float], tuple[float, float, float]], float]
Parameters:
q: Position waypointst: Nominal arrival timesdt_blend: Blend duration at each waypointdt: Sampling interval for plottingDirect polynomial fitting methods for trajectory generation with specified boundary conditions.
File: polynomials.py
Class: PolynomialTrajectory
Generates polynomial trajectories of orders 3, 5, and 7 to satisfy boundary conditions:
3rd Order (Cubic):
5th Order (Quintic):
7th Order (Septic):
The coefficients are determined by solving the linear system Ac = b where c contains the polynomial coefficients.
@dataclass
class BoundaryCondition:
position: float
velocity: float | None = None
acceleration: float | None = None
jerk: float | None = None
@dataclass
class TimeInterval:
t0: float
t1: float
class PolynomialTrajectory:
@staticmethod
def order_3_trajectory(initial: BoundaryCondition, final: BoundaryCondition,
interval: TimeInterval) -> Callable[[float], tuple[float, float, float, float]]
@staticmethod
def order_5_trajectory(initial: BoundaryCondition, final: BoundaryCondition,
interval: TimeInterval) -> Callable[[float], tuple[float, float, float, float]]
@staticmethod
def order_7_trajectory(initial: BoundaryCondition, final: BoundaryCondition,
interval: TimeInterval) -> Callable[[float], tuple[float, float, float, float]]
Returns: Function that takes time t and returns (position, velocity, acceleration, jerk)
from interpolatepy import PolynomialTrajectory, BoundaryCondition, TimeInterval
# Define boundary conditions
initial = BoundaryCondition(position=0, velocity=0, acceleration=0)
final = BoundaryCondition(position=1, velocity=0, acceleration=0)
interval = TimeInterval(t0=0, t1=2)
# Generate quintic trajectory
traj_func = PolynomialTrajectory.order_5_trajectory(initial, final, interval)
# Evaluate
pos, vel, acc, jerk = traj_func(1.0)
File: linear.py
Function: linear_traj
Simple linear interpolation between two points with constant velocity and zero acceleration:
q(t) = q₀ + (q₁ - q₀) * (t - t₀) / (t₁ - t₀)
q̇(t) = (q₁ - q₀) / (t₁ - t₀)
q̈(t) = 0
Supports both scalar and vector positions with proper broadcasting.
def linear_traj(p0: float | list[float] | np.ndarray,
p1: float | list[float] | np.ndarray,
t0: float, t1: float,
time_array: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]
Returns: (positions, velocities, accelerations) arrays
Specialized algorithms for smooth rotation interpolation using unit quaternions. These methods handle the double-cover property and provide C² continuous orientation trajectories.
File: quat_core.py
Class: Quaternion
Implements fundamental quaternion operations for 3D rotations. A unit quaternion q = w + xi + yj + zk represents a rotation, where:
Key Properties:
| Unit constraint: | q | = w² + x² + y² + z² = 1 |
Core Operations Implemented:
class Quaternion:
def __init__(self, w: float, x: float, y: float, z: float)
# Basic operations
def norm(self) -> float
def unit(self) -> 'Quaternion'
def inverse(self) -> 'Quaternion'
def conjugate(self) -> 'Quaternion'
# Interpolation
def slerp(self, other: 'Quaternion', t: float) -> 'Quaternion'
def squad(self, s1: 'Quaternion', s2: 'Quaternion',
other: 'Quaternion', t: float) -> 'Quaternion'
# Advanced operations
def Log(self) -> 'Quaternion'
def exp(self) -> 'Quaternion'
def to_axis_angle(self) -> tuple[np.ndarray, float]
@classmethod
def from_angle_axis(cls, angle: float, axis: np.ndarray) -> 'Quaternion'
File: log_quat.py
Class: LogQuaternionInterpolation
Note: This class is available in the
log_quat.pymodule but not exported in the main__init__.py. Import directly:from interpolatepy.log_quat import LogQuaternionInterpolation
Implements the Logarithmic Quaternion Interpolation (LQI) method from Parker et al. (2023). The algorithm:
Key Advantages:
Algorithm 1 (Continuous Recovery):
class LogQuaternionInterpolation:
def __init__(self, time_points: list[float], quaternions: list[Quaternion],
degree: int = 3,
initial_velocity: np.ndarray | None = None,
final_velocity: np.ndarray | None = None)
def evaluate(self, t: float) -> Quaternion
def evaluate_velocity(self, t: float) -> np.ndarray # Angular velocity
def evaluate_acceleration(self, t: float) -> np.ndarray # Angular acceleration
File: log_quat.py
Class: ModifiedLogQuaternionInterpolation
Note: This class is available in the
log_quat.pymodule but not exported in the main__init__.py. Import directly:from interpolatepy.log_quat import ModifiedLogQuaternionInterpolation
Enhanced version of LQI that decouples angle and axis interpolation:
Benefits:
class ModifiedLogQuaternionInterpolation:
def __init__(self, time_points: list[float], quaternions: list[Quaternion],
degree: int = 3, normalize_axis: bool = True,
initial_velocity: np.ndarray | None = None, # 4D: [θ̇, Ẋ, Ẏ, Ż]
final_velocity: np.ndarray | None = None)
def evaluate(self, t: float) -> Quaternion
def evaluate_velocity(self, t: float) -> np.ndarray # 4D derivative vector
def evaluate_acceleration(self, t: float) -> np.ndarray # 4D second derivative
File: squad_c2.py
Class: SquadC2
Implements C²-continuous SQUAD interpolation using the method from Wittmann et al. (ICRA 2023). Key innovations:
Extended Quaternion Sequence:
Corrected Intermediate Quaternions: Uses corrected formula (Equation 5) that properly handles non-uniform time spacing:
sᵢ = qᵢ ⊗ exp[log(qᵢ⁻¹⊗qᵢ₊₁)/(-2(1+hᵢ/hᵢ₋₁)) + log(qᵢ⁻¹⊗qᵢ₋₁)/(-2(1+hᵢ₋₁/hᵢ))]
Quintic Polynomial Parameterization:
class SquadC2:
def __init__(self, time_points: list[float], quaternions: list[Quaternion],
normalize_quaternions: bool = True,
validate_continuity: bool = True)
def evaluate(self, t: float) -> Quaternion
def evaluate_velocity(self, t: float) -> np.ndarray
def evaluate_acceleration(self, t: float) -> np.ndarray
def get_waypoints(self) -> tuple[list[float], list[Quaternion]] # Original
def get_extended_waypoints(self) -> tuple[list[float], list[Quaternion]] # With virtual
from interpolatepy import SquadC2, Quaternion
import numpy as np
# Define rotation waypoints
times = [0, 1, 2, 3]
quats = [
Quaternion.identity(),
Quaternion.from_angle_axis(np.pi/2, [1, 0, 0]), # 90° about X
Quaternion.from_angle_axis(np.pi, [0, 1, 0]), # 180° about Y
Quaternion.from_angle_axis(np.pi/4, [0, 0, 1]) # 45° about Z
]
# Create C² continuous interpolator
squad = SquadC2(times, quats)
# Evaluate smooth trajectory
t_eval = np.linspace(0, 3, 100)
trajectory = [squad.evaluate(t) for t in t_eval]
angular_vels = [squad.evaluate_velocity(t) for t in t_eval]
Utility algorithms for path-following, coordinate frame computation, and geometric operations.
File: frenet_frame.py
Function: compute_trajectory_frames
Computes Frenet frames (moving coordinate systems) along parametric curves. For a curve p(u), the Frenet frame consists of:
| Tangent vector: T = p’(u)/ | p’(u) | (direction of motion) |
| Normal vector: N = T’(u)/ | T’(u) | (direction of curvature) |
Applications:
Extensions:
def compute_trajectory_frames(
position_func: Callable[[float], tuple[np.ndarray, np.ndarray, np.ndarray]],
u_values: np.ndarray,
tool_orientation: float | tuple[float, float, float] | None = None
) -> tuple[np.ndarray, np.ndarray]
Parameters:
position_func: Function returning (position, first_derivative, second_derivative)u_values: Parameter values for frame computationtool_orientation: Additional rotation (angle or RPY tuple)Returns: (points, frames) where frames[i] = [tangent, normal, binormal]
Helper Functions:
def helicoidal_trajectory_with_derivatives(u: float, r: float = 2.0, d: float = 0.5)
def circular_trajectory_with_derivatives(u: float, r: float = 2.0)
def plot_frames(ax, points: np.ndarray, frames: np.ndarray, scale: float = 0.5)
from interpolatepy import compute_trajectory_frames, helicoidal_trajectory_with_derivatives
import numpy as np
# Define helix parameters
u_values = np.linspace(0, 4*np.pi, 100)
# Compute Frenet frames with tool orientation
points, frames = compute_trajectory_frames(
helicoidal_trajectory_with_derivatives,
u_values,
tool_orientation=(0.1, 0.2, 0.3) # Roll, pitch, yaw
)
# Visualize (requires matplotlib)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plot_frames(ax, points, frames, skip=10)
File: simple_paths.py
Classes: LinearPath, CircularPath
Provides analytical path representations for common geometric primitives.
LinearPath:
CircularPath:
class LinearPath:
def __init__(self, pi: np.ndarray, pf: np.ndarray)
def position(self, s: float) -> np.ndarray
def velocity(self, s: float) -> np.ndarray
def acceleration(self, s: float) -> np.ndarray
def evaluate_at(self, s_values: np.ndarray) -> dict[str, np.ndarray]
class CircularPath:
def __init__(self, r: np.ndarray, d: np.ndarray, pi: np.ndarray)
# r: axis direction, d: point on axis, pi: point on circle
def position(self, s: float) -> np.ndarray
def velocity(self, s: float) -> np.ndarray
def acceleration(self, s: float) -> np.ndarray
def evaluate_at(self, s_values: np.ndarray) -> dict[str, np.ndarray]
Supporting numerical methods and shared functionality.
File: tridiagonal_inv.py
Function: solve_tridiagonal
Efficient solver for tridiagonal linear systems of the form:
[b₁ c₁ 0 ... 0 ] [x₁] [d₁]
[a₂ b₂ c₂ ... 0 ] [x₂] [d₂]
[0 a₃ b₃ ... 0 ] [x₃] = [d₃]
[⋮ ⋮ ⋮ ⋱ ⋮ ] [⋮ ] [⋮ ]
[0 0 0 ... bₙ] [xₙ] [dₙ]
Uses Thomas algorithm (specialized Gaussian elimination) with O(n) complexity instead of O(n³) for general systems.
Applications in InterpolatePy:
def solve_tridiagonal(a: np.ndarray, b: np.ndarray, c: np.ndarray, d: np.ndarray) -> np.ndarray
Parameters:
a: Lower diagonal (length n, first element unused)b: Main diagonal (length n)c: Upper diagonal (length n, last element unused)d: Right-hand side vector (length n)Returns: Solution vector x
Note: Modifies input arrays during computation for memory efficiency.
This completes the comprehensive algorithm reference for InterpolatePy. Each algorithm is designed to integrate seamlessly with the others while providing specialized functionality for different trajectory planning scenarios.
For implementation examples and advanced usage patterns, see the examples/ directory in the repository.