Trajectory Planning User Guide

This guide covers the trajectory planning capabilities in ManipulaPy, including joint-space and Cartesian-space trajectory generation, dynamics integration, and collision avoidance using CUDA acceleration.

Introduction

The TrajectoryPlanning class provides comprehensive trajectory generation and execution capabilities for robotic manipulators. It combines kinematic trajectory planning with dynamic analysis and collision avoidance to generate feasible, smooth robot motions.

Key Features:

• Joint-space trajectory generation with cubic/quintic time scaling

• Cartesian-space trajectory planning for end-effector paths

• CUDA-accelerated computations for real-time performance

• Integrated dynamics analysis (forward/inverse dynamics)

• Collision detection and avoidance using potential fields

• Support for various trajectory optimization objectives

Basic Usage

Setting Up Trajectory Planning

import numpy as np
from ManipulaPy.path_planning import TrajectoryPlanning
from ManipulaPy.urdf_processor import URDFToSerialManipulator

# Load robot model
processor = URDFToSerialManipulator("robot.urdf")
robot = processor.serial_manipulator
dynamics = processor.dynamics

# Define joint and torque limits
joint_limits = [(-np.pi, np.pi)] * 6  # 6-DOF robot
torque_limits = [(-50, 50)] * 6       # ±50 N⋅m per joint

# Create trajectory planner
planner = TrajectoryPlanning(
    serial_manipulator=robot,
    urdf_path="robot.urdf",
    dynamics=dynamics,
    joint_limits=joint_limits,
    torque_limits=torque_limits
)

print("Trajectory planner initialized successfully")

Simple Joint Trajectory

import numpy as np

# Define start and end configurations
theta_start = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
theta_end = np.array([0.5, 0.3, -0.2, 0.1, 0.4, -0.1])

# Trajectory parameters
Tf = 3.0      # Duration: 3 seconds
N = 100       # Number of points
method = 3    # Cubic time scaling

# Generate trajectory
trajectory = planner.joint_trajectory(theta_start, theta_end, Tf, N, method)

print(f"Generated trajectory with {N} points")
print(f"Position shape: {trajectory['positions'].shape}")
print(f"Velocity shape: {trajectory['velocities'].shape}")
print(f"Acceleration shape: {trajectory['accelerations'].shape}")

# Verify start and end points
np.testing.assert_allclose(trajectory['positions'][0], theta_start, rtol=1e-3)
np.testing.assert_allclose(trajectory['positions'][-1], theta_end, rtol=1e-3)

TrajectoryPlanning Class

Class Constructor

TrajectoryPlanning(serial_manipulator, urdf_path, dynamics, joint_limits, torque_limits=None)

Parameters:

• serial_manipulator: SerialManipulator instance for kinematics

• urdf_path: Path to robot URDF file for collision checking

• dynamics: ManipulatorDynamics instance for dynamics computations

• joint_limits: List of (min, max) tuples for each joint

• torque_limits: Optional list of (min, max) torque limits

Attributes:

• serial_manipulator: Robot kinematics model

• dynamics: Robot dynamics model

• joint_limits: Joint position constraints

• torque_limits: Joint torque constraints

• collision_checker: Collision detection system

• potential_field: Potential field for obstacle avoidance

Core Methods

joint_trajectory()

Generates smooth joint-space trajectories with CUDA acceleration:

import numpy as np

def joint_trajectory_example():
    """Demonstrate joint trajectory generation options."""

    # Setup
    theta_start = np.zeros(6)
    theta_end = np.array([0.8, -0.5, 0.3, -0.2, 0.6, -0.4])

    # Method 1: Cubic time scaling (smooth velocity)
    traj_cubic = planner.joint_trajectory(
        theta_start, theta_end, Tf=2.0, N=50, method=3
    )

    # Method 2: Quintic time scaling (smooth acceleration)
    traj_quintic = planner.joint_trajectory(
        theta_start, theta_end, Tf=2.0, N=50, method=5
    )

    # Compare velocity profiles
    import matplotlib.pyplot as plt

    time_steps = np.linspace(0, 2.0, 50)

    plt.figure(figsize=(12, 4))

    plt.subplot(1, 2, 1)
    plt.plot(time_steps, traj_cubic['velocities'][:, 0], 'b-', label='Cubic')
    plt.plot(time_steps, traj_quintic['velocities'][:, 0], 'r-', label='Quintic')
    plt.title('Joint 1 Velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity (rad/s)')
    plt.legend()
    plt.grid(True)

    plt.subplot(1, 2, 2)
    plt.plot(time_steps, traj_cubic['accelerations'][:, 0], 'b-', label='Cubic')
    plt.plot(time_steps, traj_quintic['accelerations'][:, 0], 'r-', label='Quintic')
    plt.title('Joint 1 Acceleration')
    plt.xlabel('Time (s)')
    plt.ylabel('Acceleration (rad/s²)')
    plt.legend()
    plt.grid(True)

    plt.tight_layout()
    plt.show()

    return traj_cubic, traj_quintic

# Generate and compare trajectories
cubic_traj, quintic_traj = joint_trajectory_example()

cartesian_trajectory()

Generates Cartesian-space trajectories for end-effector motion:

import numpy as np
import matplotlib.pyplot as plt

