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 :math:`\boldsymbol\theta_{0}` and end :math:`\boldsymbol\theta_{f}`, total duration :math:`T`, define a scalar time-scaling function: .. math:: s = \frac{t}{T}, \quad s \in [0,1] Common choices: - **Cubic** (zero end-velocity): .. math:: \sigma_{3}(s) = 3s^{2} - 2s^{3} - **Quintic** (zero end-velocity & zero end-acceleration): .. math:: \sigma_{5}(s) = 10s^{3} - 15s^{4} + 6s^{5} **Joint-space trajectory:** .. math:: \boldsymbol\theta(t) = \boldsymbol\theta_{0} + \bigl(\boldsymbol\theta_{f} - \boldsymbol\theta_{0}\bigr)\,\sigma_{m}(s) with derivatives: - **Velocity:** .. math:: \dot{\boldsymbol\theta}(t) = \frac{1}{T}\bigl(\boldsymbol\theta_{f}-\boldsymbol\theta_{0}\bigr)\,\sigma_{m}'(s) - **Acceleration:** .. math:: \ddot{\boldsymbol\theta}(t) = \frac{1}{T^{2}}\bigl(\boldsymbol\theta_{f}-\boldsymbol\theta_{0}\bigr)\,\sigma_{m}''(s) where :math:`m=3` or :math:`5` for cubic/quintic time scaling. Cartesian-Space Interpolation -------------------------------- Given start/end end-effector poses :math:`X_{0},X_{f}\in SE(3)`, define the relative transform .. math:: \Delta X = X_{0}^{-1}X_{f} = \exp\bigl(\,[\Xi]\,\bigr), with twist :math:`\Xi\in\mathfrak{se}(3)` given by the matrix logarithm. Then .. math:: X(t) = X_{0}\,\exp\!\bigl([\Xi]\;\sigma_{m}(s)\bigr). Extract : - position: the translational part of :math:`X(t)` - orientation: the rotational part via Rodrigues’ formula on :math:`[\Xi]` Dynamics-Aware Torque Computation --------------------------------- Once joint positions, velocities and accelerations are known, the required torques along the trajectory follow by inverse dynamics: .. math:: \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 :math:`\mathbf p_{i}` generate repulsive potentials .. math:: 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} and an attractive potential toward the goal :math:`U_{\mathrm{att}}(\mathbf q) =\tfrac12\,\zeta\,\lVert \mathbf p(\mathbf q)-\mathbf p_{f}\rVert^{2}`. The total artificial potential .. math:: U(\mathbf q) = U_{\mathrm{att}} + U_{\mathrm{rep}}, yields a force in joint space via the Jacobian transpose: .. math:: \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 :math:`\boldsymbol\theta(t)` so that obstacles are circumvented while preserving smoothness. Putting It All Together ~~~~~~~~~~~~~~~~~~~~~~~ 1. **Time-scale** with :math:`\sigma_{3}` or :math:`\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 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. code-block:: python 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 ~~~~~~~~~~~~~~~~~~~~~~~~~~ .. code-block:: python 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 ~~~~~~~~~~~~~~~~~~~ .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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: .. code-block:: python 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. .. code-block:: python 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** .. code-block:: python # 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** .. list-table:: :header-rows: 1 :widths: 22 16 20 42 * - 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:** .. code-block:: python # 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: .. code-block:: python 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 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. code-block:: python 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 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. code-block:: python 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.