Simulation Module User Guide — ManipulaPy 1.1.0 Documentation
Description: 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, traject...

# 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:

    - FileNotFoundError – If URDF file doesn’t exist

    - ValueError – If joint_limits and URDF DOF don’t match

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

- Control Module User Guide — Controller Implementation Guide

- Trajectory Planning User Guide — Path Planning and Optimization

- Dynamics User Guide — Robot Dynamics and Physics

- Simulation API Reference — Complete API Reference

- PyBullet Documentation — Physics Engine Reference

- CuPy Documentation — GPU Acceleration Reference