def cartesian_trajectory_example():
    """Demonstrate Cartesian trajectory generation."""

    # Define start and end poses
    X_start = np.eye(4)
    X_start[:3, 3] = [0.3, 0.2, 0.5]  # Start position

    X_end = np.eye(4)
    X_end[:3, 3] = [0.5, -0.1, 0.4]   # End position
    # Add rotation (45° about Z-axis)
    angle = np.pi/4
    X_end[:3, :3] = np.array([
        [np.cos(angle), -np.sin(angle), 0],
        [np.sin(angle),  np.cos(angle), 0],
        [0,              0,             1]
    ])

    # Generate Cartesian trajectory
    cart_traj = planner.cartesian_trajectory(
        X_start, X_end, Tf=3.0, N=75, method=5
    )

    print("Cartesian trajectory generated:")
    print(f"- Positions: {cart_traj['positions'].shape}")
    print(f"- Velocities: {cart_traj['velocities'].shape}")
    print(f"- Accelerations: {cart_traj['accelerations'].shape}")
    print(f"- Orientations: {cart_traj['orientations'].shape}")

    # Visualize path
    positions = cart_traj['positions']

    plt.figure(figsize=(10, 8))

    # 3D path
    ax = plt.subplot(2, 2, 1, projection='3d')
    ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], 'b-', linewidth=2)
    ax.scatter(positions[0, 0], positions[0, 1], positions[0, 2],
              c='green', s=100, label='Start')
    ax.scatter(positions[-1, 0], positions[-1, 1], positions[-1, 2],
              c='red', s=100, label='End')
    ax.set_xlabel('X (m)')
    ax.set_ylabel('Y (m)')
    ax.set_zlabel('Z (m)')
    ax.set_title('3D Path')
    ax.legend()

    # X-Y projection
    plt.subplot(2, 2, 2)
    plt.plot(positions[:, 0], positions[:, 1], 'b-', linewidth=2)
    plt.scatter(positions[0, 0], positions[0, 1], c='green', s=100)
    plt.scatter(positions[-1, 0], positions[-1, 1], c='red', s=100)
    plt.xlabel('X (m)')
    plt.ylabel('Y (m)')
    plt.title('X-Y Projection')
    plt.grid(True)
    plt.axis('equal')

    # Velocity profile
    time_steps = np.linspace(0, 3.0, 75)
    velocities = cart_traj['velocities']
    velocity_magnitude = np.linalg.norm(velocities, axis=1)

    plt.subplot(2, 2, 3)
    plt.plot(time_steps, velocity_magnitude, 'r-', linewidth=2)
    plt.xlabel('Time (s)')
    plt.ylabel('Speed (m/s)')
    plt.title('End-Effector Speed')
    plt.grid(True)

    # Acceleration profile
    accelerations = cart_traj['accelerations']
    acceleration_magnitude = np.linalg.norm(accelerations, axis=1)

    plt.subplot(2, 2, 4)
    plt.plot(time_steps, acceleration_magnitude, 'g-', linewidth=2)
    plt.xlabel('Time (s)')
    plt.ylabel('Acceleration (m/s²)')
    plt.title('End-Effector Acceleration')
    plt.grid(True)

    plt.tight_layout()
    plt.show()

    return cart_traj

# Generate Cartesian trajectory
cartesian_traj = cartesian_trajectory_example()

Dynamics Integration

inverse_dynamics_trajectory()

Computes required joint torques along a trajectory:

import numpy as np
import matplotlib.pyplot as plt

def dynamics_analysis_example():
    """Analyze dynamics along a trajectory."""

    # Generate joint trajectory
    theta_start = np.zeros(6)
    theta_end = np.array([0.5, 0.3, -0.2, 0.1, 0.4, -0.1])

    trajectory = planner.joint_trajectory(
        theta_start, theta_end, Tf=2.0, N=50, method=5
    )

    # Compute required torques
    torques = planner.inverse_dynamics_trajectory(
        trajectory['positions'],
        trajectory['velocities'],
        trajectory['accelerations'],
        gravity_vector=[0, 0, -9.81],
        Ftip=[0, 0, 0, 0, 0, 0]  # No external forces
    )

    print(f"Torque trajectory shape: {torques.shape}")

    # Analyze torque requirements
    time_steps = np.linspace(0, 2.0, 50)

    plt.figure(figsize=(15, 10))

    # Plot joint torques
    for i in range(6):
        plt.subplot(2, 3, i+1)
        plt.plot(time_steps, torques[:, i], 'b-', linewidth=2)
        plt.axhline(y=planner.torque_limits[i][1], color='r', linestyle='--',
                   label=f'Limit: ±{planner.torque_limits[i][1]} N⋅m')
        plt.axhline(y=planner.torque_limits[i][0], color='r', linestyle='--')
        plt.xlabel('Time (s)')
        plt.ylabel('Torque (N⋅m)')
        plt.title(f'Joint {i+1} Torque')
        plt.grid(True)
        plt.legend()

    plt.tight_layout()
    plt.show()

    # Check if torques exceed limits
    max_torques = np.max(np.abs(torques), axis=0)
    torque_limits_array = np.array([limit[1] for limit in planner.torque_limits])

    safety_factors = max_torques / torque_limits_array

    print("\nTorque Analysis:")
    for i, (max_torque, limit, safety) in enumerate(zip(max_torques, torque_limits_array, safety_factors)):
        status = "⚠️ EXCEEDED" if safety > 1.0 else "✓ OK"
        print(f"Joint {i+1}: Max {max_torque:.1f} N⋅m / Limit {limit:.1f} N⋅m ({safety:.1%}) {status}")

    return torques

# Analyze dynamics
trajectory_torques = dynamics_analysis_example()

forward_dynamics_trajectory()

Simulates robot motion given applied torques:

import numpy as np
import matplotlib.pyplot as plt

