Simulation Module User Guide

The Simulation Module provides a comprehensive PyBullet-based environment for visualizing, testing, and interacting with your ManipulaPy-powered robot models. It supports real-time control, trajectory playback, collision checking, data logging, and GPU-accelerated computations through CuPy integration.

Note

This guide assumes Python 3.10+, PyBullet 3.2.5+, CuPy 12.0+, and Matplotlib 3.5+. For optimal performance, CUDA-capable GPU is recommended but not required.

Installation and Setup

Prerequisites

Before using the Simulation module, ensure you have the following dependencies:

Core Dependencies

pip install ManipulaPy[simulation] pybullet>=3.2.5 matplotlib>=3.5 numpy>=1.21

GPU Acceleration (Optional but Recommended)

pip install cupy-cuda11x  # For CUDA 11.x
# or
pip install cupy-cuda12x  # For CUDA 12.x

URDF Processing

pip install urchin>=0.0.27

Verification

Verify your installation with this simple test:

import pybullet as p
import ManipulaPy.sim as sim
import cupy as cp

print("PyBullet version:", p.__version__)
print("CuPy version:", cp.__version__)
print("CUDA available:", cp.cuda.is_available())
print("Simulation module loaded:", hasattr(sim, "Simulation"))

Tip

If CUDA is not available, the simulation will automatically fall back to CPU computation with slightly reduced performance.

Quick Start

Basic Trajectory Simulation

Launch a simple trajectory playback with your robot:

from ManipulaPy.sim import Simulation
from ManipulaPy.ManipulaPy_data.xarm import urdf_file as xarm_urdf
import numpy as np

# Define joint limits for a 6-DOF manipulator
joint_limits = [(-3.14, 3.14)] * 6
torque_limits = [(-50, 50)] * 6  # Newton-meters

# Create Simulation instance
sim = Simulation(
    urdf_file_path=xarm_urdf,
    joint_limits=joint_limits,
    torque_limits=torque_limits,
    time_step=0.01,           # 100 Hz simulation
    real_time_factor=1.0      # Real-time playback
)

# Initialize robot dynamics and planning
sim.initialize_robot()
sim.initialize_planner_and_controller()

# Generate a smooth sinusoidal trajectory
t = np.linspace(0, 4*np.pi, 300)
trajectory = []
for t_i in t:
    # Create smooth joint motion
    joint_angles = 0.5 * np.sin(0.5 * t_i) * np.array([1, 0.8, 0.6, 0.4, 0.2, 0.1])
    trajectory.append(joint_angles)

# Execute trajectory with visualization
final_ee_position = sim.run_trajectory(trajectory)
print(f"Final end-effector position: {final_ee_position}")

Interactive Manual Control

Enter manual control mode for real-time robot manipulation:

# Enter interactive mode with GUI sliders
sim.manual_control()

# This opens PyBullet GUI with:
# - Joint sliders for each degree of freedom
# - Gravity control (-20 to 20 m/sΒ²)
# - Time step adjustment (0.001 to 0.1 s)
# - Reset button to return to home position

Module API Reference

Simulation Class

Constructor Parameters

__init__(urdf_file_path, joint_limits, torque_limits=None, time_step=0.01, real_time_factor=1.0, physics_client=None)

Initialize the simulation environment.

Parameters:

  • urdf_file_path (str) – Path to the robot’s URDF file

  • joint_limits (list) – List of (min, max) tuples for each joint in radians

  • torque_limits (list) – Optional list of (min, max) torques in Nβ‹…m. If None, uses unlimited torques

  • time_step (float) – Physics simulation time step in seconds (default: 0.01)

  • real_time_factor (float) – Playback speed multiplier (1.0 = real-time, 0.5 = half-speed)

  • physics_client (int) – Existing PyBullet client ID, or None to create new GUI client

Raises:

Core Simulation Methods

Trajectory Execution

run_trajectory(joint_trajectory) → np.ndarray

Execute a sequence of joint configurations with physics simulation and visualization.

Parameters:

joint_trajectory (list) – List of joint angle arrays, each matching robot DOF

Returns:

Final end-effector position [x, y, z]

Return type:

np.ndarray

# Example: Linear interpolation between two poses
start = np.zeros(6)
end = np.array([0.5, -0.3, 0.8, 0, -0.5, 0])

trajectory = []
for i in range(100):
    alpha = i / 99.0
    pose = start + alpha * (end - start)
    trajectory.append(pose)

