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
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
velocity: :math:`dot{boldsymboltheta}(t)
= frac{1}{T}bigl(boldsymboltheta_{f}-boldsymboltheta_{0}bigr),sigma_{m}β(s)`
acceleration: :math:`ddot{boldsymboltheta}(t)
= frac{1}{T^{2}}bigl(boldsymboltheta_{f}-boldsymboltheta_{0}bigr),sigma_{m}ββ(s)`
where \(m=3\) or \(5\) for cubic/quintic. 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ο
Time-scale with \(\sigma_{3}\) or \(\sigma_{5}\) for smooth joint profiles.
Interpolate Cartesian end-effector motion on SE(3).
Compute velocities/accelerations and feed into inverse dynamics for torque evaluation.
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ο
from ManipulaPy.path_planning import TrajectoryPlanning
from ManipulaPy.urdf_processor import URDFToSerialManipulator
# Load robot model
processor = URDFToSerialManipulator("robot.urdf")
robot = processor.serial_manipulator
dynamics = processor.dynamics
# Define joint and torque limits
joint_limits = [(-np.pi, np.pi)] * 6 # 6-DOF robot
torque_limits = [(-50, 50)] * 6 # Β±50 Nβ
m per joint
# Create trajectory planner
planner = TrajectoryPlanning(
serial_manipulator=robot,
urdf_path="robot.urdf",
dynamics=dynamics,
joint_limits=joint_limits,
torque_limits=torque_limits
)
print("Trajectory planner initialized successfully")
Simple Joint Trajectoryο
import numpy as np
# Define start and end configurations
theta_start = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
theta_end = np.array([0.5, 0.3, -0.2, 0.1, 0.4, -0.1])
# Trajectory parameters
Tf = 3.0 # Duration: 3 seconds
N = 100 # Number of points
method = 3 # Cubic time scaling
# Generate trajectory
trajectory = planner.joint_trajectory(theta_start, theta_end, Tf, N, method)
print(f"Generated trajectory with {N} points")
print(f"Position shape: {trajectory['positions'].shape}")
print(f"Velocity shape: {trajectory['velocities'].shape}")
print(f"Acceleration shape: {trajectory['accelerations'].shape}")
# Verify start and end points
np.testing.assert_allclose(trajectory['positions'][0], theta_start, rtol=1e-3)
np.testing.assert_allclose(trajectory['positions'][-1], theta_end, rtol=1e-3)
TrajectoryPlanning Classο
Class Constructorο
TrajectoryPlanning(serial_manipulator, urdf_path, dynamics, joint_limits, torque_limits=None)
Parameters:
- serial_manipulator: SerialManipulator instance for kinematics
- urdf_path: Path to robot URDF file for collision checking
- dynamics: ManipulatorDynamics instance for dynamics computations
- joint_limits: List of (min, max) tuples for each joint
- torque_limits: Optional list of (min, max) torque limits
Attributes:
- serial_manipulator: Robot kinematics model
- dynamics: Robot dynamics model
- joint_limits: Joint position constraints
- torque_limits: Joint torque constraints
- collision_checker: Collision detection system
- potential_field: Potential field for obstacle avoidance
Core Methodsο
joint_trajectory()ο
Generates smooth joint-space trajectories with CUDA acceleration:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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ο
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.