def forward_dynamics_simulation():
    """Simulate robot motion using forward dynamics."""

    # Initial conditions
    theta_initial = np.array([0.1, 0.2, -0.1, 0.0, 0.3, 0.0])
    theta_dot_initial = np.zeros(6)

    # Define control torques (simple step input)
    N_steps = 100
    dt = 0.01

    tau_matrix = np.zeros((N_steps, 6))
    tau_matrix[:, 0] = 5.0   # 5 N⋅m on joint 1
    tau_matrix[:, 2] = -3.0  # -3 N⋅m on joint 3

    # External forces (none)
    Ftip_matrix = np.zeros((N_steps, 6))

    # Simulate forward dynamics
    sim_result = planner.forward_dynamics_trajectory(
        thetalist=theta_initial,
        dthetalist=theta_dot_initial,
        taumat=tau_matrix,
        g=[0, 0, -9.81],
        Ftipmat=Ftip_matrix,
        dt=dt,
        intRes=1
    )

    print("Forward dynamics simulation completed:")
    print(f"- Position trajectory: {sim_result['positions'].shape}")
    print(f"- Velocity trajectory: {sim_result['velocities'].shape}")
    print(f"- Acceleration trajectory: {sim_result['accelerations'].shape}")

    # Plot results
    time_steps = np.arange(N_steps) * dt

    plt.figure(figsize=(15, 8))

    # Joint positions
    plt.subplot(2, 3, 1)
    for i in range(6):
        plt.plot(time_steps, np.degrees(sim_result['positions'][:, i]),
                label=f'Joint {i+1}')
    plt.xlabel('Time (s)')
    plt.ylabel('Position (degrees)')
    plt.title('Joint Positions')
    plt.legend()
    plt.grid(True)

    # Joint velocities
    plt.subplot(2, 3, 2)
    for i in range(6):
        plt.plot(time_steps, sim_result['velocities'][:, i],
                label=f'Joint {i+1}')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity (rad/s)')
    plt.title('Joint Velocities')
    plt.legend()
    plt.grid(True)

    # Applied torques
    plt.subplot(2, 3, 3)
    for i in range(6):
        plt.plot(time_steps, tau_matrix[:, i], label=f'Joint {i+1}')
    plt.xlabel('Time (s)')
    plt.ylabel('Torque (N⋅m)')
    plt.title('Applied Torques')
    plt.legend()
    plt.grid(True)

    # End-effector trajectory
    ee_positions = []
    for pos in sim_result['positions']:
        T = planner.serial_manipulator.forward_kinematics(pos)
        ee_positions.append(T[:3, 3])
    ee_positions = np.array(ee_positions)

    ax = plt.subplot(2, 3, 4, projection='3d')
    ax.plot(ee_positions[:, 0], ee_positions[:, 1], ee_positions[:, 2], 'b-', linewidth=2)
    ax.set_xlabel('X (m)')
    ax.set_ylabel('Y (m)')
    ax.set_zlabel('Z (m)')
    ax.set_title('End-Effector Path')

    # Energy analysis
    kinetic_energies = []
    for i, (pos, vel) in enumerate(zip(sim_result['positions'], sim_result['velocities'])):
        M = planner.dynamics.mass_matrix(pos)
        kinetic_energy = 0.5 * vel.T @ M @ vel
        kinetic_energies.append(kinetic_energy)

    plt.subplot(2, 3, 5)
    plt.plot(time_steps, kinetic_energies, 'r-', linewidth=2)
    plt.xlabel('Time (s)')
    plt.ylabel('Kinetic Energy (J)')
    plt.title('System Kinetic Energy')
    plt.grid(True)

    # Phase plot (position vs velocity for joint 1)
    plt.subplot(2, 3, 6)
    plt.plot(np.degrees(sim_result['positions'][:, 0]),
            sim_result['velocities'][:, 0], 'g-', linewidth=2)
    plt.xlabel('Joint 1 Position (degrees)')
    plt.ylabel('Joint 1 Velocity (rad/s)')
    plt.title('Phase Plot (Joint 1)')
    plt.grid(True)

    plt.tight_layout()
    plt.show()

    return sim_result

# Run forward dynamics simulation
simulation_result = forward_dynamics_simulation()

Trajectory Visualization

plot_trajectory()

Static plotting of trajectory data:

import numpy as np

def trajectory_visualization_example():
    """Comprehensive trajectory visualization."""

    # Generate sample trajectory
    theta_start = np.array([0.0, 0.5, -0.3, 0.0, 0.2, 0.0])
    theta_end = np.array([0.8, -0.2, 0.4, -0.5, 0.6, -0.3])

    trajectory = planner.joint_trajectory(
        theta_start, theta_end, Tf=3.0, N=100, method=5
    )

    # Use built-in plotting method
    TrajectoryPlanning.plot_trajectory(
        trajectory,
        Tf=3.0,
        title="6-DOF Robot Joint Trajectory",
        labels=[f"Joint {i+1}" for i in range(6)]
    )

    return trajectory

# Visualize trajectory
sample_trajectory = trajectory_visualization_example()

plot_cartesian_trajectory()

Visualization for Cartesian trajectories:

import numpy as np

def cartesian_visualization_example():
    """Visualize Cartesian trajectory."""

    # Generate Cartesian trajectory
    X_start = np.eye(4)
    X_start[:3, 3] = [0.4, 0.3, 0.5]

    X_end = np.eye(4)
    X_end[:3, 3] = [0.6, -0.2, 0.3]

    cart_traj = planner.cartesian_trajectory(
        X_start, X_end, Tf=2.5, N=80, method=3
    )

    # Use built-in Cartesian plotting
    planner.plot_cartesian_trajectory(
        cart_traj,
        Tf=2.5,
        title="End-Effector Cartesian Trajectory"
    )

    return cart_traj

# Visualize Cartesian trajectory
cartesian_viz = cartesian_visualization_example()

Advanced Features

Collision Avoidance

The trajectory planner includes collision detection and avoidance:

import numpy as np

def collision_avoidance_example():
    """Demonstrate collision avoidance in trajectory planning."""

    # Generate trajectory that might have collisions
    theta_start = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
    theta_end = np.array([np.pi/2, np.pi/3, -np.pi/4, 0.0, np.pi/6, 0.0])

    trajectory = planner.joint_trajectory(
        theta_start, theta_end, Tf=3.0, N=150, method=5
    )

    print("Trajectory generated with collision avoidance:")
    print(f"- Points: {trajectory['positions'].shape[0]}")
    print(f"- Collision checks: Integrated via potential fields")

    # The trajectory planner automatically applies potential field
    # modifications to avoid collisions during generation

    # Analyze trajectory smoothness
    positions = trajectory['positions']
    velocities = trajectory['velocities']
    accelerations = trajectory['accelerations']

    # Compute smoothness metrics
    velocity_changes = np.diff(velocities, axis=0)
    acceleration_changes = np.diff(accelerations, axis=0)

    smoothness_metric = np.mean(np.linalg.norm(acceleration_changes, axis=1))
    print(f"- Trajectory smoothness metric: {smoothness_metric:.6f}")

    return trajectory