final_pos = sim.run_trajectory(trajectory)

Controller Integration

run_controller(controller, desired_positions, desired_velocities, desired_accelerations, g, Ftip, Kp, Ki, Kd) → np.ndarray

Execute closed-loop control with real-time feedback and visualization.

Parameters:

  • controller (ManipulatorController) – Controller instance from ManipulaPy.control

  • desired_positions (np.ndarray) – Desired joint positions (N_steps Γ— DOF)

  • desired_velocities (np.ndarray) – Desired joint velocities (N_steps Γ— DOF)

  • desired_accelerations (np.ndarray) – Desired joint accelerations (N_steps Γ— DOF)

  • g (list) – Gravity vector [gx, gy, gz] in m/sΒ²

  • Ftip (list) – External force/torque at end-effector [fx, fy, fz, Ο„x, Ο„y, Ο„z]

  • Kp (list) – Proportional gains for each joint

  • Ki (list) – Integral gains for each joint

  • Kd (list) – Derivative gains for each joint

Returns:

Final end-effector position

Return type:

np.ndarray

Manual Control and Interaction

manual_control()

Enter interactive manual control mode with GUI sliders.

Creates real-time sliders for: - Each robot joint (within specified limits) - Gravity magnitude and direction - Physics time step - Reset button for returning to home position

Raises:

KeyboardInterrupt – Exit manual mode with Ctrl+C

Initialization and Setup Methods

initialize_robot()

Process URDF file and create robot dynamics model.

This method: - Loads and parses the URDF file - Extracts kinematic and dynamic parameters - Creates SerialManipulator and ManipulatorDynamics instances - Identifies non-fixed joints for control

initialize_planner_and_controller()

Initialize trajectory planning and control modules.

Creates: - TrajectoryPlanning instance for path generation - ManipulatorController for closed-loop control - Collision checking and potential field modules

State Management and Monitoring

get_joint_positions() → np.ndarray

Get current joint positions from simulation.

Returns:

Joint angles in radians

Return type:

np.ndarray

set_joint_positions(joint_positions)

Set target joint positions for position control.

Parameters:

joint_positions (array_like) – Target angles in radians

check_collisions()

Check for self-collisions and log contact points.

Queries PyBullet contact detection and logs warnings for any detected collisions between robot links.

Data Logging and Analysis

save_joint_states(filename='joint_states.csv')

Save current joint states to CSV file.

Parameters:

filename (str) – Output CSV filename

Creates CSV with columns: Position, Velocity for each joint.

plot_trajectory_in_scene(joint_trajectory, end_effector_trajectory)

Create 3D visualization of end-effector trajectory.

Parameters:

  • joint_trajectory (list) – Joint angle trajectory

  • end_effector_trajectory (list) – End-effector position trajectory

Advanced Examples

Closed-Loop Control Simulation

Demonstrate advanced controller integration with GPU acceleration:

from ManipulaPy.sim import Simulation
from ManipulaPy.control import ManipulatorController
from ManipulaPy.path_planning import TrajectoryPlanning
import cupy as cp
import numpy as np

# Setup simulation
sim = Simulation(urdf_file_path="robot.urdf",
                joint_limits=[(-np.pi, np.pi)]*6,
                torque_limits=[(-100, 100)]*6)
sim.initialize_robot()
sim.initialize_planner_and_controller()

# Generate reference trajectory
planner = TrajectoryPlanning(sim.robot, "robot.urdf", sim.dynamics,
                            sim.joint_limits, sim.torque_limits)

traj_data = planner.joint_trajectory(
    thetastart=np.zeros(6),
    thetaend=np.array([0.5, -0.3, 0.8, 0, -0.5, 0]),
    Tf=5.0,  # 5 second trajectory
    N=500,   # 500 waypoints
    method=5 # Quintic time scaling
)

# Controller parameters
controller = ManipulatorController(sim.dynamics)
Kp = np.array([50, 40, 30, 20, 15, 10])  # Position gains
Ki = np.array([0.1, 0.1, 0.1, 0.05, 0.05, 0.05])  # Integral gains
Kd = np.array([5, 4, 3, 2, 1.5, 1])     # Derivative gains

