diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml
index 44fce04..64e6515 100644
--- a/.github/workflows/pre-commit.yml
+++ b/.github/workflows/pre-commit.yml
@@ -13,5 +13,5 @@ jobs:
- name: Set up Python
uses: actions/setup-python@v5
with:
- python-version: '3.11'
+ python-version: '3.10'
- uses: pre-commit/action@v3.0.1
diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml
index c3825f9..b7d635c 100644
--- a/.github/workflows/publish.yml
+++ b/.github/workflows/publish.yml
@@ -12,7 +12,7 @@ jobs:
- name: Set up Python
uses: actions/setup-python@v5
with:
- python-version: '3.11'
+ python-version: '3.10'
- name: Install dependencies
run: |
python -m pip install --upgrade pip
diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
index f2f678c..866ee00 100644
--- a/.github/workflows/test.yml
+++ b/.github/workflows/test.yml
@@ -1,3 +1,4 @@
+
name: ci-test
on:
@@ -9,34 +10,40 @@ on:
jobs:
build-linux:
runs-on: ubuntu-latest
+ strategy:
+ matrix:
+ python-version: ['3.10', '3.11', '3.12']
steps:
- - uses: actions/checkout@v4
- - name: Set up Python 3.11
- uses: actions/setup-python@v5
- with:
- python-version: 3.11
- - name: Install dependencies
- run: |
- python -m pip install --upgrade pip
- pip install -r requirements-dev.txt
- pip install -e .
- - name: Testing
- run: |
- python -m pytest tests
+ - uses: actions/checkout@v4
+ - name: Set up Python ${{ matrix.python-version }}
+ uses: actions/setup-python@v5
+ with:
+ python-version: ${{ matrix.python-version }}
+ - name: Install dependencies
+ run: |
+ python -m pip install --upgrade pip
+ pip install -r requirements-dev.txt
+ pip install -e .
+ - name: Testing
+ run: |
+ python -m pytest tests
build-macos:
runs-on: macos-latest
+ strategy:
+ matrix:
+ python-version: ['3.10', '3.11', '3.12']
steps:
- - uses: actions/checkout@v4
- - name: Set up Python 3.11
- uses: actions/setup-python@v5
- with:
- python-version: 3.11
- - name: Install dependencies
- run: |
- python -m pip install --upgrade pip
- pip install -r requirements-dev.txt
- pip install -e .
- - name: Testing
- run: |
- python -m pytest tests
+ - uses: actions/checkout@v4
+ - name: Set up Python ${{ matrix.python-version }}
+ uses: actions/setup-python@v5
+ with:
+ python-version: ${{ matrix.python-version }}
+ - name: Install dependencies
+ run: |
+ python -m pip install --upgrade pip
+ pip install -r requirements-dev.txt
+ pip install -e .
+ - name: Testing
+ run: |
+ python -m pytest tests
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 129272e..e3e5cfa 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -21,7 +21,7 @@ repos:
- id: black
- repo: https://github.com/astral-sh/ruff-pre-commit
- rev: 'v0.11.0'
+ rev: 'v0.11.10'
hooks:
- id: ruff
types_or: [python, pyi, jupyter]
@@ -32,3 +32,5 @@ repos:
hooks:
- id: mypy
args: [--ignore-missing-imports]
+
+# pre-commit autoupdate
diff --git a/README.md b/README.md
index b65c746..3f9d446 100644
--- a/README.md
+++ b/README.md
@@ -1,366 +1,164 @@
# InterpolatePy
-
+
+[](https://pepy.tech/projects/interpolatepy)
[](https://github.com/GiorgioMedico/InterpolatePy/actions/workflows/pre-commit.yml)
[](https://github.com/GiorgioMedico/InterpolatePy/actions/workflows/test.yml)
[](https://opensource.org/licenses/MIT)
-## Support the Project
-
-If you find InterpolatePy useful for your work, please consider giving it a star on GitHub! ⭐
-
-Your stars help make the project more visible to others who might benefit from these tools and encourage continued development and maintenance.
-
-[](https://github.com/GiorgioMedico/InterpolatePy/stargazers/)
-
-Have you implemented something cool with InterpolatePy? We'd love to hear about it in the Issues section or as a feature on our wiki!
-
-## Table of Contents
-
-- [InterpolatePy](#interpolatepy)
- - [Support the Project](#support-the-project)
- - [Table of Contents](#table-of-contents)
- - [Overview](#overview)
- - [Future Implementations](#future-implementations)
- - [Key Features](#key-features)
- - [Spline Interpolation](#spline-interpolation)
- - [B-Splines](#b-splines)
- - [Cubic Splines](#cubic-splines)
- - [Imposing Acceleration Constraints at Endpoints](#imposing-acceleration-constraints-at-endpoints)
- - [Motion Profiles](#motion-profiles)
- - [Path Generation](#path-generation)
- - [Utility Functions](#utility-functions)
- - [Installation](#installation)
- - [Using pip](#using-pip)
- - [From Source](#from-source)
- - [Optional Dependencies](#optional-dependencies)
- - [Usage Examples](#usage-examples)
- - [Cubic Spline Trajectory](#cubic-spline-trajectory)
- - [Cubic Spline with Acceleration Constraints](#cubic-spline-with-acceleration-constraints)
- - [Double-S Trajectory](#double-s-trajectory)
- - [B-Spline Curve](#b-spline-curve)
- - [Trapezoidal Trajectory with Waypoints](#trapezoidal-trajectory-with-waypoints)
- - [3D Path with Frenet Frames](#3d-path-with-frenet-frames)
- - [Mathematical Concepts](#mathematical-concepts)
- - [B-splines](#b-splines-1)
- - [Cubic Splines](#cubic-splines-1)
- - [Smoothing Splines](#smoothing-splines)
- - [Trapezoidal Velocity Profiles](#trapezoidal-velocity-profiles)
- - [Double-S Trajectories](#double-s-trajectories)
- - [Frenet Frames](#frenet-frames)
- - [Requirements](#requirements)
- - [Development](#development)
- - [Running Tests](#running-tests)
- - [Contributing](#contributing)
- - [License](#license)
- - [Acknowledgments](#acknowledgments)
- - [Citation](#citation)
+> **Smooth trajectories, precise motion — one library.**
+> InterpolatePy brings together classic and modern interpolation techniques for robotics, animation, and scientific computing in a single, easy‑to‑use Python package.
-## Overview
+---
-InterpolatePy is a comprehensive Python library for generating smooth trajectories and curves with precise control over position, velocity, acceleration, and jerk profiles. Designed for robotics, motion planning, computer graphics, and scientific computing applications, it provides a wide range of interpolation techniques from simple linear interpolation to advanced B-splines and motion profiles.
+## ⭐️ Support the Project
-Whether you need to generate smooth robotic joint motions, create path planning for autonomous vehicles, or design animation curves with specific dynamic properties, InterpolatePy offers the tools to create trajectories that maintain continuity while adhering to physical constraints.
+If InterpolatePy saves you time or powers your research, please consider **starring** the repo – it helps others discover the project and motivates future development!
-## Future Implementations
-InterpolatePy is continuously evolving, with several exciting features planned for future releases:
+Have you built something cool on top of InterpolatePy? Open an issue or start a discussion – we’d love to showcase community projects.
-- [ ] **Bezier Curves**: Implementation of parametric Bezier curves with arbitrary degree control
-- [ ] **Linear Interpolation with Polynomial Blend**: Enhanced linear interpolation using quintic Bezier curves for smooth transitions between line segments
-- [ ] **Linear Interpolation with Parabolic Blends**: An alternative blending approach using parabolic segments for smooth connections
-- [ ] **Spherical Path**: Tools for interpolation along spherical paths
-- [ ] **LERP (Linear Interpolation)**: Linear interpolation functions for quaternions
-- [ ] **SLERP (Spherical Linear Interpolation)**: Algorithms for smooth interpolation between orientations represented as quaternions
-- [ ] **SQUAD (Spherical Quadrangle Interpolation)**: Advanced spherical interpolation with cubic-like smoothness for quaternion interpolation
-- [ ] **B-spline Quaternion Curves**: Extension of B-spline techniques to quaternion space for smooth orientation interpolation
-These implementations will significantly enhance InterpolatePy's capabilities for orientation interpolation and complex curve generation, particularly for robotic applications, character animation, and camera path planning.
+## Overview
-## Key Features
+InterpolatePy is a **comprehensive collection of trajectory‑generation algorithms** – from simple linear blends to high‑order B‑splines – with a consistent, NumPy‑friendly API. Designed for robotics, animation, path planning, and data smoothing, it lets you craft trajectories that respect position, velocity, acceleration **and** jerk constraints.
-### Spline Interpolation
+Key design goals:
-#### B-Splines
+* **Breadth** – one package for splines *and* motion profiles.
+* **Visualization‑ready** – every spline exposes `plot()` helpers built on Matplotlib.
+* **Pure Python ≥ 3.10** – no compiled extensions; installs quickly everywhere.
-- **BSpline**: Versatile implementation with customizable degree and knot vectors
-- **ApproximationBSpline**: Efficiently approximates sets of points with a B-spline curve
-- **CubicBSplineInterpolation**: Specialized cubic B-spline interpolation that passes through all points
-- **BSplineInterpolator**: General B-spline interpolation with controllable continuity (C²-C⁴)
-- **SmoothingCubicBSpline**: B-splines with adjustable smoothness-vs-accuracy tradeoff
+---
-#### Cubic Splines
+## Roadmap
-- **CubicSpline**: Standard cubic spline with velocity constraints at endpoints
-- **CubicSplineWithAcceleration1**: Cubic spline with velocity and acceleration constraints (extra points method)
-- **CubicSplineWithAcceleration2**: Alternative cubic spline with acceleration constraints (quintic segments method)
-- **CubicSmoothingSpline**: Cubic splines with μ parameter for smoothness control
-- **SplineConfig/smoothing_spline_with_tolerance**: Tools for finding optimal smoothing parameters
+Upcoming features (✅ done, 🚧 planned):
-##### Imposing Acceleration Constraints at Endpoints
+| Status | Feature |
+| ------ | ------------------------------------------------------------------------- |
+| 🚧 | **Bezier curves** – arbitrary degree |
+| 🚧 | **Quaternion interpolation**: LERP / SLERP / SQUAD & B‑spline‑quaternions |
+| 🚧 | **Linear blends** with quintic/parabolic smoothing |
+| 🚧 | **Spherical paths** & great‑circle splines |
-InterpolatePy offers two distinct methods for implementing cubic splines with endpoint acceleration constraints:
+---
-1. **Extra Points Method (`CubicSplineWithAcceleration1`)**: This approach adds two extra points in the first and last segments to satisfy the acceleration constraints while maintaining C² continuity throughout the entire curve. The extra points are placed at the midpoints of the first and last segments, with positions calculated to ensure the specified accelerations at endpoints are achieved.
+## Key Features
-2. **Quintic Segments Method (`CubicSplineWithAcceleration2`)**: This approach uses standard cubic polynomials for interior segments, but replaces the first and last segments with quintic (5th degree) polynomials. The higher degree provides the additional degrees of freedom needed to satisfy the acceleration constraints at endpoints while maintaining overall C² continuity at all knot points.
+### 1 · Spline Interpolation
-### Motion Profiles
+* **B‑Splines** – cubic, approximating, smoothing.
+* **Cubic Splines** – with optional velocity/acceleration endpoint constraints.
+* **Global B‑Spline Interpolation** – C², C³, C⁴ continuity (degree 3–5).
-- **DoubleSTrajectory**: S-curve motion profile with bounded velocity, acceleration, and jerk
-- **linear_traj**: Simple linear interpolation with constant velocity
-- **PolynomialTrajectory**: Trajectory generation using polynomials of orders 3, 5, and 7
-- **TrapezoidalTrajectory**: Trapezoidal velocity profiles with various constraint options
+### 2 · Motion Profiles
-### Path Generation
+* **Double‑S** (S‑curve) – bounded jerk.
+* **Trapezoidal** – classic industrial profile.
+* **Polynomial** – 3/5/7‑order with boundary conditions.
-- **LinearPath**: Simple linear paths with constant velocity
-- **CircularPath**: Circular arcs and paths in 3D
-- **Frenet Frames**: Tools for computing and visualizing Frenet frames along parametric curves
+### 3 · Path Utilities
-### Utility Functions
+* **Linear & Circular paths** in 3‑D.
+* **Frenet frames** helper for tool orientation along curves.
-- **solve_tridiagonal**: Efficient tridiagonal matrix solver (Thomas algorithm)
+---
## Installation
-### Using pip
+InterpolatePy lives on PyPI. Install the latest stable release with:
```bash
pip install InterpolatePy
```
-### From Source
-
-To install the latest development version with all dependencies:
+Development version (with test & dev extras):
```bash
-# Clone the repository
git clone https://github.com/GiorgioMedico/InterpolatePy.git
cd InterpolatePy
-
-# Install with development dependencies
-pip install -e ".[all]"
+pip install -e '.[all]'
```
-### Optional Dependencies
-
-You can install specific dependency groups:
+Optional extras:
```bash
-# For testing dependencies only
-pip install -e ".[test]"
-
-# For development tools only
-pip install -e ".[dev]"
+pip install InterpolatePy[test] # testing only
+pip install InterpolatePy[dev] # linting & build tools
```
-## Usage Examples
-
-### Cubic Spline Trajectory
+---
-Create a smooth trajectory through waypoints with velocity constraints:
+## Quick Start
```python
from interpolatepy.cubic_spline import CubicSpline
-# Define waypoints
-t_points = [0.0, 5.0, 7.0, 8.0, 10.0, 15.0, 18.0]
-q_points = [3.0, -2.0, -5.0, 0.0, 6.0, 12.0, 8.0]
-
-# Create cubic spline with initial and final velocities
-spline = CubicSpline(t_points, q_points, v0=2.0, vn=-3.0)
-
-# Evaluate at specific time
-position = spline.evaluate(6.0)
-velocity = spline.evaluate_velocity(6.0)
-acceleration = spline.evaluate_acceleration(6.0)
-
-# Plot the trajectory with position, velocity, and acceleration profiles
-spline.plot()
-```
-
-### Cubic Spline with Acceleration Constraints
+t = [0, 5, 10]
+q = [0, 2, 0]
-Create a smooth trajectory with both velocity and acceleration constraints at endpoints:
-
-```python
-from interpolatepy.c_s_with_acc2 import CubicSplineWithAcceleration2, SplineParameters
-
-# Define waypoints
-t_points = [0.0, 5.0, 7.0, 8.0, 10.0, 15.0, 18.0]
-q_points = [3.0, -2.0, -5.0, 0.0, 6.0, 12.0, 8.0]
-
-# Create parameters with velocity and acceleration constraints
-params = SplineParameters(
- v0=2.0, # Initial velocity
- vn=-3.0, # Final velocity
- a0=0.0, # Initial acceleration
- an=0.0 # Final acceleration
-)
-
-# Create spline with quintic segments at endpoints
-spline = CubicSplineWithAcceleration2(t_points, q_points, params)
-
-# Evaluate at specific time
-position = spline.evaluate(6.0)
-velocity = spline.evaluate_velocity(6.0)
-acceleration = spline.evaluate_acceleration(6.0)
-
-# Plot the trajectory
-spline.plot()
+spline = CubicSpline(t, q, v0=0.0, vn=0.0)
+print(spline.evaluate(7.5)) # position at t = 7.5 s
+spline.plot() # visualize position/velocity/acceleration
```
-### Double-S Trajectory
-
-Generate a trajectory with bounded jerk for smooth motion profiles:
-
-```python
-from interpolatepy.double_s import DoubleSTrajectory, StateParams, TrajectoryBounds
-
-# Create parameters for trajectory
-state = StateParams(q_0=0.0, q_1=10.0, v_0=0.0, v_1=0.0)
-bounds = TrajectoryBounds(v_bound=5.0, a_bound=10.0, j_bound=30.0)
-
-# Create trajectory
-trajectory = DoubleSTrajectory(state, bounds)
+---
-# Get trajectory information
-duration = trajectory.get_duration()
-phases = trajectory.get_phase_durations()
-
-# Generate trajectory points
-import numpy as np
-time_points = np.linspace(0, duration, 100)
-positions, velocities, accelerations, jerks = trajectory.evaluate(time_points)
-```
-
-### B-Spline Curve
+## Usage Examples
-Create and manipulate a B-spline curve with control points:
+
+Cubic spline with velocity constraints
```python
-import numpy as np
-from interpolatepy.b_spline import BSpline
-
-# Define control points, degree, and knot vector
-control_points = np.array([[0, 0], [1, 2], [3, 1], [4, 0]])
-degree = 3
-knots = BSpline.create_uniform_knots(degree, len(control_points))
-
-# Create B-spline
-bspline = BSpline(degree, knots, control_points)
-
-# Evaluate at parameter value
-point = bspline.evaluate(0.5)
+from interpolatepy.cubic_spline import CubicSpline
-# Generate curve points for plotting
-u_values, curve_points = bspline.generate_curve_points(100)
+t_points = [0.0, 5.0, 7.0, 10.0]
+q_points = [1.0, 3.0, -1.0, 2.0]
-# Plot the curve with control polygon
-bspline.plot_2d(show_control_polygon=True)
+s = CubicSpline(t_points, q_points, v0=1.0, vn=0.0)
+position = s.evaluate(6.0)
```
-### Trapezoidal Trajectory with Waypoints
+
-Generate a trajectory with trapezoidal velocity profile through multiple points:
+
+Double‑S trajectory
```python
-from interpolatepy.trapezoidal import TrapezoidalTrajectory, InterpolationParams
-
-# Define waypoints
-points = [0.0, 5.0, 3.0, 8.0, 2.0]
-
-# Create interpolation parameters
-params = InterpolationParams(
- points=points,
- v0=0.0, # Initial velocity
- vn=0.0, # Final velocity
- amax=10.0, # Maximum acceleration
- vmax=5.0 # Maximum velocity
-)
-
-# Generate trajectory
-traj_func, duration = TrapezoidalTrajectory.interpolate_waypoints(params)
-
-# Evaluate at specific time
-position, velocity, acceleration = traj_func(2.5)
-```
-
-### 3D Path with Frenet Frames
-
-Create and visualize a trajectory with coordinate frames along the path:
+from interpolatepy.double_s import DoubleSTrajectory, StateParams, TrajectoryBounds
-```python
-import numpy as np
-import matplotlib.pyplot as plt
-from interpolatepy.frenet_frame import (
- helicoidal_trajectory_with_derivatives,
- compute_trajectory_frames,
- plot_frames
-)
-
-# Create a helicoidal path
-u_values = np.linspace(0, 4 * np.pi, 100)
-def helix_func(u):
- return helicoidal_trajectory_with_derivatives(u, r=2.0, d=0.5)
-
-# Compute Frenet frames along the path
-points, frames = compute_trajectory_frames(helix_func, u_values)
-
-# Visualize
-fig = plt.figure(figsize=(10, 8))
-ax = fig.add_subplot(111, projection='3d')
-plot_frames(ax, points, frames, scale=0.5, skip=10)
-plt.show()
+state = StateParams(q_0=0, q_1=10, v_0=0, v_1=0)
+bounds = TrajectoryBounds(v_bound=5, a_bound=10, j_bound=30)
+traj = DoubleSTrajectory(state, bounds)
```
-## Mathematical Concepts
-
-InterpolatePy implements several key mathematical concepts for trajectory generation:
-
-### B-splines
-
-Piecewise parametric curves defined by control points and a knot vector. B-splines offer local control (changes to a control point only affect the curve locally) and customizable continuity.
-
-### Cubic Splines
-
-Piecewise polynomials with C² continuity (continuous position, velocity, and acceleration) that interpolate a given set of points.
+
-### Smoothing Splines
+For more, see the [examples folder](examples/) or the full API docs (coming soon).
-Splines with a controllable balance between accuracy (passing through points exactly) and smoothness (minimizing curvature). The μ parameter controls this tradeoff.
-
-### Trapezoidal Velocity Profiles
-
-Trajectories with linear segments of constant acceleration and velocity, creating a trapezoidal shape in the velocity profile.
-
-### Double-S Trajectories
-
-Motion profiles with bounded jerk, acceleration, and velocity, creating smooth S-curves in the acceleration profile. These are ideal for robotic motion to reduce stress on mechanical systems.
-
-### Frenet Frames
-
-Local coordinate systems defined by tangent, normal, and binormal vectors along a curve, useful for tool orientation and trajectory tracking.
+---
## Requirements
-- Python 3.10+
-- NumPy 2.0.0+
-- Matplotlib 3.10.1+
-- SciPy 1.15.2+
+* Python ≥ 3.10
+* NumPy ≥ 2.0
+* SciPy ≥ 1.15
+* Matplotlib ≥ 3.10
+
+---
## Development
InterpolatePy uses modern Python tooling for development:
-- **Code Quality**: Black and isort for formatting, Ruff and mypy for linting and type checking
-- **Testing**: pytest for unit tests and benchmarks
+* **Code Quality**: Black and isort for formatting, Ruff and mypy for linting and type checking
+* **Testing**: pytest for unit tests and benchmarks
To set up the development environment:
```bash
-pip install -e ".[all]"
+pip install -e '.[all]'
pre-commit install
```
@@ -372,42 +170,51 @@ python -m pytest tests
## Contributing
-Contributions to InterpolatePy are welcome! To contribute:
+We love pull requests — thanks for helping improve **InterpolatePy**!
-1. Fork the repository
-2. Create a feature branch
-3. Add your changes
-4. Run tests to ensure they pass
-5. Submit a pull request
+1. **Fork** the repository and create a descriptive branch (`feat/my-feature`).
+2. **Install** dev dependencies:
-Please follow the existing code style and include appropriate tests for new features.
+ ```bash
+ pip install -e '.[all]'
+ pre-commit install
+ ```
+3. **Code** your change, following our style (Black, isort, Ruff, mypy).
+4. **Test** with `pytest` and run `pre-commit run --all-files`.
+5. **Open** a pull request and explain *why* & *how* your change helps.
-## License
+For larger ideas, open an issue first so we can discuss direction and scope.
-InterpolatePy is released under the MIT License. See the [LICENSE](https://github.com/GiorgioMedico/InterpolatePy/blob/main/LICENSE) file for more details.
+---
## Acknowledgments
InterpolatePy implements algorithms and mathematical concepts primarily from the following authoritative textbooks:
-- Biagiotti, L., & Melchiorri, C. (2008). *Trajectory Planning for Automatic Machines and Robots*. Springer.
-- Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2010). *Robotics: Modelling, Planning and Control*. Springer.
+* **Biagiotti, L., & Melchiorri, C.** (2008). *Trajectory Planning for Automatic Machines and Robots*. Springer.
+* **Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G.** (2010). *Robotics: Modelling, Planning and Control*. Springer.
The library's implementation draws heavily from the theoretical frameworks, mathematical formulations, and algorithms presented in these works.
I express my gratitude to these authors for their significant contributions to the field of trajectory planning and robotics, which have made this library possible.
+---
+
+## License
+
+InterpolatePy is released under the MIT License – do whatever you want, but please give credit.
+
+---
+
## Citation
-If you use InterpolatePy in your research or project, please cite it as follows:
+If InterpolatePy contributes to your academic work, consider citing it:
-```bibtex
-@software{InterpolatePy,
- author = {Medico, Giorgio},
- title = {InterpolatePy: A Comprehensive Python Library for Trajectory Planning and Interpolation},
- year = {2025},
- url = {https://github.com/GiorgioMedico/InterpolatePy}
+```text
+@misc{InterpolatePy,
+ author = {Giorgio Medico},
+ title = {InterpolatePy: Trajectory and Spline Library},
+ year = {2025},
+ howpublished = {\url{https://github.com/GiorgioMedico/InterpolatePy}}
}
```
-
-For specific methods or algorithms implemented in InterpolatePy, please also consider citing the original research papers or textbooks referenced in the documentation and acknowledgments section.
diff --git a/examples/lin_poly_parabolic_ex.py b/examples/lin_poly_parabolic_ex.py
new file mode 100644
index 0000000..3860f03
--- /dev/null
+++ b/examples/lin_poly_parabolic_ex.py
@@ -0,0 +1,123 @@
+from collections.abc import Callable
+from collections.abc import Sequence
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from interpolatepy.lin_poly_parabolic import ParabolicBlendTrajectory
+
+
+def plot_trajectory_with_waypoints(
+ traj: ParabolicBlendTrajectory,
+ q: Sequence[float],
+ t: Sequence[float],
+) -> None:
+ """Plot the trajectory along with waypoints highlighted.
+
+ The function generates position, velocity, and acceleration profiles
+ from the provided trajectory and displays them in three subplots.
+ A dashed linear interpolation between waypoints is also plotted.
+
+ Parameters
+ ----------
+ traj : ParabolicBlendTrajectory
+ Trajectory object created with waypoints and blend durations.
+ q : Sequence[float]
+ List or array of position waypoints.
+ t : Sequence[float]
+ List or array of times corresponding to each waypoint.
+
+ Returns
+ -------
+ None
+ Displays the plot directly.
+ """
+ traj_func, duration = traj.generate()
+ times = np.arange(0.0, duration + traj.dt, traj.dt)
+
+ # Evaluate trajectory at each time point
+ positions = np.empty_like(times)
+ velocities = np.empty_like(times)
+ accelerations = np.empty_like(times)
+
+ for i, time in enumerate(times):
+ positions[i], velocities[i], accelerations[i] = traj_func(time)
+
+ # Adjust waypoint times to align with trajectory time scale
+ adjusted_t = [time + traj.dt_blend[0] / 2 for time in t]
+
+ # Create the figure with three subplots
+ fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+
+ # Plot position trajectory with waypoints
+ ax1.plot(times, positions, label="Position Trajectory")
+ linear_positions = np.interp(times, adjusted_t, q)
+ ax1.plot(times, linear_positions, "--", label="Linear Interpolation")
+ ax1.scatter(adjusted_t, q, color="red", s=50, zorder=5, label="Via Points")
+ ax1.set_ylabel("Position")
+ ax1.legend()
+ ax1.grid(True)
+
+ # Plot velocity profile
+ ax2.plot(times, velocities, label="Velocity")
+ ax2.set_ylabel("Velocity")
+ ax2.axhline(0, linestyle="--", alpha=0.3)
+ ax2.legend()
+ ax2.grid(True)
+
+ # Plot acceleration profile
+ ax3.plot(times, accelerations, label="Acceleration")
+ ax3.set_ylabel("Acceleration")
+ ax3.set_xlabel("Time [s]")
+ ax3.axhline(0, linestyle="--", alpha=0.3)
+ ax3.legend()
+ ax3.grid(True)
+
+ fig.tight_layout()
+ plt.show()
+
+
+if __name__ == "__main__":
+ # Define waypoints and blend durations
+ q: list[float] = [0, 2 * np.pi, np.pi / 2, np.pi]
+ t: list[float] = [0, 2, 3, 5]
+ dt_blend: np.ndarray = np.full(len(t), 0.6)
+
+ print("Creating parabolic blend trajectory with:")
+ print(f" Positions: {q}")
+ print(f" Times: {t}")
+ print(f" Blend durations: {dt_blend}")
+
+ traj: ParabolicBlendTrajectory = ParabolicBlendTrajectory(q, t, dt_blend)
+
+ # Option 1: Use the built-in plot method
+ print("\nPlotting trajectory using built-in plot method...")
+ traj.plot()
+
+ # Option 2: Custom plotting with waypoints highlighted
+ print("\nPlotting trajectory with highlighted waypoints...")
+ plot_trajectory_with_waypoints(traj, q, t)
+
+ # Option 3: Direct usage of the trajectory function
+ print("\nDirect usage of trajectory function:")
+ traj_func: Callable[[float], tuple[float, float, float]]
+ traj_func, duration = traj.generate()
+
+ # Evaluate at specific times
+ evaluation_times: list[float] = [0.5, 2.1, 3.5, 4.8]
+ print(f"\nTotal trajectory duration: {duration:.2f} seconds")
+ print("\nEvaluating trajectory at specific time points:")
+
+ for time_point in evaluation_times:
+ position, velocity, acceleration = traj_func(time_point)
+ print(
+ f"At t={time_point:.2f}s: position={position:.4f}, velocity={velocity:.4f}, acceleration={acceleration:.4f}" # noqa: E501
+ )
+
+ # Demonstrate out-of-bounds handling
+ print("\nDemonstrating out-of-bounds handling:")
+ out_of_bounds_time: float = duration + 1.0
+ position, velocity, acceleration = traj_func(out_of_bounds_time)
+ print(
+ f"At t={out_of_bounds_time:.2f}s (beyond duration): position={position:.4f}, velocity={velocity:.4f}, acceleration={acceleration:.4f}" # noqa: E501
+ )
diff --git a/interpolatepy/lin_poly_parabolic.py b/interpolatepy/lin_poly_parabolic.py
new file mode 100755
index 0000000..3700c66
--- /dev/null
+++ b/interpolatepy/lin_poly_parabolic.py
@@ -0,0 +1,221 @@
+from collections.abc import Callable # noqa: EXE002
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+class ParabolicBlendTrajectory:
+ """
+ Class to generate trajectories composed of linear segments with parabolic blends at via points.
+
+ The trajectory duration is extended:
+ total_duration = t[-1] - t[0] + (dt_blend[0] + dt_blend[-1]) / 2
+
+ Initial and final velocities are set to zero (q̇0,1 = q̇N,N+1 = 0).
+
+ Parameters
+ ----------
+ q : list | np.ndarray
+ Positions of via points (length N).
+ t : list | np.ndarray
+ Nominal times at which via points are reached (length N).
+ dt_blend : list | np.ndarray
+ Blend durations at via points (length N).
+ dt : float, optional
+ Sampling interval for plotting trajectory. Default is 0.01.
+ """
+
+ def __init__(
+ self,
+ q: list | np.ndarray,
+ t: list | np.ndarray,
+ dt_blend: list | np.ndarray,
+ dt: float = 0.01,
+ ) -> None:
+ self.q = np.asarray(q, dtype=float)
+ self.t = np.asarray(t, dtype=float)
+ self.dt_blend = np.asarray(dt_blend, dtype=float)
+ self.dt = dt
+
+ if not (len(self.q) == len(self.t) == len(self.dt_blend)):
+ raise ValueError("Lengths of q, t, and dt_blend must match.")
+
+ def generate(self) -> tuple[Callable[[float], tuple[float, float, float]], float]:
+ """
+ Generate the parabolic blend trajectory function.
+
+ Returns
+ -------
+ trajectory_function : callable
+ Function that takes a time value and returns position, velocity, and acceleration.
+ total_duration : float
+ The total duration of the trajectory.
+ """
+ # Use lowercase for count variable per style
+ n = len(self.q)
+
+ # Vectorized computation of segment velocities with zero initial and final
+ v_before = np.zeros(n)
+ # Calculate differences in positions and times
+ dq = np.diff(self.q)
+ dt = np.diff(self.t)
+ # Vectorized velocity calculation
+ v_before[1:] = dq / dt
+
+ # Shift velocities for v_after
+ v_after = np.zeros(n)
+ v_after[:-1] = v_before[1:]
+
+ # Accelerations for parabolic blends
+ a = (v_after - v_before) / self.dt_blend
+
+ # Preallocate arrays for region data with structure of arrays (SoA)
+ # Estimating 2*n-1 regions (initial blend + n-1 pairs of linear+blend)
+ num_regions = 2 * n - 1
+ reg_t0 = np.zeros(num_regions)
+ reg_t1 = np.zeros(num_regions)
+ reg_q0 = np.zeros(num_regions)
+ reg_v0 = np.zeros(num_regions)
+ reg_a = np.zeros(num_regions)
+
+ # Initial blend
+ reg_idx = 0
+ t0 = self.t[0] - self.dt_blend[0] / 2
+ t1 = self.t[0] + self.dt_blend[0] / 2
+ reg_t0[reg_idx] = t0
+ reg_t1[reg_idx] = t1
+ reg_q0[reg_idx] = self.q[0]
+ reg_v0[reg_idx] = v_before[0]
+ reg_a[reg_idx] = a[0]
+ reg_idx += 1
+
+ # Build remaining regions efficiently
+ for k in range(n - 1):
+ # Constant-velocity segment
+ t0_c = reg_t1[reg_idx - 1]
+ t1_c = self.t[k + 1] - self.dt_blend[k + 1] / 2
+
+ # Calculate position at start of constant velocity segment
+ dt0 = t0_c - reg_t0[reg_idx - 1]
+ q0_c = (
+ reg_q0[reg_idx - 1] + reg_v0[reg_idx - 1] * dt0 + 0.5 * reg_a[reg_idx - 1] * dt0**2
+ )
+
+ # Store constant velocity segment
+ reg_t0[reg_idx] = t0_c
+ reg_t1[reg_idx] = t1_c
+ reg_q0[reg_idx] = q0_c
+ reg_v0[reg_idx] = v_after[k]
+ reg_a[reg_idx] = 0.0
+ reg_idx += 1
+
+ # Parabolic blend
+ t0_b = t1_c
+ t1_b = self.t[k + 1] + self.dt_blend[k + 1] / 2
+
+ # Calculate position at start of blend
+ dt0b = t0_b - reg_t0[reg_idx - 1]
+ q0_b = reg_q0[reg_idx - 1] + reg_v0[reg_idx - 1] * dt0b
+
+ # Store parabolic blend
+ reg_t0[reg_idx] = t0_b
+ reg_t1[reg_idx] = t1_b
+ reg_q0[reg_idx] = q0_b
+ reg_v0[reg_idx] = v_before[k + 1]
+ reg_a[reg_idx] = a[k + 1]
+ reg_idx += 1
+
+ # Trim unused regions if we overestimated
+ if reg_idx < num_regions:
+ reg_t0 = reg_t0[:reg_idx]
+ reg_t1 = reg_t1[:reg_idx]
+ reg_q0 = reg_q0[:reg_idx]
+ reg_v0 = reg_v0[:reg_idx]
+ reg_a = reg_a[:reg_idx]
+
+ # Determine overall duration
+ t_start = reg_t0[0]
+ t_end = reg_t1[-1]
+ total_duration = t_end - t_start
+
+ # Prepare binary search for region lookup
+ region_boundaries = np.append(reg_t0[0], reg_t1)
+
+ # Function to evaluate trajectory at any time t
+ def trajectory_function(t: float) -> tuple[float, float, float]:
+ """
+ Evaluate the trajectory at time t.
+
+ Parameters
+ ----------
+ t : float
+ Time at which to evaluate the trajectory
+
+ Returns
+ -------
+ tuple[float, float, float]
+ Tuple containing position, velocity, and acceleration at time t
+ """
+ # Clip time to valid range
+ t = np.clip(t, 0.0, total_duration)
+
+ # Convert to absolute time
+ t_abs = t + t_start
+
+ # Find region using binary search
+ region_idx = np.searchsorted(region_boundaries, t_abs, side="right") - 1
+ region_idx = min(region_idx, len(reg_t0) - 1)
+
+ # Calculate values
+ u = t_abs - reg_t0[region_idx]
+ pos = reg_q0[region_idx] + reg_v0[region_idx] * u + 0.5 * reg_a[region_idx] * u**2
+ vel = reg_v0[region_idx] + reg_a[region_idx] * u
+ acc = reg_a[region_idx]
+
+ return pos, vel, acc
+
+ return trajectory_function, total_duration
+
+ def plot(
+ self,
+ times: np.ndarray | None = None,
+ pos: np.ndarray | None = None,
+ vel: np.ndarray | None = None,
+ acc: np.ndarray | None = None,
+ ) -> None:
+ """
+ Plot the trajectory's position, velocity, and acceleration.
+
+ If trajectory data is not provided, it will be generated.
+
+ Parameters
+ ----------
+ times : ndarray, optional
+ Time samples; if None, generated from trajectory function.
+ pos : ndarray, optional
+ Position samples; if None, generated from trajectory function.
+ vel : ndarray, optional
+ Velocity samples; if None, generated from trajectory function.
+ acc : ndarray, optional
+ Acceleration samples; if None, generated from trajectory function.
+ """
+ if times is None or pos is None or vel is None or acc is None:
+ traj_func, total_duration = self.generate()
+ times = np.arange(0.0, total_duration + self.dt, self.dt)
+ pos = np.zeros_like(times)
+ vel = np.zeros_like(times)
+ acc = np.zeros_like(times)
+
+ for i, t in enumerate(times):
+ pos[i], vel[i], acc[i] = traj_func(t)
+
+ fig, (ax1, ax2, ax3) = plt.subplots(3, 1, sharex=True)
+ ax1.plot(times, pos)
+ ax1.set_ylabel("Position")
+ ax2.plot(times, vel)
+ ax2.set_ylabel("Velocity")
+ ax3.plot(times, acc)
+ ax3.set_ylabel("Acceleration")
+ ax3.set_xlabel("Time")
+ fig.tight_layout()
+ plt.show()
diff --git a/interpolatepy/version.py b/interpolatepy/version.py
index ebd1e9d..3f11d31 100644
--- a/interpolatepy/version.py
+++ b/interpolatepy/version.py
@@ -2,4 +2,4 @@
__version__ file.
"""
-__version__ = "1.0.3"
+__version__ = "1.1.0"
diff --git a/pyproject.toml b/pyproject.toml
index 07744c2..ace08ec 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -12,7 +12,7 @@ description = "A comprehensive Python library for generating smooth trajectories
readme = "README.md"
license = "MIT"
license-files = ["LICENSE"]
-requires-python = ">=3.11"
+requires-python = ">=3.10"
keywords = [
"interpolation",
"trajectory planning",
@@ -30,6 +30,7 @@ classifiers = [
"Intended Audience :: Developers",
"Intended Audience :: Education",
"Programming Language :: Python :: 3",
+ "Programming Language :: Python :: 3.10",
"Programming Language :: Python :: 3.11",
"Programming Language :: Python :: 3.12",
"Operating System :: Microsoft :: Windows",
@@ -50,8 +51,6 @@ dynamic = ["version"]
[project.urls]
"Homepage" = "https://github.com/GiorgioMedico/InterpolatePy"
"Bug Tracker" = "https://github.com/GiorgioMedico/InterpolatePy/issues"
-"Repository" = "https://github.com/GiorgioMedico/InterpolatePy.git"
-"Documentation" = "https://github.com/GiorgioMedico/InterpolatePy#readme"
[project.optional-dependencies]
test = [
@@ -86,7 +85,7 @@ testpaths = "tests"
######## Tools
[tool.black]
-target-version = ['py311']
+target-version = ['py310']
line-length = 100
skip-string-normalization = false
skip-magic-trailing-comma = false
@@ -98,7 +97,7 @@ force-exclude = '''
'''
[tool.isort]
-py_version = 311
+py_version = 310
sections = ["FUTURE", "STDLIB", "THIRDPARTY", "FIRSTPARTY", "LOCALFOLDER"]
default_section = "FIRSTPARTY"
known_third_party = [
@@ -139,7 +138,7 @@ skip_glob = ["docs/*", "setup.py"]
filter_files = true
[tool.ruff]
-target-version = "py311"
+target-version = "py310"
line-length = 100
extend-exclude = ["docs", "test", "tests"]
@@ -278,7 +277,7 @@ line-ending = "auto"
[tool.mypy]
# Platform configuration
-python_version = "3.11"
+python_version = "3.10"
# imports related
ignore_missing_imports = true
follow_imports = "silent"
@@ -318,7 +317,7 @@ show_error_codes = true
exclude = ["docs"]
[tool.pyright]
-pythonVersion = "3.11"
+pythonVersion = "3.10"
typeCheckingMode = "basic"
# enable subset of "strict"
reportDuplicateImport = true
diff --git a/requirements-dev.txt b/requirements-dev.txt
index 42cfc31..5d7a2fd 100644
--- a/requirements-dev.txt
+++ b/requirements-dev.txt
@@ -17,3 +17,23 @@ pyright>=1.1.335
pytest>=7.4.3
pytest-benchmark>=4.0.0
lark>=0.0.0
+
+# Documentation
+mkdocs>=1.5.3
+mkdocstrings>=0.23.0
+mkdocstrings-python>=1.7.3
+mkdocs-material>=9.4.8
+Pygments>=2.16.1
+
+# Resolve existing dependency-conflicts
+jinja2>=3.0,<4.0
+markupsafe>=2.0.1
+typeguard>=2.0.0
+colorama~=0.4
+pytz>=2020.1
+lxml>=4.9.0
+beautifulsoup4>=4.12.2
+webencodings>=0.5.1
+psutil>=5.9.5
+decorator>=5.1.1
+pexpect>=4.8.0