# Generate collision-aware trajectory
safe_trajectory = collision_avoidance_example()

Multi-Point Trajectories

Creating trajectories through multiple waypoints:

import numpy as np
import matplotlib.pyplot as plt

def multi_waypoint_trajectory():
    """Generate trajectory through multiple waypoints."""

    # Define waypoints
    waypoints = [
        np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),           # Start
        np.array([0.3, 0.5, -0.2, 0.1, 0.3, -0.1]),         # Waypoint 1
        np.array([0.6, -0.3, 0.4, -0.2, 0.6, 0.2]),         # Waypoint 2
        np.array([0.8, 0.2, -0.1, 0.3, -0.2, -0.3])         # End
    ]

    # Generate trajectory segments
    segment_duration = 2.0
    points_per_segment = 50

    full_trajectory = {
        'positions': [],
        'velocities': [],
        'accelerations': []
    }

    for i in range(len(waypoints) - 1):
        segment = planner.joint_trajectory(
            waypoints[i], waypoints[i+1],
            Tf=segment_duration, N=points_per_segment, method=5
        )

        # Append to full trajectory (avoid duplicate points)
        if i == 0:
            full_trajectory['positions'].extend(segment['positions'])
            full_trajectory['velocities'].extend(segment['velocities'])
            full_trajectory['accelerations'].extend(segment['accelerations'])
        else:
            # Skip first point to avoid duplication
            full_trajectory['positions'].extend(segment['positions'][1:])
            full_trajectory['velocities'].extend(segment['velocities'][1:])
            full_trajectory['accelerations'].extend(segment['accelerations'][1:])

    # Convert to numpy arrays
    for key in full_trajectory:
        full_trajectory[key] = np.array(full_trajectory[key])

    total_time = segment_duration * (len(waypoints) - 1)
    total_points = full_trajectory['positions'].shape[0]

    print(f"Multi-waypoint trajectory generated:")
    print(f"- Waypoints: {len(waypoints)}")
    print(f"- Total duration: {total_time} seconds")
    print(f"- Total points: {total_points}")

    # Plot the full trajectory
    time_steps = np.linspace(0, total_time, total_points)

    plt.figure(figsize=(15, 5))

    # Joint positions
    plt.subplot(1, 3, 1)
    for i in range(6):
        plt.plot(time_steps, np.degrees(full_trajectory['positions'][:, i]),
                label=f'Joint {i+1}')
    plt.xlabel('Time (s)')
    plt.ylabel('Position (degrees)')
    plt.title('Multi-Waypoint Joint Positions')
    plt.legend()
    plt.grid(True)

    # Mark waypoints
    waypoint_times = [i * segment_duration for i in range(len(waypoints))]
    for wpt_time in waypoint_times:
        plt.axvline(x=wpt_time, color='red', linestyle='--', alpha=0.7)

    # Joint velocities
    plt.subplot(1, 3, 2)
    for i in range(6):
        plt.plot(time_steps, full_trajectory['velocities'][:, i],
                label=f'Joint {i+1}')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity (rad/s)')
    plt.title('Joint Velocities')
    plt.legend()
    plt.grid(True)

    # End-effector path
    ee_positions = []
    for pos in full_trajectory['positions']:
        T = planner.serial_manipulator.forward_kinematics(pos)
        ee_positions.append(T[:3, 3])
    ee_positions = np.array(ee_positions)

    ax = plt.subplot(1, 3, 3, projection='3d')
    ax.plot(ee_positions[:, 0], ee_positions[:, 1], ee_positions[:, 2],
           'b-', linewidth=2, label='Path')

    # Mark waypoint positions
    for i, waypoint in enumerate(waypoints):
        T = planner.serial_manipulator.forward_kinematics(waypoint)
        pos = T[:3, 3]
        ax.scatter(pos[0], pos[1], pos[2], c='red', s=100,
                  label=f'Waypoint {i+1}' if i == 0 else "")

    ax.set_xlabel('X (m)')
    ax.set_ylabel('Y (m)')
    ax.set_zlabel('Z (m)')
    ax.set_title('End-Effector Path')
    ax.legend()

    plt.tight_layout()
    plt.show()

    return full_trajectory, waypoints

# Generate multi-waypoint trajectory
multi_traj, waypoints = multi_waypoint_trajectory()

Performance Optimization

CUDA Acceleration

The trajectory planner uses CUDA for high-performance computations:

import numpy as np

def performance_comparison():
    """Compare CPU vs CUDA performance for trajectory generation."""

    import time

    # Large trajectory for performance testing
    theta_start = np.zeros(6)
    theta_end = np.array([1.0, -0.8, 0.6, -0.4, 1.2, -0.6])

    N_large = 1000  # Many points for performance test
    Tf = 5.0

    print("Performance Comparison: CPU vs CUDA")
    print("=" * 40)

    # Time the trajectory generation
    start_time = time.time()

    trajectory_cuda = planner.joint_trajectory(
        theta_start, theta_end, Tf, N_large, method=5
    )

    cuda_time = time.time() - start_time

    print(f"CUDA trajectory generation:")
    print(f"- Points: {N_large}")
    print(f"- Time: {cuda_time:.3f} seconds")
    print(f"- Rate: {N_large/cuda_time:.1f} points/second")

    # Memory usage estimation
    memory_per_point = 6 * 4 * 3  # 6 joints * 4 bytes * 3 arrays (pos, vel, acc)
    total_memory = N_large * memory_per_point / 1024 / 1024  # MB

    print(f"- Memory usage: ~{total_memory:.1f} MB")

    # Test dynamics integration performance
    start_time = time.time()

    torques = planner.inverse_dynamics_trajectory(
        trajectory_cuda['positions'],
        trajectory_cuda['velocities'],
        trajectory_cuda['accelerations']
    )

    dynamics_time = time.time() - start_time

    print(f"\nDynamics computation:")
    print(f"- Time: {dynamics_time:.3f} seconds")
    print(f"- Rate: {N_large/dynamics_time:.1f} points/second")

    return trajectory_cuda, cuda_time, dynamics_time