# Execute closed-loop control
final_pos = sim.run_controller(
    controller=controller,
    desired_positions=traj_data["positions"],
    desired_velocities=traj_data["velocities"],
    desired_accelerations=traj_data["accelerations"],
    g=[0, 0, -9.81],
    Ftip=[0, 0, 0, 0, 0, 0],
    Kp=Kp, Ki=Ki, Kd=Kd
)

Multi-Phase Simulation Workflow

Combine trajectory execution, manual control, and data logging:

import time
from pathlib import Path

# Phase 1: Automated trajectory execution
print("Phase 1: Running automated trajectory...")

# Create complex multi-segment trajectory
segments = []
waypoints = [
    np.array([0, 0, 0, 0, 0, 0]),           # Home
    np.array([0.8, -0.5, 0.3, 0.2, -0.1, 0]), # Intermediate
    np.array([0.2, 0.7, -0.4, -0.3, 0.6, 0.8]) # Target
]

for i in range(len(waypoints)-1):
    segment = []
    for j in range(50):
        alpha = j / 49.0
        pose = waypoints[i] + alpha * (waypoints[i+1] - waypoints[i])
        segment.append(pose)
    segments.extend(segment)

sim.run_trajectory(segments)

# Phase 2: Manual inspection and adjustment
print("Phase 2: Manual control mode...")
print("Use GUI sliders to inspect robot configuration")
print("Press Reset button when done")

try:
    sim.manual_control()
except KeyboardInterrupt:
    print("Manual control ended")

# Phase 3: Data logging and analysis
print("Phase 3: Saving simulation data...")

# Save joint states
timestamp = int(time.time())
sim.save_joint_states(f"joint_states_{timestamp}.csv")

# Additional trajectory analysis
ee_positions = []
for joint_config in segments[-10:]:  # Last 10 poses
    sim.set_joint_positions(joint_config)
    time.sleep(0.1)
    ee_pos = sim.get_joint_positions()  # Current state
    ee_positions.append(ee_pos)

print(f"Trajectory analysis complete. Final EE positions saved.")

GPU-Accelerated Batch Simulation

Leverage CuPy for high-performance batch processing:

import cupy as cp
import time

# Generate large batch of test trajectories
n_trajectories = 100
trajectory_length = 200

print(f"Generating {n_trajectories} test trajectories...")

# Create random but smooth trajectories
np.random.seed(42)
trajectories = []

for i in range(n_trajectories):
    # Random waypoints
    waypoints = np.random.uniform(-1, 1, (5, 6))  # 5 waypoints, 6 DOF

    # Smooth interpolation
    trajectory = []
    for j in range(4):  # 4 segments
        for k in range(trajectory_length // 4):
            alpha = k / (trajectory_length // 4 - 1)
            pose = waypoints[j] + alpha * (waypoints[j+1] - waypoints[j])
            trajectory.append(pose)

    trajectories.append(trajectory)

# Batch execution with timing
execution_times = []
final_positions = []

print("Executing batch simulation...")
for i, traj in enumerate(trajectories):
    start_time = time.time()

    # Clip to joint limits
    traj_clipped = []
    for pose in traj:
        clipped = np.clip(pose,
                        [limits[0] for limits in sim.joint_limits],
                        [limits[1] for limits in sim.joint_limits])
        traj_clipped.append(clipped)

    final_pos = sim.run_trajectory(traj_clipped)
    execution_time = time.time() - start_time

    execution_times.append(execution_time)
    final_positions.append(final_pos)

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

# Performance analysis
avg_time = np.mean(execution_times)
std_time = np.std(execution_times)

print(f"\nBatch Simulation Results:")
print(f"Average execution time: {avg_time:.3f} Β± {std_time:.3f} seconds")
print(f"Total simulation time: {sum(execution_times):.1f} seconds")
print(f"Trajectories per minute: {60 * n_trajectories / sum(execution_times):.1f}")

Advanced Configuration

Physics Parameter Tuning

Fine-tune simulation physics for accuracy vs. performance:

# High-precision configuration
sim_precise = Simulation(
    urdf_file_path="robot.urdf",
    joint_limits=joint_limits,
    time_step=0.001,      # 1000 Hz for high precision
    real_time_factor=0.1  # Slow motion for detailed analysis
)

# Real-time configuration
sim_realtime = Simulation(
    urdf_file_path="robot.urdf",
    joint_limits=joint_limits,
    time_step=0.02,       # 50 Hz for real-time performance
    real_time_factor=1.0  # Real-time execution
)

# After initialization, adjust physics parameters
import pybullet as p

# Set solver iterations for accuracy
p.setPhysicsEngineParameter(numSolverIterations=100)

# Enable additional collision margin
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)

# Configure constraint solving
p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS)

