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 OptimizedTrajectoryPlanning 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 with optional CUDA acceleration.

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

Mathematical Background#

This section summarizes the key mathematical constructs behind joint-space and Cartesian trajectory generation, as well as obstacle avoidance via potential fields.

Joint-Space Time Scaling#

Given start \(\boldsymbol\theta_{0}\) and end \(\boldsymbol\theta_{f}\), total duration \(T\), define a scalar time-scaling function:

\[s = \frac{t}{T}, \quad s \in [0,1]\]

Common choices:

  • Cubic (zero end-velocity):

    \[\sigma_{3}(s) = 3s^{2} - 2s^{3}\]

  • Quintic (zero end-velocity & zero end-acceleration):

    \[\sigma_{5}(s) = 10s^{3} - 15s^{4} + 6s^{5}\]

Joint-space trajectory:

\[\boldsymbol\theta(t) = \boldsymbol\theta_{0} + \bigl(\boldsymbol\theta_{f} - \boldsymbol\theta_{0}\bigr)\,\sigma_{m}(s)\]

with derivatives:

  • Velocity:

    \[\dot{\boldsymbol\theta}(t) = \frac{1}{T}\bigl(\boldsymbol\theta_{f}-\boldsymbol\theta_{0}\bigr)\,\sigma_{m}'(s)\]

  • Acceleration:

    \[\ddot{\boldsymbol\theta}(t) = \frac{1}{T^{2}}\bigl(\boldsymbol\theta_{f}-\boldsymbol\theta_{0}\bigr)\,\sigma_{m}''(s)\]

where \(m=3\) or \(5\) for cubic/quintic time scaling.

Cartesian-Space Interpolation#

Given start/end end-effector poses \(X_{0},X_{f}\in SE(3)\), define the relative transform

\[\Delta X = X_{0}^{-1}X_{f} = \exp\bigl(\,[\Xi]\,\bigr),\]

with twist \(\Xi\in\mathfrak{se}(3)\) given by the matrix logarithm. Then

\[X(t) = X_{0}\,\exp\!\bigl([\Xi]\;\sigma_{m}(s)\bigr).\]

Extract :

  • position: the translational part of \(X(t)\)

  • orientation: the rotational part via Rodrigues’ formula on \([\Xi]\)

Dynamics-Aware Torque Computation#

Once joint positions, velocities and accelerations are known, the required torques along the trajectory follow by inverse dynamics:

\[\boldsymbol\tau(t) = M\bigl(\boldsymbol\theta(t)\bigr)\,\ddot{\boldsymbol\theta}(t) + C\bigl(\boldsymbol\theta(t),\dot{\boldsymbol\theta}(t)\bigr)\,\dot{\boldsymbol\theta}(t) + G\bigl(\boldsymbol\theta(t)\bigr) \;+\; J(\boldsymbol\theta(t))^{T}\,\mathbf F_{\mathrm{tip}}.\]

Potential-Field Collision Avoidance#

Obstacles at positions \(\mathbf p_{i}\) generate repulsive potentials

\[\begin{split}U_{\mathrm{rep}}(\mathbf q) = \sum_{i} \begin{cases} \tfrac12\,\eta\Bigl(\tfrac{1}{\lVert \mathbf p(\mathbf q)-\mathbf p_{i}\rVert} - \tfrac{1}{d_{0}}\Bigr)^{2}, & \lVert \mathbf p(\mathbf q)-\mathbf p_{i}\rVert < d_{0},\\ 0, & \text{otherwise}, \end{cases}\end{split}\]

and an attractive potential toward the goal \(U_{\mathrm{att}}(\mathbf q) =\tfrac12\,\zeta\,\lVert \mathbf p(\mathbf q)-\mathbf p_{f}\rVert^{2}\).

The total artificial potential

\[U(\mathbf q) = U_{\mathrm{att}} + U_{\mathrm{rep}},\]

yields a force in joint space via the Jacobian transpose:

\[\boldsymbol\tau_{\mathrm{obs}} = -J(\mathbf q)^{T}\,\nabla_{\mathbf p}U\bigl(\mathbf p(\mathbf q)\bigr).\]

Trajectory generation incorporates these collision-avoidance torques into an optimization loop to adjust \(\boldsymbol\theta(t)\) so that obstacles are circumvented while preserving smoothness.

Putting It All Together#

  1. Time-scale with \(\sigma_{3}\) or \(\sigma_{5}\) for smooth joint profiles.

  2. Interpolate Cartesian end-effector motion on SE(3).

  3. Compute velocities/accelerations and feed into inverse dynamics for torque evaluation.

  4. Inject obstacle gradients from potential fields to reshape the path.

This mathematical framework underlies all high-level methods in the TrajectoryPlanning class.

Basic Usage#

Setting Up Trajectory Planning#

import numpy as np
from ManipulaPy.path_planning import OptimizedTrajectoryPlanning
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 = OptimizedTrajectoryPlanning(
    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}")
# Note: CPU and GPU paths both enforce joint limits; collision hooks run when a checker is available.
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#

High-Frequency Control Simulation#

Important

Real-time semantics

In ManipulaPy, real-time refers to soft real-time (best-effort) loops: a fixed-rate cycle (e.g., 100–1000 Hz) driven by a monotonic clock where the average period is met and small jitter/missed deadlines are acceptable. Python does not provide hard real-time guarantees. For hard real-time (deterministic deadlines), implement the inner loop on RT hardware/OS and use ManipulaPy for trajectory generation, validation, and supervision.

Control Loop Simulation at Industrial Rates#

This section demonstrates trajectory-following simulation at typical industrial control frequencies (100–1000 Hz). While Python cannot guarantee hard real-time, these examples show how to design trajectories suitable for deployment on dedicated RT controllers.

Control Frequency Considerations#

Soft Real-Time Simulation (Python):

  • Simulated control loops at 100–1000 Hz.

  • Suitable for algorithm validation and controller tuning.

  • No timing guarantees — for development only.

Hard Real-Time Implementation (Target Hardware):

  • Requires a real-time OS (e.g., PREEMPT_RT Linux, Xenomai, VxWorks) or MCU/PLC.

  • Deterministic timing guarantees.

  • Use ManipulaPy for offline trajectory design and export to the RT stack.

Trajectory Execution Simulation (100 Hz)#

The example below simulates a 100 Hz control loop for trajectory tracking. It also includes optional soft RT scheduling to align each iteration with the 10 ms period and report deadline misses.

import time
import numpy as np
import matplotlib.pyplot as plt

def simulate_high_frequency_control(enforce_timing: bool = True):
    """
    Simulate high-frequency trajectory following control (100 Hz).

    Notes:
    - This is a simulation, not hard real-time.
    - Assumes 'planner' is already constructed (robot, dynamics, limits).
    """

    # Generate reference trajectory offline
    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 (dt = 0.01 s)
    )

    # Control loop target
    dt = 0.01  # 100 Hz
    n_steps = ref_trajectory["positions"].shape[0]

    # Simple PD + feedforward gains
    Kp = np.diag([100, 80, 60, 40, 30, 20])
    Kd = np.diag([10,  8,  6,  4,  3,  2])

    # State
    current_pos = theta_start.copy()
    current_vel = np.zeros(6)

    # Logs
    actual_positions  = []
    actual_velocities = []
    control_torques   = []
    tracking_errors   = []
    loop_durations    = []
    deadline_misses   = 0

    if enforce_timing:
        next_deadline = time.perf_counter() + dt

    print("Simulating control at 100 Hz (soft real-time best-effort).")

    for i in range(n_steps):
        loop_start = time.perf_counter()

        # Reference at index i
        ref_pos = ref_trajectory["positions"][i]
        ref_vel = ref_trajectory["velocities"][i]
        ref_acc = ref_trajectory["accelerations"][i]

        # PD + feedforward inverse dynamics
        pos_error = ref_pos - current_pos
        vel_error = ref_vel - current_vel
        tau_pd    = Kp @ pos_error + Kd @ vel_error

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

        tau_total = tau_pd + tau_ff

        # Torque limits
        for j in range(6):
            lo, hi = planner.torque_limits[j]
            tau_total[j] = np.clip(tau_total[j], lo, hi)

        # Forward dynamics + Euler integrate
        acc = planner.dynamics.forward_dynamics(
            current_pos, current_vel, tau_total, [0, 0, -9.81], np.zeros(6)
        )
        current_vel += acc * dt
        current_pos += current_vel * dt

        # Joint limits
        for j in range(6):
            lo, hi = planner.joint_limits[j]
            if current_pos[j] < lo:
                current_pos[j] = lo
                current_vel[j] = 0.0
            elif current_pos[j] > hi:
                current_pos[j] = hi
                current_vel[j] = 0.0

        # Log
        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))

        # Best-effort scheduling to hit 10 ms period
        now = time.perf_counter()
        loop_durations.append(now - loop_start)

        if enforce_timing:
            sleep_s = next_deadline - now
            if sleep_s > 0:
                time.sleep(sleep_s)
            else:
                deadline_misses += 1
            next_deadline += dt

    # 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)
    loop_durations    = np.array(loop_durations)
    time_steps        = np.arange(n_steps) * dt

    # Summary
    jitter_ms = (loop_durations - dt) * 1e3
    print("Simulation complete:")
    print(f"- Duration: {time_steps[-1]:.2f} s")
    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"- Mean loop: {loop_durations.mean()*1e3:.2f} ms (target {dt*1e3:.2f} ms)")
    print(f"- Jitter RMS: {jitter_ms.std():.2f} ms")
    if enforce_timing:
        print(f"- Deadline misses: {deadline_misses}")

    # (Optional) Plotting omitted for brevity...
    return {
        "reference": ref_trajectory,
        "actual_positions": actual_positions,
        "actual_velocities": actual_velocities,
        "control_torques": control_torques,
        "tracking_errors": tracking_errors,
        "loop_durations": loop_durations,
        "deadline_misses": deadline_misses if enforce_timing else None,
    }