# Run performance comparison
perf_traj, traj_time, dyn_time = performance_comparison()

Batch Processing

Processing multiple trajectories efficiently:

import numpy as np

def batch_trajectory_processing():
    """Process multiple trajectories in batch for efficiency."""

    # Generate multiple start/end configurations
    n_trajectories = 10

    start_configs = []
    end_configs = []

    for i in range(n_trajectories):
        start = np.random.uniform(-0.5, 0.5, 6)
        end = np.random.uniform(-0.8, 0.8, 6)
        start_configs.append(start)
        end_configs.append(end)

    print(f"Batch processing {n_trajectories} trajectories:")

    # Process all trajectories
    trajectories = []
    torque_profiles = []

    start_time = time.time()

    for i, (start, end) in enumerate(zip(start_configs, end_configs)):
        # Generate trajectory
        traj = planner.joint_trajectory(start, end, Tf=2.0, N=50, method=5)

        # Compute dynamics
        torques = planner.inverse_dynamics_trajectory(
            traj['positions'], traj['velocities'], traj['accelerations']
        )

        trajectories.append(traj)
        torque_profiles.append(torques)

        if (i + 1) % 5 == 0:
            print(f"  Processed {i + 1}/{n_trajectories} trajectories")

    total_time = time.time() - start_time

    print(f"Batch processing completed:")
    print(f"- Total time: {total_time:.3f} seconds")
    print(f"- Average per trajectory: {total_time/n_trajectories:.3f} seconds")

    # Analyze batch results
    max_torques = []
    for torques in torque_profiles:
        max_torque = np.max(np.abs(torques))
        max_torques.append(max_torque)

    print(f"\nBatch analysis:")
    print(f"- Average max torque: {np.mean(max_torques):.2f} N⋅m")
    print(f"- Max torque range: {np.min(max_torques):.2f} - {np.max(max_torques):.2f} N⋅m")

    return trajectories, torque_profiles

# Run batch processing
batch_trajs, batch_torques = batch_trajectory_processing()

Real-Time Applications

Trajectory Execution

Real-time trajectory following for robot control:

import numpy as np
import matplotlib.pyplot as plt