Custom Visualization and Debugging

Add custom visual elements and debugging information:

import pybullet as p

def add_coordinate_frame(position, orientation, size=0.1):
    """Add coordinate frame visualization."""
    # X-axis (red)
    p.addUserDebugLine(position,
                      [position[0]+size, position[1], position[2]],
                      [1,0,0], lineWidth=3)
    # Y-axis (green)
    p.addUserDebugLine(position,
                      [position[0], position[1]+size, position[2]],
                      [0,1,0], lineWidth=3)
    # Z-axis (blue)
    p.addUserDebugLine(position,
                      [position[0], position[1], position[2]+size],
                      [0,0,1], lineWidth=3)

def add_workspace_visualization(sim, resolution=20):
    """Visualize robot workspace boundary."""
    workspace_points = []

    # Sample random joint configurations
    for _ in range(1000):
        joints = []
        for limit in sim.joint_limits:
            joints.append(np.random.uniform(limit[0], limit[1]))

        # Get end-effector position
        T = sim.robot.forward_kinematics(joints)
        workspace_points.append(T[:3, 3])

    # Draw workspace boundary points
    for point in workspace_points[::10]:  # Subsample for performance
        p.addUserDebugLine(point, point, [0.5, 0.5, 0.5], lineWidth=1)

# Usage in simulation
sim.initialize_robot()

# Add visualization elements
add_coordinate_frame([0, 0, 0], [0, 0, 0, 1], 0.2)  # World frame
add_workspace_visualization(sim)

Performance Optimization

Memory Management

Optimize memory usage for long-running simulations:

import gc
import psutil
import cupy as cp

def monitor_memory():
    """Monitor CPU and GPU memory usage."""
    # CPU memory
    cpu_memory = psutil.virtual_memory()
    print(f"CPU Memory: {cpu_memory.percent:.1f}% used")

    # GPU memory (if available)
    if cp.cuda.is_available():
        mempool = cp.get_default_memory_pool()
        print(f"GPU Memory: {mempool.used_bytes() / 1024**3:.2f} GB used")

def cleanup_simulation():
    """Clean up memory after simulation."""
    # Force garbage collection
    gc.collect()

    # Clear GPU memory pool
    if cp.cuda.is_available():
        mempool = cp.get_default_memory_pool()
        mempool.free_all_blocks()

    print("Memory cleanup completed")

# Example usage in long simulation loop
for i in range(100):
    # Run trajectory
    sim.run_trajectory(trajectory)

    # Periodic cleanup
    if i % 10 == 0:
        monitor_memory()
        cleanup_simulation()

Parallel Simulation Setup

Configure multiple simulation instances for parallel processing:

import multiprocessing as mp
import pybullet as p

def run_simulation_worker(worker_id, urdf_path, joint_limits, trajectory_queue, result_queue):
    """Worker function for parallel simulation."""
    # Create simulation in DIRECT mode (no GUI)
    client = p.connect(p.DIRECT)

    sim = Simulation(urdf_file_path=urdf_path,
                    joint_limits=joint_limits,
                    physics_client=client)
    sim.initialize_robot()

    while True:
        try:
            trajectory = trajectory_queue.get(timeout=1)
            if trajectory is None:  # Shutdown signal
                break

            final_pos = sim.run_trajectory(trajectory)
            result_queue.put((worker_id, final_pos))

        except:
            break

    p.disconnect(client)

# Usage example
def parallel_simulation_example():
    n_workers = 4
    trajectory_queue = mp.Queue()
    result_queue = mp.Queue()

    # Start worker processes
    workers = []
    for i in range(n_workers):
        worker = mp.Process(target=run_simulation_worker,
                          args=(i, "robot.urdf", joint_limits,
                                trajectory_queue, result_queue))
        worker.start()
        workers.append(worker)

    # Submit trajectories
    for traj in trajectories:
        trajectory_queue.put(traj)

    # Collect results
    results = []
    for _ in trajectories:
        results.append(result_queue.get())

    # Shutdown workers
    for _ in workers:
        trajectory_queue.put(None)
    for worker in workers:
        worker.join()

    return results

Troubleshooting Guide

Common Issues and Solutions

1. Black screen or no GUI

