linear done

Author: GiorgioMedico

examples/linear_ex.py

@@ -0,0 +1,149 @@
+"""
+Example file demonstrating the use of linear_traj function.
+
+This example shows:
+1. How to generate a linear trajectory between two points
+2. How to use the function with both scalar and vector positions
+3. Visualization of the generated trajectories
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from interpolatepy.linear import linear_traj
+
+
+def scalar_trajectory_example() -> None:
+    """Demonstrate linear_traj with scalar positions."""
+    print("Scalar Trajectory Example")
+    print("--------------------------")
+
+    # Define start and end points
+    p0 = 0.0
+    p1 = 10.0
+
+    # Define time interval
+    t0 = 0.0
+    t1 = 2.0
+
+    # Generate time array
+    num_points = 100
+    time_array = np.linspace(t0, t1, num_points)
+
+    # Calculate trajectory
+    positions, velocities, accelerations = linear_traj(p0, p1, t0, t1, time_array)
+
+    # Print some values
+    print(f"Start position: {positions[0]}")
+    print(f"End position: {positions[-1]}")
+    print(f"Constant velocity: {velocities[0]}")
+    print(f"Acceleration: {accelerations[0]}")
+
+    # Plot results
+    plt.figure(figsize=(12, 8))
+
+    # Position plot
+    plt.subplot(3, 1, 1)
+    plt.plot(time_array, positions, "b-", linewidth=2)
+    plt.grid(True)
+    plt.ylabel("Position")
+    plt.title("Linear Trajectory - Scalar Case")
+
+    # Velocity plot
+    plt.subplot(3, 1, 2)
+    plt.plot(time_array, velocities, "g-", linewidth=2)
+    plt.grid(True)
+    plt.ylabel("Velocity")
+
+    # Acceleration plot
+    plt.subplot(3, 1, 3)
+    plt.plot(time_array, accelerations, "r-", linewidth=2)
+    plt.grid(True)
+    plt.ylabel("Acceleration")
+    plt.xlabel("Time")
+
+    plt.tight_layout()
+    plt.show()
+
+
+def vector_trajectory_example() -> None:
+    """Demonstrate linear_traj with vector positions (2D points)."""
+    print("\nVector Trajectory Example (2D)")
+    print("------------------------------")
+
+    # Define start and end points (2D vectors)
+    p0 = [0.0, 0.0]  # Starting at origin
+    p1 = [10.0, 5.0]  # Ending at (10, 5)
+
+    # Define time interval
+    t0 = 0.0
+    t1 = 3.0
+
+    # Generate time array
+    num_points = 100
+    time_array = np.linspace(t0, t1, num_points)
+
+    # Calculate trajectory
+    positions, velocities, accelerations = linear_traj(p0, p1, t0, t1, time_array)
+
+    # Print some values
+    print(f"Start position: {positions[0]}")
+    print(f"End position: {positions[-1]}")
+    print(f"Constant velocity: {velocities[0]}")
+    print(f"Acceleration: {accelerations[0]}")
+
+    # Extract x and y components
+    x_positions = positions[:, 0]
+    y_positions = positions[:, 1]
+    x_velocities = velocities[:, 0]
+    y_velocities = velocities[:, 1]
+
+    # Plot results
+    plt.figure(figsize=(12, 10))
+
+    # Trajectory in 2D space
+    plt.subplot(3, 1, 1)
+    plt.plot(x_positions, y_positions, "b-", linewidth=2)
+    plt.plot(p0[0], p0[1], "ro", markersize=8, label="Start")
+    plt.plot(p1[0], p1[1], "go", markersize=8, label="End")
+    plt.grid(True)
+    plt.xlabel("X position")
+    plt.ylabel("Y position")
+    plt.title("Linear Trajectory in 2D Space")
+    plt.legend()
+
+    # X and Y positions over time
+    plt.subplot(3, 1, 2)
+    plt.plot(time_array, x_positions, "b-", linewidth=2, label="X position")
+    plt.plot(time_array, y_positions, "g-", linewidth=2, label="Y position")
+    plt.grid(True)
+    plt.ylabel("Position")
+    plt.legend()
+
+    # X and Y velocities over time
+    plt.subplot(3, 1, 3)
+    plt.plot(time_array, x_velocities, "b-", linewidth=2, label="X velocity")
+    plt.plot(time_array, y_velocities, "g-", linewidth=2, label="Y velocity")
+    plt.grid(True)
+    plt.ylabel("Velocity")
+    plt.xlabel("Time")
+    plt.legend()
+
+    plt.tight_layout()
+    plt.show()
+
+
+def main() -> None:
+    """Run both examples."""
+    print("Linear Trajectory Examples")
+    print("=========================\n")
+
+    # Run scalar example
+    scalar_trajectory_example()
+
+    # Run vector example
+    vector_trajectory_example()
+
+
+if __name__ == "__main__":
+    main()

interpolatepy/linear.py

@@ -0,0 +1,107 @@
+import numpy as np
+
+
+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]:
+    """
+    Generate points along a linear trajectory using NumPy vectorization.
+
+    This function computes positions, velocities, and accelerations for points
+    along a linear trajectory between starting position p0 and ending position p1.
+    The trajectory is calculated for each time point in time_array.
+
+    Parameters
+    ----------
+    p0 : float or list[float] or np.ndarray
+        Starting position. Can be a scalar for 1D motion or an array/list for
+        multi-dimensional motion.
+    p1 : float or list[float] or np.ndarray
+        Ending position. Must have the same dimensionality as p0.
+    t0 : float
+        Start time of the trajectory.
+    t1 : float
+        End time of the trajectory.
+    time_array : np.ndarray
+        Array of time points at which to calculate the trajectory.
+
+    Returns
+    -------
+    positions : np.ndarray
+        Array of positions at each time point. For scalar inputs, shape is (len(time_array),).
+        For vector inputs, shape is (len(time_array), dim) where dim is the dimension of p0/p1.
+    velocities : np.ndarray
+        Constant velocity at each time point, with the same shape as positions.
+    accelerations : np.ndarray
+        Zero acceleration at each time point, with the same shape as positions.
+
+    Examples
+    --------
+    Scalar positions (1D motion):
+
+    >>> import numpy as np
+    >>> times = np.linspace(0, 1, 5)
+    >>> pos, vel, acc = linear_traj(0, 1, 0, 1, times)
+    >>> print(pos)
+    [0.   0.25 0.5  0.75 1.  ]
+    >>> print(vel)
+    [1. 1. 1. 1. 1.]
+    >>> print(acc)
+    [0. 0. 0. 0. 0.]
+
+    Vector positions (2D motion):
+
+    >>> import numpy as np
+    >>> times = np.linspace(0, 2, 3)
+    >>> p0 = [0, 0]  # Start at origin
+    >>> p1 = [4, 6]  # End at point (4, 6)
+    >>> pos, vel, acc = linear_traj(p0, p1, 0, 2, times)
+    >>> print(pos)
+    [[0. 0.]
+     [2. 3.]
+     [4. 6.]]
+    >>> print(vel)
+    [[2. 3.]
+     [2. 3.]
+     [2. 3.]]
+
+    Notes
+    -----
+    - This function implements linear interpolation with constant velocity and
+      zero acceleration.
+    - For vector inputs (multi-dimensional motion), proper broadcasting is applied
+      to ensure correct calculation across all dimensions.
+    - The function handles both scalar and vector inputs automatically:
+        * For scalar inputs: outputs have shape (len(time_array),)
+        * For vector inputs: outputs have shape (len(time_array), dim)
+    - Time points outside the range [t0, t1] will still produce valid positions
+      by extrapolating the linear trajectory.
+    - The velocity is always constant and equal to (p1 - p0) / (t1 - t0).
+    - The acceleration is always zero.
+    """
+    # Convert inputs to numpy arrays if they aren't already
+    p0 = np.array(p0)
+    p1 = np.array(p1)
+    time_array = np.array(time_array)
+
+    # Calculate coefficients
+    a0 = p0
+    a1 = (p1 - p0) / (t1 - t0)
+
+    # Handle broadcasting differently based on whether positions are scalar or vector
+    if np.isscalar(p0) or p0.ndim == 0:
+        positions = a0 + a1 * (time_array - t0)
+        velocities = np.ones_like(time_array) * a1
+        accelerations = np.zeros_like(time_array)
+    else:
+        # Vector case - reshape for proper broadcasting
+        time_offset = (time_array - t0).reshape(-1, 1)
+        positions = a0 + a1 * time_offset
+        velocities = np.tile(a1, (len(time_array), 1))
+        accelerations = np.zeros((len(time_array), len(a0)))
+
+    return positions, velocities, accelerations