def real_time_trajectory_execution():
    """Simulate real-time trajectory execution."""

    # Generate reference trajectory
    theta_start = np.array([0.1, 0.2, -0.1, 0.0, 0.3, 0.0])
    theta_end = np.array([0.8, -0.3, 0.5, -0.2, 0.6, -0.4])

    ref_trajectory = planner.joint_trajectory(
        theta_start, theta_end, Tf=4.0, N=400, method=5  # 100 Hz
    )

    # Simulation parameters
    dt = 0.01  # 100 Hz control rate
    n_steps = ref_trajectory['positions'].shape[0]

    # Control parameters
    Kp = np.diag([100, 80, 60, 40, 30, 20])
    Kd = np.diag([10, 8, 6, 4, 3, 2])

    # Initialize simulation state
    current_pos = theta_start.copy()
    current_vel = np.zeros(6)

    # Storage for results
    actual_positions = []
    actual_velocities = []
    control_torques = []
    tracking_errors = []

    print("Simulating real-time trajectory execution...")

    for i in range(n_steps):
        # Get reference at current time
        ref_pos = ref_trajectory['positions'][i]
        ref_vel = ref_trajectory['velocities'][i]
        ref_acc = ref_trajectory['accelerations'][i]

        # Compute tracking error
        pos_error = ref_pos - current_pos
        vel_error = ref_vel - current_vel

        # PD control with feedforward
        tau_pd = Kp @ pos_error + Kd @ vel_error

        # Feedforward compensation
        tau_ff = planner.dynamics.inverse_dynamics(
            ref_pos, ref_vel, ref_acc, [0, 0, -9.81], np.zeros(6)
        )

        # Total control torque
        tau_total = tau_pd + tau_ff

        # Apply torque limits
        for j in range(6):
            tau_total[j] = np.clip(tau_total[j],
                                 planner.torque_limits[j][0],
                                 planner.torque_limits[j][1])

        # Simulate robot dynamics
        acceleration = planner.dynamics.forward_dynamics(
            current_pos, current_vel, tau_total, [0, 0, -9.81], np.zeros(6)
        )

        # Integrate (simple Euler integration)
        current_vel += acceleration * dt
        current_pos += current_vel * dt

        # Apply joint limits
        for j in range(6):
            if current_pos[j] < planner.joint_limits[j][0]:
                current_pos[j] = planner.joint_limits[j][0]
                current_vel[j] = 0
            elif current_pos[j] > planner.joint_limits[j][1]:
                current_pos[j] = planner.joint_limits[j][1]
                current_vel[j] = 0

        # Store results
        actual_positions.append(current_pos.copy())
        actual_velocities.append(current_vel.copy())
        control_torques.append(tau_total.copy())
        tracking_errors.append(np.linalg.norm(pos_error))

    # Convert to arrays
    actual_positions = np.array(actual_positions)
    actual_velocities = np.array(actual_velocities)
    control_torques = np.array(control_torques)
    tracking_errors = np.array(tracking_errors)

    # Analysis
    time_steps = np.arange(n_steps) * dt

    print("Trajectory execution completed:")
    print(f"- Duration: {time_steps[-1]:.1f} seconds")
    print(f"- Final tracking error: {tracking_errors[-1]:.4f} rad")
    print(f"- RMS tracking error: {np.sqrt(np.mean(tracking_errors**2)):.4f} rad")
    print(f"- Max tracking error: {np.max(tracking_errors):.4f} rad")

    # Plot results
    plt.figure(figsize=(15, 12))

    # Position tracking
    plt.subplot(3, 2, 1)
    for i in range(6):
        plt.plot(time_steps, np.degrees(ref_trajectory['positions'][:, i]),
                '--', alpha=0.7, label=f'Ref Joint {i+1}')
        plt.plot(time_steps, np.degrees(actual_positions[:, i]),
                '-', linewidth=2, label=f'Act Joint {i+1}')
    plt.xlabel('Time (s)')
    plt.ylabel('Position (degrees)')
    plt.title('Position Tracking')
    plt.legend()
    plt.grid(True)

    # Velocity tracking
    plt.subplot(3, 2, 2)
    for i in range(6):
        plt.plot(time_steps, ref_trajectory['velocities'][:, i],
                '--', alpha=0.7, label=f'Ref Joint {i+1}')
        plt.plot(time_steps, actual_velocities[:, i],
                '-', linewidth=2, label=f'Act Joint {i+1}')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity (rad/s)')
    plt.title('Velocity Tracking')
    plt.legend()
    plt.grid(True)

    # Control torques
    plt.subplot(3, 2, 3)
    for i in range(6):
        plt.plot(time_steps, control_torques[:, i], label=f'Joint {i+1}')
    plt.xlabel('Time (s)')
    plt.ylabel('Torque (N⋅m)')
    plt.title('Control Torques')
    plt.legend()
    plt.grid(True)

    # Tracking error
    plt.subplot(3, 2, 4)
    plt.plot(time_steps, np.degrees(tracking_errors), 'r-', linewidth=2)
    plt.xlabel('Time (s)')
    plt.ylabel('Tracking Error (degrees)')
    plt.title('Position Tracking Error')
    plt.grid(True)

    # End-effector tracking
    ref_ee_positions = []
    actual_ee_positions = []

    for ref_pos, act_pos in zip(ref_trajectory['positions'], actual_positions):
        T_ref = planner.serial_manipulator.forward_kinematics(ref_pos)
        T_act = planner.serial_manipulator.forward_kinematics(act_pos)
        ref_ee_positions.append(T_ref[:3, 3])
        actual_ee_positions.append(T_act[:3, 3])

    ref_ee_positions = np.array(ref_ee_positions)
    actual_ee_positions = np.array(actual_ee_positions)

    ax = plt.subplot(3, 2, 5, projection='3d')
    ax.plot(ref_ee_positions[:, 0], ref_ee_positions[:, 1], ref_ee_positions[:, 2],
           'b--', alpha=0.7, linewidth=2, label='Reference')
    ax.plot(actual_ee_positions[:, 0], actual_ee_positions[:, 1], actual_ee_positions[:, 2],
           'r-', linewidth=2, label='Actual')
    ax.set_xlabel('X (m)')
    ax.set_ylabel('Y (m)')
    ax.set_zlabel('Z (m)')
    ax.set_title('End-Effector Tracking')
    ax.legend()

    # Control effort
    plt.subplot(3, 2, 6)
    control_effort = np.linalg.norm(control_torques, axis=1)
    plt.plot(time_steps, control_effort, 'g-', linewidth=2)
    plt.xlabel('Time (s)')
    plt.ylabel('Total Control Effort (N⋅m)')
    plt.title('Control Effort')
    plt.grid(True)

    plt.tight_layout()
    plt.show()

    return {
        'reference': ref_trajectory,
        'actual_positions': actual_positions,
        'actual_velocities': actual_velocities,
        'control_torques': control_torques,
        'tracking_errors': tracking_errors
    }

# Run real-time simulation
execution_results = real_time_trajectory_execution()

Practical Applications

Pick and Place Operation

Complete pick-and-place trajectory planning:

import numpy as np
import matplotlib.pyplot as plt