# Check PyBullet GUI availability
try:
    client = p.connect(p.GUI)
    print("GUI mode available")
except:
    print("GUI mode not available, using DIRECT mode")
    client = p.connect(p.DIRECT)
finally:
    p.disconnect(client)

Solutions: - Ensure X11 forwarding for remote systems: ssh -X username@hostname - Install GUI libraries: sudo apt-get install python3-tk - Use DIRECT mode for headless servers

2. Joint limit violations

def validate_trajectory(trajectory, joint_limits):
    """Validate trajectory against joint limits."""
    violations = []
    for i, pose in enumerate(trajectory):
        for j, (angle, (min_val, max_val)) in enumerate(zip(pose, joint_limits)):
            if angle < min_val or angle > max_val:
                violations.append((i, j, angle, min_val, max_val))
    return violations

# Usage
violations = validate_trajectory(trajectory, sim.joint_limits)
if violations:
    print(f"Found {len(violations)} joint limit violations")
    for step, joint, angle, min_val, max_val in violations[:5]:
        print(f"Step {step}, Joint {joint}: {angle:.3f} not in [{min_val:.3f}, {max_val:.3f}]")

3. Simulation instability

Reduce time step and increase solver iterations:

# More stable configuration
sim = Simulation(urdf_file_path="robot.urdf",
                joint_limits=joint_limits,
                time_step=0.001)  # Smaller time step

# Increase solver accuracy
p.setPhysicsEngineParameter(numSolverIterations=200)
p.setPhysicsEngineParameter(useSplitImpulse=True)

4. Performance issues

# Profile simulation performance
import time
import cProfile

def profile_trajectory(sim, trajectory):
    """Profile trajectory execution."""
    profiler = cProfile.Profile()

    start_time = time.time()
    profiler.enable()

    sim.run_trajectory(trajectory)

    profiler.disable()
    execution_time = time.time() - start_time

    print(f"Execution time: {execution_time:.3f} seconds")
    print(f"FPS: {len(trajectory) / execution_time:.1f}")

    # Print top time consumers
    profiler.print_stats(sort='cumtime', lines=10)

5. CUDA/CuPy errors

# Graceful fallback to CPU
try:
    import cupy as cp
    device = cp.cuda.Device()
    print(f"Using GPU: {device.id}")
except ImportError:
    print("CuPy not available, using NumPy")
    import numpy as cp
except Exception as e:
    print(f"CUDA error: {e}, falling back to NumPy")
    import numpy as cp

Debugging Tools

Enable detailed logging and visualization:

import logging

# Enable debug logging
logging.basicConfig(level=logging.DEBUG,
                   format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')

# Add trajectory debugging
def debug_trajectory(sim, trajectory, step_interval=10):
    """Debug trajectory execution with detailed logging."""
    for i, pose in enumerate(trajectory):
        if i % step_interval == 0:
            # Log joint positions
            print(f"Step {i}: joints = {pose}")

            # Check joint limits
            for j, (angle, (min_val, max_val)) in enumerate(zip(pose, sim.joint_limits)):
                if angle < min_val or angle > max_val:
                    print(f"  WARNING: Joint {j} out of limits: {angle:.3f}")

            # Visualize current pose
            sim.set_joint_positions(pose)
            time.sleep(0.1)

Performance Benchmarks

Typical performance metrics for reference:

Performance Benchmarks

Configuration

FPS (GUI)

FPS (Headless)

Notes

Default (dt=0.01)

60-100

200-500

Standard real-time

High precision (dt=0.001)

10-20

50-100

Detailed physics

Fast preview (dt=0.02)

100-200

500-1000

Quick visualization

GPU accelerated

+20-50%

+30-70%

With CuPy controller

Best Practices

  1. Always validate trajectories before execution

  2. Use appropriate time steps for your application

  3. Monitor memory usage in long simulations

  4. Enable collision checking for safety-critical applications

  5. Save simulation data for post-analysis

  6. Use GPU acceleration for compute-intensive controllers

  7. Profile performance to identify bottlenecks

Integration with Other Modules

The Simulation module integrates seamlessly with other ManipulaPy components:

  • Kinematics: Forward/inverse kinematics for trajectory generation

  • Dynamics: Physics-based simulation and control

  • Path Planning: Trajectory optimization and collision avoidance

  • Control: Closed-loop feedback control implementation

  • Vision: Sensor simulation and perception testing

See Also