# Example run
results = simulate_high_frequency_control(enforce_timing=True)

Real-Time Implementation Guidelines#

For actual hard real-time control on hardware:

  1. Trajectory pre-computation

    # Use ManipulaPy offline to generate trajectories
trajectory = planner.joint_trajectory(start, goal, Tf, N, method)

# Export for the RT system
np.save("trajectory_positions.npy",      trajectory["positions"])
np.save("trajectory_velocities.npy",     trajectory["velocities"])
np.save("trajectory_accelerations.npy",  trajectory["accelerations"])

  2. Real-time system integration

    • Load precomputed trajectories on the RT controller.

    • Implement the inner loop in C/C++ or an RT-capable environment.

    • Use deterministic fieldbuses (e.g., EtherCAT) or RT IPC for setpoints.

  3. Typical timing requirements

    Application

    Frequency

    Timing Type

    Hardware Requirements

    Position Control

    100–200 Hz

    Soft RT

    Industrial PC + RT kernel

    Force/Impedance Control

    1000+ Hz

    Hard RT

    Dedicated RT controller / MCU / FPGA

    Vision Servoing

    60–120 Hz

    Soft RT

    Vision PC + RT bridge

    Safety/Interlocks

    1000+ Hz

    Hard RT

    Safety-rated hardware

Performance Validation#

The simulation helps validate:

  • Control stability at target loop rates.

  • Trajectory smoothness for different time-scaling methods.

  • Computational margins prior to RT deployment.

  • Tracking accuracy under realistic timing constraints.

Integration with Real-Time Systems#

Common RT platforms:

  • ROS 2 on RT kernel — soft real-time.

  • PREEMPT_RT / Xenomai / VxWorks — hard real-time capable.

  • Industrial controllers / PLCs / MCUs / FPGAs — deterministic low-latency loops.

Example workflow:

# 1) Design & validate offline (ManipulaPy)
traj = planner.joint_trajectory(...)

# 2) Validate via simulation
sim_out = simulate_high_frequency_control(enforce_timing=True)

# 3) Export for RT deployment
export_for_realtime_system(traj, target_platform="ros2_rt")

# 4) Deploy to hardware with RT guarantees (C/C++ loop + deterministic comms)

Terminology Clarification#

  • Simulation: What the Python examples do (no timing guarantees).

  • Soft Real-Time: Best-effort timing; occasional jitter/misses permissible.

  • Hard Real-Time: Guaranteed deadlines; requires RT OS/hardware.

  • Real-Time (generic): Use “soft” or “hard” explicitly to avoid ambiguity.

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.