def pick_and_place_trajectory():
    """Generate trajectory for pick-and-place operation."""

    # Define task waypoints
    home_joints = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

    # Approach position (above object)
    approach_pos = np.array([0.3, 0.2, 0.4])
    approach_joints = planner.serial_manipulator.iterative_inverse_kinematics(
        np.array([[1, 0, 0, approach_pos[0]],
                  [0, 1, 0, approach_pos[1]],
                  [0, 0, 1, approach_pos[2]],
                  [0, 0, 0, 1]]),
        home_joints
    )[0]

    # Pick position (at object)
    pick_pos = approach_pos - np.array([0, 0, 0.1])
    pick_joints = planner.serial_manipulator.iterative_inverse_kinematics(
        np.array([[1, 0, 0, pick_pos[0]],
                  [0, 1, 0, pick_pos[1]],
                  [0, 0, 1, pick_pos[2]],
                  [0, 0, 0, 1]]),
        approach_joints
    )[0]

    # Place position
    place_pos = np.array([0.5, -0.1, 0.3])
    place_joints = planner.serial_manipulator.iterative_inverse_kinematics(
        np.array([[1, 0, 0, place_pos[0]],
                  [0, 1, 0, place_pos[1]],
                  [0, 0, 1, place_pos[2]],
                  [0, 0, 0, 1]]),
        pick_joints
    )[0]

    # Define trajectory segments
    segments = [
        ("Move to approach", home_joints, approach_joints, 2.0),
        ("Approach object", approach_joints, pick_joints, 1.0),
        ("Pick up", pick_joints, approach_joints, 1.0),  # Lift
        ("Move to place", approach_joints, place_joints, 3.0),
        ("Place object", place_joints, pick_joints, 1.0),  # Lower
        ("Return home", pick_joints, home_joints, 2.0)
    ]

    # Generate complete trajectory
    complete_trajectory = {
        'positions': [],
        'velocities': [],
        'accelerations': [],
        'segments': []
    }

    print("Generating pick-and-place trajectory:")

    for i, (name, start, end, duration) in enumerate(segments):
        print(f"  {i+1}. {name} ({duration}s)")

        # Generate segment
        segment = planner.joint_trajectory(
            start, end, Tf=duration, N=int(duration*50), method=5  # 50 Hz
        )

        # Add to complete trajectory
        if i == 0:
            complete_trajectory['positions'].extend(segment['positions'])
            complete_trajectory['velocities'].extend(segment['velocities'])
            complete_trajectory['accelerations'].extend(segment['accelerations'])
        else:
            # Skip first point to avoid duplication
            complete_trajectory['positions'].extend(segment['positions'][1:])
            complete_trajectory['velocities'].extend(segment['velocities'][1:])
            complete_trajectory['accelerations'].extend(segment['accelerations'][1:])

        complete_trajectory['segments'].append({
            'name': name,
            'start_index': len(complete_trajectory['positions']) - len(segment['positions']),
            'end_index': len(complete_trajectory['positions']) - 1,
            'duration': duration
        })

    # Convert to arrays
    for key in ['positions', 'velocities', 'accelerations']:
        complete_trajectory[key] = np.array(complete_trajectory[key])

    total_duration = sum(seg[3] for seg in segments)
    total_points = complete_trajectory['positions'].shape[0]

    print(f"\nTrajectory generated:")
    print(f"- Total duration: {total_duration} seconds")
    print(f"- Total points: {total_points}")

    # Compute dynamics for entire trajectory
    torques = planner.inverse_dynamics_trajectory(
        complete_trajectory['positions'],
        complete_trajectory['velocities'],
        complete_trajectory['accelerations']
    )

    # Visualize complete operation
    time_steps = np.linspace(0, total_duration, total_points)

    plt.figure(figsize=(15, 10))

    # Joint trajectories with segment markers
    plt.subplot(2, 2, 1)
    for i in range(6):
        plt.plot(time_steps, np.degrees(complete_trajectory['positions'][:, i]),
                label=f'Joint {i+1}')

    # Mark segment boundaries
    current_time = 0
    for segment in segments:
        current_time += segment[3]
        plt.axvline(x=current_time, color='red', linestyle='--', alpha=0.5)

    plt.xlabel('Time (s)')
    plt.ylabel('Position (degrees)')
    plt.title('Pick-and-Place Joint Trajectories')
    plt.legend()
    plt.grid(True)

    # End-effector path
    ee_positions = []
    for pos in complete_trajectory['positions']:
        T = planner.serial_manipulator.forward_kinematics(pos)
        ee_positions.append(T[:3, 3])
    ee_positions = np.array(ee_positions)

    ax = plt.subplot(2, 2, 2, projection='3d')
    ax.plot(ee_positions[:, 0], ee_positions[:, 1], ee_positions[:, 2],
           'b-', linewidth=2, label='End-effector path')

    # Mark key positions
    key_positions = [approach_pos, pick_pos, place_pos]
    key_labels = ['Approach', 'Pick', 'Place']
    colors = ['green', 'red', 'blue']

    for pos, label, color in zip(key_positions, key_labels, colors):
        ax.scatter(pos[0], pos[1], pos[2], c=color, s=100, label=label)

    ax.set_xlabel('X (m)')
    ax.set_ylabel('Y (m)')
    ax.set_zlabel('Z (m)')
    ax.set_title('End-Effector Path')
    ax.legend()

    # Torque requirements
    plt.subplot(2, 2, 3)
    for i in range(6):
        plt.plot(time_steps, torques[:, i], label=f'Joint {i+1}')

    # Mark segment boundaries
    current_time = 0
    for segment in segments:
        current_time += segment[3]
        plt.axvline(x=current_time, color='red', linestyle='--', alpha=0.5)

    plt.xlabel('Time (s)')
    plt.ylabel('Torque (N⋅m)')
    plt.title('Required Torques')
    plt.legend()
    plt.grid(True)

    # Velocity profile
    plt.subplot(2, 2, 4)
    velocity_magnitude = np.linalg.norm(complete_trajectory['velocities'], axis=1)
    plt.plot(time_steps, velocity_magnitude, 'g-', linewidth=2)

    # Mark segment boundaries
    current_time = 0
    for i, segment in enumerate(segments):
        current_time += segment[3]
        plt.axvline(x=current_time, color='red', linestyle='--', alpha=0.5)
        if i < len(segments) - 1:
            plt.text(current_time - segment[3]/2, plt.ylim()[1]*0.8,
                    segment[0], rotation=90, ha='center', fontsize=8)

    plt.xlabel('Time (s)')
    plt.ylabel('Joint Velocity Magnitude (rad/s)')
    plt.title('Velocity Profile')
    plt.grid(True)

    plt.tight_layout()
    plt.show()

    return complete_trajectory, torques

# Generate pick-and-place trajectory
pick_place_traj, pick_place_torques = pick_and_place_trajectory()

Best Practices

Trajectory Design Guidelines

