linear polynomials with parabolic blends

GiorgioMedico · GiorgioMedico · commit 6813678cbeea · 2025-05-17T13:20:59.000+02:00
diff --git a/examples/lin_poly_parabolic_ex.py 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
@@ -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
@@ -2,4 +2,4 @@
 __version__ file.
 """
 
-__version__ = "1.0.3"
+__version__ = "1.1.0"
diff --git 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
