Trajectory Planning User Guide ============================== This guide covers the trajectory planning capabilities in ManipulaPy, including joint-space and Cartesian-space trajectory generation, dynamics integration, and collision avoidance using CUDA acceleration. Introduction ------------ The TrajectoryPlanning class provides comprehensive trajectory generation and execution capabilities for robotic manipulators. It combines kinematic trajectory planning with dynamic analysis and collision avoidance to generate feasible, smooth robot motions. **Key Features:** - Joint-space trajectory generation with cubic/quintic time scaling - Cartesian-space trajectory planning for end-effector paths - CUDA-accelerated computations for real-time performance - Integrated dynamics analysis (forward/inverse dynamics) - Collision detection and avoidance using potential fields - Support for various trajectory optimization objectives Basic Usage ----------- Setting Up Trajectory Planning ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. code-block:: python import numpy as np from ManipulaPy.path_planning import TrajectoryPlanning from ManipulaPy.urdf_processor import URDFToSerialManipulator # Load robot model processor = URDFToSerialManipulator("robot.urdf") robot = processor.serial_manipulator dynamics = processor.dynamics # Define joint and torque limits joint_limits = [(-np.pi, np.pi)] * 6 # 6-DOF robot torque_limits = [(-50, 50)] * 6 # ±50 N⋅m per joint # Create trajectory planner planner = TrajectoryPlanning( serial_manipulator=robot, urdf_path="robot.urdf", dynamics=dynamics, joint_limits=joint_limits, torque_limits=torque_limits ) print("Trajectory planner initialized successfully") Simple Joint Trajectory ~~~~~~~~~~~~~~~~~~~~~~~ .. 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}") 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 ---------------------- Trajectory Execution ~~~~~~~~~~~~~~~~~~~~ Real-time trajectory following for robot control: .. code-block:: python import numpy as np import matplotlib.pyplot as plt def real_time_trajectory_execution(): """Simulate real-time trajectory execution.""" # Generate reference trajectory theta_start = np.array([0.1, 0.2, -0.1, 0.0, 0.3, 0.0]) theta_end = np.array([0.8, -0.3, 0.5, -0.2, 0.6, -0.4]) ref_trajectory = planner.joint_trajectory( theta_start, theta_end, Tf=4.0, N=400, method=5 # 100 Hz ) # Simulation parameters dt = 0.01 # 100 Hz control rate n_steps = ref_trajectory['positions'].shape[0] # Control parameters Kp = np.diag([100, 80, 60, 40, 30, 20]) Kd = np.diag([10, 8, 6, 4, 3, 2]) # Initialize simulation state current_pos = theta_start.copy() current_vel = np.zeros(6) # Storage for results actual_positions = [] actual_velocities = [] control_torques = [] tracking_errors = [] print("Simulating real-time trajectory execution...") for i in range(n_steps): # Get reference at current time ref_pos = ref_trajectory['positions'][i] ref_vel = ref_trajectory['velocities'][i] ref_acc = ref_trajectory['accelerations'][i] # Compute tracking error pos_error = ref_pos - current_pos vel_error = ref_vel - current_vel # PD control with feedforward tau_pd = Kp @ pos_error + Kd @ vel_error # Feedforward compensation tau_ff = planner.dynamics.inverse_dynamics( ref_pos, ref_vel, ref_acc, [0, 0, -9.81], np.zeros(6) ) # Total control torque tau_total = tau_pd + tau_ff # Apply torque limits for j in range(6): tau_total[j] = np.clip(tau_total[j], planner.torque_limits[j][0], planner.torque_limits[j][1]) # Simulate robot dynamics acceleration = planner.dynamics.forward_dynamics( current_pos, current_vel, tau_total, [0, 0, -9.81], np.zeros(6) ) # Integrate (simple Euler integration) current_vel += acceleration * dt current_pos += current_vel * dt # Apply joint limits for j in range(6): if current_pos[j] < planner.joint_limits[j][0]: current_pos[j] = planner.joint_limits[j][0] current_vel[j] = 0 elif current_pos[j] > planner.joint_limits[j][1]: current_pos[j] = planner.joint_limits[j][1] current_vel[j] = 0 # Store results actual_positions.append(current_pos.copy()) actual_velocities.append(current_vel.copy()) control_torques.append(tau_total.copy()) tracking_errors.append(np.linalg.norm(pos_error)) # Convert to arrays actual_positions = np.array(actual_positions) actual_velocities = np.array(actual_velocities) control_torques = np.array(control_torques) tracking_errors = np.array(tracking_errors) # Analysis time_steps = np.arange(n_steps) * dt print("Trajectory execution completed:") print(f"- Duration: {time_steps[-1]:.1f} seconds") print(f"- Final tracking error: {tracking_errors[-1]:.4f} rad") print(f"- RMS tracking error: {np.sqrt(np.mean(tracking_errors**2)):.4f} rad") print(f"- Max tracking error: {np.max(tracking_errors):.4f} rad") # Plot results plt.figure(figsize=(15, 12)) # Position tracking plt.subplot(3, 2, 1) for i in range(6): plt.plot(time_steps, np.degrees(ref_trajectory['positions'][:, i]), '--', alpha=0.7, label=f'Ref Joint {i+1}') plt.plot(time_steps, np.degrees(actual_positions[:, i]), '-', linewidth=2, label=f'Act Joint {i+1}') plt.xlabel('Time (s)') plt.ylabel('Position (degrees)') plt.title('Position Tracking') plt.legend() plt.grid(True) # Velocity tracking plt.subplot(3, 2, 2) for i in range(6): plt.plot(time_steps, ref_trajectory['velocities'][:, i], '--', alpha=0.7, label=f'Ref Joint {i+1}') plt.plot(time_steps, actual_velocities[:, i], '-', linewidth=2, label=f'Act Joint {i+1}') plt.xlabel('Time (s)') plt.ylabel('Velocity (rad/s)') plt.title('Velocity Tracking') plt.legend() plt.grid(True) # Control torques plt.subplot(3, 2, 3) for i in range(6): plt.plot(time_steps, control_torques[:, i], label=f'Joint {i+1}') plt.xlabel('Time (s)') plt.ylabel('Torque (N⋅m)') plt.title('Control Torques') plt.legend() plt.grid(True) # Tracking error plt.subplot(3, 2, 4) plt.plot(time_steps, np.degrees(tracking_errors), 'r-', linewidth=2) plt.xlabel('Time (s)') plt.ylabel('Tracking Error (degrees)') plt.title('Position Tracking Error') plt.grid(True) # End-effector tracking ref_ee_positions = [] actual_ee_positions = [] for ref_pos, act_pos in zip(ref_trajectory['positions'], actual_positions): T_ref = planner.serial_manipulator.forward_kinematics(ref_pos) T_act = planner.serial_manipulator.forward_kinematics(act_pos) ref_ee_positions.append(T_ref[:3, 3]) actual_ee_positions.append(T_act[:3, 3]) ref_ee_positions = np.array(ref_ee_positions) actual_ee_positions = np.array(actual_ee_positions) ax = plt.subplot(3, 2, 5, projection='3d') ax.plot(ref_ee_positions[:, 0], ref_ee_positions[:, 1], ref_ee_positions[:, 2], 'b--', alpha=0.7, linewidth=2, label='Reference') ax.plot(actual_ee_positions[:, 0], actual_ee_positions[:, 1], actual_ee_positions[:, 2], 'r-', linewidth=2, label='Actual') ax.set_xlabel('X (m)') ax.set_ylabel('Y (m)') ax.set_zlabel('Z (m)') ax.set_title('End-Effector Tracking') ax.legend() # Control effort plt.subplot(3, 2, 6) control_effort = np.linalg.norm(control_torques, axis=1) plt.plot(time_steps, control_effort, 'g-', linewidth=2) plt.xlabel('Time (s)') plt.ylabel('Total Control Effort (N⋅m)') plt.title('Control Effort') plt.grid(True) plt.tight_layout() plt.show() return { 'reference': ref_trajectory, 'actual_positions': actual_positions, 'actual_velocities': actual_velocities, 'control_torques': control_torques, 'tracking_errors': tracking_errors } # Run real-time simulation execution_results = real_time_trajectory_execution() Practical Applications ---------------------- Pick and Place Operation ~~~~~~~~~~~~~~~~~~~~~~~~ Complete pick-and-place trajectory planning: .. 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.