def trajectory_design_guidelines():
    """Best practices for trajectory design."""

    guidelines = {
        "Time Scaling": {
            "description": "Choose appropriate time scaling method",
            "recommendations": [
                "Use cubic (method=3) for smooth velocity profiles",
                "Use quintic (method=5) for smooth acceleration profiles",
                "Quintic is preferred for high-speed operations",
                "Consider jerk constraints for smooth robot motion"
            ]
        },

        "Duration Selection": {
            "description": "Set appropriate trajectory duration",
            "recommendations": [
                "Longer durations reduce peak velocities and accelerations",
                "Consider robot dynamics limits when setting duration",
                "Balance between speed and smoothness requirements",
                "Account for payload and operational constraints"
            ]
        },

        "Sampling Rate": {
            "description": "Choose appropriate number of trajectory points",
            "recommendations": [
                "Use 50-100 Hz for typical robot control",
                "Higher rates for high-speed or precision operations",
                "Consider computational resources for real-time execution",
                "Ensure sufficient resolution for smooth motion"
            ]
        },

        "Joint Limits": {
            "description": "Respect robot physical constraints",
            "recommendations": [
                "Always check joint position limits",
                "Consider velocity and acceleration limits",
                "Include safety margins in limit checking",
                "Use inverse kinematics to verify reachability"
            ]
        },

        "Dynamics Considerations": {
            "description": "Account for robot dynamics",
            "recommendations": [
                "Verify torque requirements don't exceed limits",
                "Consider payload effects on dynamics",
                "Account for gravity compensation needs",
                "Plan for energy-efficient trajectories"
            ]
        }
    }

    print("Trajectory Design Best Practices")
    print("=" * 50)

    for category, info in guidelines.items():
        print(f"\n{category}:")
        print(f"  {info['description']}")
        for rec in info['recommendations']:
            print(f"  • {rec}")

    return guidelines

# Display guidelines
design_guidelines = trajectory_design_guidelines()

Error Handling and Debugging

import numpy as np

def trajectory_debugging_tools():
    """Tools for debugging trajectory planning issues."""

    def validate_trajectory(trajectory):
        """Validate trajectory properties."""

        print("Trajectory Validation:")
        print("-" * 25)

        positions = trajectory['positions']
        velocities = trajectory['velocities']
        accelerations = trajectory['accelerations']

        # Check shapes
        assert positions.shape[0] == velocities.shape[0] == accelerations.shape[0]
        print(f"✓ Consistent trajectory length: {positions.shape[0]} points")

        # Check for NaN or infinite values
        if np.any(~np.isfinite(positions)):
            print("❌ Invalid positions detected")
            return False
        print("✓ All positions are finite")

        if np.any(~np.isfinite(velocities)):
            print("❌ Invalid velocities detected")
            return False
        print("✓ All velocities are finite")

        if np.any(~np.isfinite(accelerations)):
            print("❌ Invalid accelerations detected")
            return False
        print("✓ All accelerations are finite")

        # Check boundary conditions
        start_vel = np.linalg.norm(velocities[0])
        end_vel = np.linalg.norm(velocities[-1])

        if start_vel > 1e-3:
            print(f"⚠️ Non-zero start velocity: {start_vel:.6f}")
        else:
            print("✓ Zero start velocity")

        if end_vel > 1e-3:
            print(f"⚠️ Non-zero end velocity: {end_vel:.6f}")
        else:
            print("✓ Zero end velocity")

        # Check smoothness
        vel_changes = np.diff(velocities, axis=0)
        max_vel_change = np.max(np.linalg.norm(vel_changes, axis=1))
        print(f"✓ Max velocity change: {max_vel_change:.6f} rad/s")

        return True

    def check_dynamics_feasibility(trajectory, planner):
        """Check if trajectory is dynamically feasible."""

        print("\nDynamics Feasibility Check:")
        print("-" * 30)

        try:
            torques = planner.inverse_dynamics_trajectory(
                trajectory['positions'],
                trajectory['velocities'],
                trajectory['accelerations']
            )

            # Check torque limits
            max_torques = np.max(np.abs(torques), axis=0)
            torque_limits = np.array([limit[1] for limit in planner.torque_limits])

            violations = max_torques > torque_limits

            if np.any(violations):
                print("❌ Torque limit violations detected:")
                for i, violation in enumerate(violations):
                    if violation:
                        print(f"   Joint {i+1}: {max_torques[i]:.1f} > {torque_limits[i]:.1f} N⋅m")
                return False
            else:
                print("✓ All torques within limits")
                max_usage = np.max(max_torques / torque_limits)
                print(f"✓ Max torque usage: {max_usage:.1%}")
                return True

        except Exception as e:
            print(f"❌ Dynamics computation failed: {e}")
            return False

    # Example usage
    print("Trajectory Debugging Tools")
    print("=" * 40)

    # Generate test trajectory
    theta_start = np.zeros(6)
    theta_end = np.array([0.5, 0.3, -0.2, 0.1, 0.4, -0.1])

    test_trajectory = planner.joint_trajectory(
        theta_start, theta_end, Tf=2.0, N=50, method=5
    )

    # Run validation
    is_valid = validate_trajectory(test_trajectory)
    is_feasible = check_dynamics_feasibility(test_trajectory, planner)

    overall_status = "✓ PASSED" if (is_valid and is_feasible) else "❌ FAILED"
    print(f"\nOverall Status: {overall_status}")

    return is_valid and is_feasible

# Run debugging tools
debug_result = trajectory_debugging_tools()

Summary

The ManipulaPy Trajectory Planning module provides comprehensive trajectory generation capabilities for robotic manipulators:

Core Features:

• Joint-space trajectories with cubic/quintic time scaling

• Cartesian-space trajectories for end-effector motion

• CUDA acceleration for high-performance computation

• Dynamics integration for torque analysis and simulation

• Collision avoidance using potential field methods

Key Classes and Methods:

• TrajectoryPlanning: Main class for trajectory generation

• joint_trajectory(): Generate smooth joint-space paths

• cartesian_trajectory(): Create end-effector trajectories

• inverse_dynamics_trajectory(): Compute required torques

• forward_dynamics_trajectory(): Simulate robot motion

Advanced Capabilities:

• Multi-waypoint trajectory generation

• Real-time trajectory execution simulation

• Batch processing for multiple trajectories

• Pick-and-place operation planning

• Performance optimization with CUDA

Best Practices:

• Use quintic scaling for smooth acceleration profiles

• Validate trajectories for dynamics feasibility

• Check joint and torque limit compliance

• Consider collision avoidance requirements

• Optimize for computational performance

The trajectory planning module enables users to generate smooth, dynamically feasible robot motions for a wide range of applications from simple point-to-point movements to complex multi-segment operations.