From a079bde6a2c71086aa4a14800d89178129942e32 Mon Sep 17 00:00:00 2001 From: ShashwatPatil Date: Sat, 14 Jun 2025 18:48:32 +0530 Subject: [PATCH] updated readme, restructured and renamed the files --- .../02_target_pose_control/README.md | 225 ----------- .../03_gamepad_control/README.md | 280 -------------- .../04_gamepad_collision_avoidance/README.md | 139 ------- .../collision_avoidance.yml | 35 -- .../nodes/franka_collision_controller.py | 366 ------------------ examples/franka-mujoco-control/README.md | 46 --- examples/mujoco-sim/README.md | 21 + .../URDF}/franka_panda/panda.urdf | 0 .../URDF}/kuka_iiwa/model.urdf | 0 .../basic_simulation}/README.md | 14 +- .../basic_simulation}/basic.yml | 2 +- examples/mujoco-sim/gamepad_control/README.md | 75 ++++ .../gamepad_control}/gamepad_control.yml | 12 +- .../nodes/gamepad_controller.py} | 11 +- .../mujoco-sim/target_pose_control/README.md | 99 +++++ .../target_pose_control/nodes/controller.py} | 0 .../nodes/pose_publisher.py | 0 .../target_pose_control.yml} | 14 +- node-hub/gamepad/README.md | 102 ++++- 19 files changed, 308 insertions(+), 1133 deletions(-) delete mode 100644 examples/franka-mujoco-control/02_target_pose_control/README.md delete mode 100644 examples/franka-mujoco-control/03_gamepad_control/README.md delete mode 100644 examples/franka-mujoco-control/04_gamepad_collision_avoidance/README.md delete mode 100644 examples/franka-mujoco-control/04_gamepad_collision_avoidance/collision_avoidance.yml delete mode 100644 examples/franka-mujoco-control/04_gamepad_collision_avoidance/nodes/franka_collision_controller.py delete mode 100644 examples/franka-mujoco-control/README.md create mode 100644 examples/mujoco-sim/README.md rename examples/{franka-mujoco-control => mujoco-sim/URDF}/franka_panda/panda.urdf (100%) rename examples/{franka-mujoco-control => mujoco-sim/URDF}/kuka_iiwa/model.urdf (100%) rename examples/{franka-mujoco-control/01_basic_simulation => mujoco-sim/basic_simulation}/README.md (71%) rename examples/{franka-mujoco-control/01_basic_simulation => mujoco-sim/basic_simulation}/basic.yml (78%) create mode 100644 examples/mujoco-sim/gamepad_control/README.md rename examples/{franka-mujoco-control/03_gamepad_control => mujoco-sim/gamepad_control}/gamepad_control.yml (76%) rename examples/{franka-mujoco-control/03_gamepad_control/nodes/franka_gamepad_controller.py => mujoco-sim/gamepad_control/nodes/gamepad_controller.py} (95%) create mode 100644 examples/mujoco-sim/target_pose_control/README.md rename examples/{franka-mujoco-control/02_target_pose_control/nodes/franka_controller_pytorch.py => mujoco-sim/target_pose_control/nodes/controller.py} (100%) rename examples/{franka-mujoco-control/02_target_pose_control => mujoco-sim/target_pose_control}/nodes/pose_publisher.py (100%) rename examples/{franka-mujoco-control/02_target_pose_control/target_pose_control_pytorch.yml => mujoco-sim/target_pose_control/target_pose_control.yml} (71%) diff --git a/examples/franka-mujoco-control/02_target_pose_control/README.md b/examples/franka-mujoco-control/02_target_pose_control/README.md deleted file mode 100644 index 9b0d593e..00000000 --- a/examples/franka-mujoco-control/02_target_pose_control/README.md +++ /dev/null @@ -1,225 +0,0 @@ -# 02. Target Pose Control - -This example demonstrates Cartesian space control by creating a dedicated Franka controller node that processes target pose commands and outputs joint commands using inverse kinematics with **PyTorch Kinematics**. - -## Running the Example - -```bash -cd 02_target_pose_control -dora build target_pose_control_pytorch.yml -dora run target_pose_control_pytorch.yml -``` - -You should see: -1. Robot moves to predefined target poses automatically -2. Smooth Cartesian space motion with differential inverse kinematics -3. End-effector following target positions accurately -4. GPU-accelerated kinematics computation (if CUDA available) - -## Interactive Control - -While the simulation is running, you can send custom poses: - -```python -# In another terminal -python3 -c " -import pyarrow as pa -from dora import Node -import time - -node = Node() - -# Move to position (0.5, 0.2, 0.6) with downward orientation -target_pose = [0.5, 0.2, 0.6, 180.0, 0.0, 90.0] # x, y, z, roll, pitch, yaw - -node.send_output( - 'target_pose', - pa.array(target_pose, type=pa.float64()), - metadata={'timestamp': time.time()} -) -" -``` - -## How It Works - -### Node Breakdown - -#### 1. **Pose Publisher Node** (`pose_publisher.py`) -```python -class PosePublisher: - def __init__(self): - # Predefined sequence of target poses [x, y, z, roll, pitch, yaw] - self.target_poses = [ - [0.5, 0.5, 0.3, 180.0, 0.0, 90.0], # Position + RPY orientation - [0.6, 0.2, 0.5, 180.0, 0.0, 45.0], # Different orientation - # ... more poses - ] -``` - -**What it does**: -- Sends target poses every 10 seconds -- Cycles through predefined positions and orientations -- Can be replaced with path planning, user input, or any pose generation logic -- **Output**: `target_pose` array `[x, y, z, roll, pitch, yaw]` - -#### 2. **Franka Controller Node** (`franka_controller_pytorch.py`) - -**Key Components**: - -```python -class FrankaController: - def __init__(self): - urdf_path = "path to the file/panda.urdf" - with open(urdf_path, 'rb') as f: - urdf_content = f.read() - - self.chain = pk.build_serial_chain_from_urdf(urdf_content, "panda_hand") - self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - self.chain = self.chain.to(device=self.device) -``` - -**What it does**: -- **Input**: Target poses `[x, y, z, roll, pitch, yaw]` and current joint positions from MuJoCo -- **Processing**: Differential inverse kinematics using PyTorch Kinematics -- **Output**: Joint position commands for the robot - -**Control Algorithm (Differential IK)**: -```python -def apply_differential_ik_control(self): - # 1. Get current end-effector pose using PyTorch Kinematics FK - current_ee_pos, current_ee_rot = self.get_current_ee_pose(self.current_joint_pos) - - # 2. Calculate position and orientation errors - pos_error = self.target_pos - current_ee_pos - rot_error = (desired_rot * current_rot.inv()).as_rotvec() - - # 3. Create 6D twist vector [linear_vel, angular_vel] - twist = np.zeros(6) - twist[:3] = self.Kpos * pos_error / self.integration_dt - twist[3:] = self.Kori * rot_error / self.integration_dt - - # 4. Compute Jacobian using PyTorch Kinematics - jac = self.compute_jacobian_pytorch(self.current_joint_pos) # (6, 7) - - # 5. Solve differential IK with damped least squares - diag = self.damping * np.eye(6) - dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist) - - # 6. Add nullspace control to prefer home position - N = np.eye(7) - np.linalg.pinv(jac) @ jac # Nullspace projection - dq_null = self.Kn * (self.home_pos - self.current_joint_pos) - dq += N @ dq_null - - # 7. Integrate to get new joint positions - new_joints = self.current_joint_pos + dq * self.integration_dt -``` - -#### 3. **MuJoCo Simulation Node** (`dora-mujoco`) -- **Input**: Joint commands from controller -- **Processing**: Physics simulation, rendering, forward kinematics -- **Output**: Joint positions, velocities, sensor data - -## Technical Implementation Details - -- **Main Simulation** (`dora-mujoco` node): Physics, rendering, joint state -- **Controller** (`franka_controller_pytorch.py`): PyTorch Kinematics for FK/Jacobian -- **Single source of truth**: MuJoCo simulation provides all joint states / sensor feedback in case of hardware - -```python -class FrankaController: - def __init__(self): - # Load kinematics model for computation only - self.chain = pk.build_serial_chain_from_urdf(urdf_content, "panda_hand") - self.chain = self.chain.to(device=self.device) # GPU acceleration - - def get_current_ee_pose(self, joint_angles): - """PyTorch Kinematics FK""" - q = torch.tensor(joint_angles, device=self.device, dtype=torch.float32).unsqueeze(0) - tf = self.chain.forward_kinematics(q) - # ... extract position and rotation -``` - -### Key Advantages - -**Performance Benefits**: -- **GPU Acceleration**: PyTorch Kinematics can leverage CUDA for faster computation -- **Optimized Gradients**: Built-in automatic differentiation - - -**Control Benefits**: -- **Differential IK**: Smoother motion than discrete IK solving -- **Nullspace Control**: Avoids joint limits and singularities -- **Real-time Performance**: currently 500Hz control loops - -### Differential vs. Analytical IK - -**Differential IK** (Current Implementation): -```python -# Compute small joint movements based on end-effector error -dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist) -new_joints = current_joints + dq * dt -``` - -**Analytical IK** (Alternative): -```python -# Solve for exact joint configuration to reach target [sometimes exact solution is not available can result in `None return`] -ik_solver = pk.PseudoInverseIK(chain, ...) -solution = ik_solver.solve(target_transform) -``` - -**Why Differential IK**: -- **Smoother motion**: Continuous trajectory without jumps -- **Better convergence**: Less likely to get stuck in local minima -- **Singularity handling**: Graceful behavior near workspace boundaries - -### PyTorch Kinematics Features Used - -1. **Forward Kinematics**: `chain.forward_kinematics(q)` -2. **Jacobian Computation**: `chain.jacobian(q)` -3. **GPU Support**: `.to(device=device)` -4. **Batch Processing**: Handle multiple configurations simultaneously -5. **Automatic Differentiation**: Could enable learning-based control - -## Production Recommendations - -For real robot deployment: -1. **Kinematics Library**: Currently the urdf is used to create the model for pytorch, for you robot you need to make it manually -2. **Use Quaternions**: Replace RPY with quaternion orientation representation -3. **Add Safety Monitors**: Joint limit monitoring, collision checking -4. **Real Robot Interface**: Replace MuJoCo with actual robot drivers -5. **Advanced Control**: Force/torque control, compliant motion - -## Extensions - -This example can be extended with: -- **Learning-Based Control**: Use PyTorch's autodiff for learned components -- **Multi-Robot Coordination**: Leverage GPU parallel processing -- **Advanced IK Solvers**: Try different PyTorch Kinematics IK algorithms -- **Collision Avoidance**: Integrate with [pytorch-volumetric](https://github.com/UM-ARM-Lab/pytorch_volumetric) for SDF queries -- **Real Robot**: Replace MuJoCo with actual robot drivers - -## Troubleshooting - -**"PyTorch Kinematics not found"** -```bash -pip install pytorch-kinematics -``` - -**"CUDA out of memory"** -- Set `device = "cpu"` in controller initialization -- Reduce batch sizes if using advanced features - -**"Robot moves erratically"** -- Check joint limits are correctly applied -- Verify URDF file path is correct -- Try reducing control gains if oscillating - -**"Controller slower than expected"** -- Ensure PyTorch is using GPU: check `torch.cuda.is_available()` -- Profile the forward kinematics and Jacobian computation times - -## Performance Notes - -- **GPU Acceleration**: ~10x speedup for kinematics computation with CUDA -- **Memory Usage**: Minimal - only loads kinematic chain, not full physics -- **Scalability**: Can handle multiple robot arms with batch processing diff --git a/examples/franka-mujoco-control/03_gamepad_control/README.md b/examples/franka-mujoco-control/03_gamepad_control/README.md deleted file mode 100644 index 5424fd39..00000000 --- a/examples/franka-mujoco-control/03_gamepad_control/README.md +++ /dev/null @@ -1,280 +0,0 @@ -# 03. Gamepad Control - -This example demonstrates real-time interactive control by connecting a gamepad to the Franka controller. It builds upon the target pose control example by adding gamepad input processing while using **PyTorch Kinematics** for efficient computation. Shows how to integrate multiple input sources and implement teleoperation with GPU-accelerated kinematics. - -## Controls - -- **Left Stick X**: Move along X-axis (forward/backward) -- **Left Stick Y**: Move along Y-axis (left/right) -- **Right Stick Y**: Move along Z-axis (up/down) -- **LB/RB**: Decrease/Increase movement speed (0.1-1.0x scale) -- **START**: Reset to home position -- **X Button**: Close gripper -- **Y Button**: Open gripper - -## Running the Example - -1. **Connect a gamepad** (Xbox/PlayStation controller via USB or Bluetooth) -2. **Run the simulation**: -```bash -cd 03_gamepad_control -dora build gamepad_control.yml -dora run gamepad_control.yml -``` - -You should see: -1. Robot responds to gamepad input in real-time -2. Smooth incremental movement based on stick input -3. Speed control with bumper buttons -4. Gripper control with face buttons -5. GPU-accelerated kinematics computation (if CUDA available) - -## How It Works - -### Node Breakdown - -#### 1. **Gamepad Node** (`gamepad`) -Built-in Dora node that interfaces with system gamepad drivers. - -**What it does**: -- Polls connected gamepad at 100Hz (`tick: dora/timer/millis/10`) -- Converts raw gamepad input to standardized JSON format -- **Outputs**: - - `raw_control`: Raw gamepad data with axes and button states - - `cmd_vel`: Velocity commands (unused in this example) - -**Raw Control Format**: -```json -{ - "axes": [stick_x, stick_y, trigger_l, stick_rx, stick_ry, trigger_r], - "buttons": [X, A, B, Y, LB, RB, ..., START, ...], - "mapping": {"button_names": {...}, "axis_names": {...}} -} -``` - -#### 2. **Enhanced Franka Controller** (`franka_gamepad_controller.py`) - -**Key Enhancement**: Extends the target pose controller with gamepad input processing using PyTorch Kinematics. - -**Gamepad Input Processing**: -```python -def process_gamepad_input(self, raw_control): - axes = raw_control["axes"] - buttons = raw_control["buttons"] - - # 1. Button handling - if buttons[9]: # START button - # Reset target to home position using PyTorch Kinematics FK - home_ee_pos, _ = self.get_current_ee_pose(self.home_pos) - self.target_pos = home_ee_pos.copy() - print("Reset target to home position") - - # 2. Gripper control - if buttons[0]: # X button - Close gripper - self.gripper_state = 0.0 - print("Gripper: CLOSED") - elif buttons[3]: # Y button - Open gripper - self.gripper_state = 1.0 - print("Gripper: OPEN") - - # 3. Speed scaling - if buttons[4]: self.speed_scale = max(0.1, self.speed_scale - 0.1) # LB - if buttons[5]: self.speed_scale = min(1.0, self.speed_scale + 0.1) # RB - - # 4. Incremental position control with deadzone - dx = self.apply_deadzone(axes[0]) * self.speed_scale * 0.1 - dy = -self.apply_deadzone(axes[1]) * self.speed_scale * 0.1 # Inverted Y - dz = -self.apply_deadzone(axes[3]) * self.speed_scale * 0.1 # Right stick Y - - # 5. Update target position incrementally - if abs(dx) > 0 or abs(dy) > 0 or abs(dz) > 0: - self.target_pos += np.array([dx, dy, dz]) - self.last_input_source = "gamepad" - print(f"Gamepad control: dx={dx:.3f}, dy={dy:.3f}, dz={dz:.3f}") -``` - -**Control Flow with PyTorch Kinematics**: -```python -def apply_cartesian_control(self, current_joints): - # Filter to first 7 joints (arm only) - self.current_joint_pos = current_joints[:7] - - # Get current end-effector pose using PyTorch Kinematics FK - current_ee_pos, current_ee_rot = self.get_current_ee_pose(self.current_joint_pos) - - # Calculate position error - pos_error = self.target_pos - current_ee_pos - - # Construct 6D twist (3 position + 3 orientation) - twist = np.zeros(6) - twist[:3] = self.Kpos * pos_error / self.integration_dt - - # Orientation control - current_rot = Rotation.from_matrix(current_ee_rot) - desired_rot = Rotation.from_matrix(self.get_target_rotation_matrix()) - rot_error = (desired_rot * current_rot.inv()).as_rotvec() - twist[3:] = self.Kori * rot_error / self.integration_dt - - # Get Jacobian using PyTorch Kinematics - jac = self.compute_jacobian_pytorch(self.current_joint_pos) # (6, 7) - - # Damped least squares + nullspace control - diag = self.damping * np.eye(6) - dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist) - - # Nullspace control - drive towards home position - jac_pinv = np.linalg.pinv(jac) - N = np.eye(7) - jac_pinv @ jac # Nullspace projection matrix - dq_null = self.Kn * (self.home_pos - self.current_joint_pos) - dq += N @ dq_null - - # Integrate to get new joint positions - new_joints = self.current_joint_pos + dq * self.integration_dt - - # Return 8-dimensional control: 7 arm joints + gripper - full_commands = np.zeros(8) - full_commands[:7] = new_joints - full_commands[7] = self.gripper_range[0] if self.gripper_state < 0.5 else self.gripper_range[1] - - return full_commands -``` - -#### 3. **MuJoCo Simulation Node** (`dora-mujoco`) -- **Input**: 8D joint commands (7 arm + 1 gripper) from enhanced controller -- **Processing**: Physics simulation, rendering, forward kinematics -- **Output**: Joint positions, velocities, sensor data - -## Technical Implementation Details - -### Control Modes Comparison - -| Feature | Target Pose Control | Gamepad Control | -|---------|-------------------|-----------------| -| **Input Type** | Absolute positions | Incremental movements | -| **Update Rate** | Configurable | Real-time | -| **Control Precision** | Exact target poses | Controller/gamepad positioning | -| **Gripper Control** | None | X/Y button control | -| **Speed Control** | Fixed | Variable (LB/RB buttons) | - -### Incremental vs Absolute Control - -**Target Pose Control** (Absolute): -```python -# Direct target assignment -self.target_pos = np.array([0.5, 0.2, 0.6]) # Go exactly here -``` - -**Gamepad Control** (Incremental): -```python -# Relative movement from current target -dx = gamepad_input * speed_scale * 0.1 -self.target_pos += np.array([dx, dy, dz]) # Move relative to current -``` - -### Deadzone Implementation - -```python -def apply_deadzone(self, value, deadzone=0.05): - """Prevent controller drift by ignoring small inputs.""" - return 0.0 if abs(value) < deadzone else value -``` - -**Why needed**: Analog sticks rarely return exactly 0.0, causing unwanted drift. - -## Extension Ideas - -### Easy Extensions -1. **Add Orientation Control**: - ```python - # Use right stick X for yaw rotation - dyaw = self.apply_deadzone(axes[2]) * self.speed_scale * 0.1 - self.target_rpy[2] += dyaw # Update yaw angle - ``` - -2. **Workspace Limits with FK Validation**: - ```python - # Validate target position is reachable using PyTorch Kinematics - def validate_target_position(self, target_pos): - # Use FK to check if any joint configuration can reach target - # Could use IK solver to verify reachability - pass - ``` - -3. **Custom Button Mapping**: - ```python - # Load button mappings from config file - button_config = { - 'gripper_close': 'X', - 'gripper_open': 'Y', - 'speed_up': 'RB', - 'home_reset': 'START' - } - ``` - -### Advanced Extensions -1. **Force Feedback**: Rumble controller when approaching limits or singularities -2. **Multi-Robot Control**: Leverage PyTorch Kinematics batch processing for multiple arms -3. **Recording/Playback**: Record gamepad sessions with precise end-effector trajectories -4. **Learning Integration**: Use PyTorch's autodiff for learning-based assistance -5. **Collision Avoidance**: Integrate with [pytorch-volumetric](https://github.com/UM-ARM-Lab/pytorch_volumetric) for SDF-based collision checking - -### Multi-Modal Input Example -```python -def main(): - # Controller can handle both gamepad and programmatic input - for event in node: - if event["id"] == "raw_control": - # Gamepad input - incremental control - controller.process_gamepad_input(raw_control) - elif event["id"] == "target_pose": - # Programmatic input - absolute positioning - controller.process_target_pose(target_pose) - - # Same differential IK handles both input types seamlessly - commands = controller.apply_cartesian_control(joint_positions) -``` - -## Troubleshooting - -### "Gamepad not detected" -```bash -# Check if gamepad is connected -ls /dev/input/js* - -# Test gamepad input -jstest /dev/input/js0 -``` - -### "Robot doesn't respond to gamepad" -- Check that gamepad node is outputting `raw_control` data -- Verify controller is receiving gamepad events -- Ensure PyTorch Kinematics is properly initialized - - -### "Slow response / choppy movement" -- Enable GPU acceleration: check `torch.cuda.is_available()` -- Reduce gamepad polling rate if CPU-limited -- Profile FK/Jacobian computation times - -### "Robot moves erratically with gamepad" -- Adjust deadzone: increase `self.deadzone = 0.1` for less sensitive sticks -- Reduce speed scale: lower `self.speed_scale = 0.2` for finer control -- Check for controller drift: test with `jstest` - -## Performance Notes - -- **Real-time Control**: PyTorch Kinematics enables smooth 100Hz gamepad response -- **GPU Acceleration**: Significant speedup for FK/Jacobian computation -- **Memory Efficiency**: Minimal memory overhead compared to dual MuJoCo -- **Scalability**: Could theoretically control multiple robots with one gamepad - -## Next Steps - -This gamepad control example demonstrates a complete teleoperation system with modern kinematics computation. Consider exploring: - -- **Direct Robot Control**: Replace MuJoCo with real robot drivers (FrankaEmika SDK) -- **Advanced Input Devices**: 3D mice, haptic devices, VR controllers -- **Autonomous + Manual**: Blend autonomous behaviors with manual override -- **Multi-Modal Control**: Voice commands, gesture recognition, eye tracking -- **Learning-Based Assistance**: Use PyTorch for adaptive control behaviors -- **Collaborative Control**: Multiple operators controlling different aspects diff --git a/examples/franka-mujoco-control/04_gamepad_collision_avoidance/README.md b/examples/franka-mujoco-control/04_gamepad_collision_avoidance/README.md deleted file mode 100644 index 8b147d07..00000000 --- a/examples/franka-mujoco-control/04_gamepad_collision_avoidance/README.md +++ /dev/null @@ -1,139 +0,0 @@ -# Franka Real-Time Collision Avoidance Controller - -Franka Panda robot control with gamepad input and real-time self-collision avoidance using repulsive forces. The robot provides smooth, collision-aware motion. - -## Features - -- **Real-Time Gamepad Control**: Intuitive joystick control for precise robot movement -- **Self-Collision Avoidance**: Continuous collision detection and avoidance using repulsive forces -- **PyTorch Kinematics**: GPU-accelerated forward/inverse kinematics for high performance -- **Smooth Motion**: Natural robot behavior with collision-aware motion planning - -## Controls - -### Gamepad Layout -- **Left Stick X/Y**: Move end-effector in X/Y plane -- **Right Stick Y**: Move end-effector up/down (Z axis) -- **X Button**: Close gripper -- **Y Button**: Open gripper -- **LB/RB**: Decrease/Increase movement speed (0.1x - 1.0x) -- **START**: Reset to home position with downward orientation - -## Collision Avoidance System - -The system implements a sophisticated collision avoidance algorithm: - -### Real-Time Detection -- **Continuous Monitoring**: Checks for potential self-collisions every control cycle -- **Geometric Collision Pairs**: Monitors 18 non-adjacent link pairs for potential collisions -- **Distance-Based Forces**: Generates repulsive forces based on proximity to collision - -### Repulsive Force Model -```python -# Force magnitude calculation -if distance < min_safe_distance: - force = collision_gain * (1/distance - 1/min_safe_distance) -else: - force = 0 # No force outside influence zone -``` - -### Key Parameters -- **`min_link_distance`**: 0.02m - Minimum safe distance between robot links -- **`collision_force_gain`**: 1000.0 - Strength of repulsive forces -- **`collision_influence_distance`**: 0.05m - Distance at which collision avoidance activates - -## Installation - -### Prerequisites -```bash -# Install required Python packages -pip install pytorch-kinematics scipy numpy torch pyarrow dora-rs -``` - -### Running the System -```bash -# Navigate to the project directory -cd /path/to/dora-rs/examples/franka-mujoco-control/04_gamepad_collision_avoidance/ - -# Start the Dora runtime -dora up - -# Launch the collision avoidance system -dora start collision_avoidance.yml -``` - -## System Architecture - - - -### Collision Detection Pipeline -1. **Forward Kinematics**: Calculate all link positions using PyTorch -2. **Distance Calculation**: Check distances between all collision pairs -3. **Force Generation**: Generate repulsive forces for close pairs -4. **Force Integration**: Convert Cartesian forces to joint space -5. **Motion Blending**: Combine tracking and collision avoidance motions - -## Configuration - -### Controller Parameters -```python -# Control gains -Kpos = 0.95 # Position control gain -Kori = 0.95 # Orientation control gain -max_angvel = 0.785 # Maximum joint velocity (rad/s) - -# Collision avoidance -min_link_distance = 0.02 # 2cm minimum safe distance -collision_force_gain = 1000.0 # Repulsive force strength -collision_influence_distance = 0.05 # 5cm activation distance -``` - -### Link Geometry Configuration -The system models each robot link as a cylinder with specified radius and offset: -```python -link_geometries = { - 'panda_link1': {'radius': 0.08, 'offset': [0, 0, 0.075]}, - 'panda_link2': {'radius': 0.08, 'offset': [0, 0, 0]}, - # ... etc for all 7 links + hand -} -``` - -## Troubleshooting - -### Common Issues -1. **Robot Not Moving**: Check gamepad connection and button mappings -2. **Jerky Motion**: Reduce collision force gains -3. **GPU Errors**: Ensure CUDA-compatible PyTorch installation -4. **Joint Limits**: Robot automatically respects joint limits and stops at boundaries - -### Debug Information -The system provides real-time collision status: -```bash -# Console output shows active collisions -Active collisions: panda_link1<->panda_link4: 0.03m; panda_link2<->panda_link5: 0.04m -Collision forces cleared - normal motion resumed -``` - -## Performance Notes - -- **Control Frequency**: 500Hz for smooth, responsive control -- **Collision Detection**: Runs every control cycle (no artificial delays) -- **GPU Utilization**: Automatic GPU acceleration when CUDA available -- **Memory Usage**: Efficient tensor operations minimize memory footprint - -## Safety Features - -- **Joint Limit Protection**: Automatic velocity limiting near joint boundaries -- **Velocity Clamping**: Maximum joint velocities enforced at all times -- **Self-Collision Avoidance**: Repulsive force approach to avoid collision diff --git a/examples/franka-mujoco-control/04_gamepad_collision_avoidance/collision_avoidance.yml b/examples/franka-mujoco-control/04_gamepad_collision_avoidance/collision_avoidance.yml deleted file mode 100644 index a14d6356..00000000 --- a/examples/franka-mujoco-control/04_gamepad_collision_avoidance/collision_avoidance.yml +++ /dev/null @@ -1,35 +0,0 @@ -nodes: - - id: gamepad - build: pip install -e ../../../node-hub/gamepad - path: gamepad - outputs: - - cmd_vel - - raw_control - inputs: - tick: dora/timer/millis/10 - - - - id: mujoco_sim - build: pip install -e ../../../node-hub/dora-mujoco - path: dora-mujoco - inputs: - tick: dora/timer/millis/2 # 500 Hz simulation - control_input: franka_collision_controller/joint_commands - outputs: - - joint_positions - - joint_velocities - - actuator_controls - - sensor_data - env: - MODEL_NAME: "panda" - - - id: franka_collision_controller - path: nodes/franka_collision_controller.py - inputs: - tick: dora/timer/millis/2 - joint_positions: mujoco_sim/joint_positions - raw_control: gamepad/raw_control - outputs: - - joint_commands - - ee_position - - collision_status diff --git a/examples/franka-mujoco-control/04_gamepad_collision_avoidance/nodes/franka_collision_controller.py b/examples/franka-mujoco-control/04_gamepad_collision_avoidance/nodes/franka_collision_controller.py deleted file mode 100644 index 4e5e5d83..00000000 --- a/examples/franka-mujoco-control/04_gamepad_collision_avoidance/nodes/franka_collision_controller.py +++ /dev/null @@ -1,366 +0,0 @@ -"""Enhanced Franka controller with gamepad support and collision avoidance using repulsive forces.""" - -import json -import time -import numpy as np -import pyarrow as pa -import torch -from dora import Node -import pytorch_kinematics as pk -from scipy.spatial.transform import Rotation - -class FrankaCollisionController: - """Franka controller with gamepad support and self-collision avoidance using repulsive forces.""" - - def __init__(self): - """Initialize controller with PyTorch Kinematics and collision avoidance.""" - # Load the robot model for IK and FK - urdf_path = "../franka_panda/panda.urdf" - with open(urdf_path, 'rb') as f: - urdf_content = f.read() - - # Build serial chain for kinematics - self.chain = pk.build_serial_chain_from_urdf(urdf_content, "panda_hand") - - # Move to GPU if available - self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - self.chain = self.chain.to(device=self.device) - - # Control parameters - self.integration_dt = 0.1 - self.damping = 1e-4 - self.Kpos = 0.95 - self.Kori = 0.95 - self.Kn = np.array([10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0]) - self.max_angvel = 0.785 - - # Collision avoidance parameters - self.collision_check_enabled = True - self.min_link_distance = 0.02 - self.collision_force_gain = 1000.0 - self.collision_influence_distance = 0.05 - - # Define which links can collide with each other (non-adjacent only) - self.collision_pairs = [ - ('panda_link1', 'panda_link3'), ('panda_link1', 'panda_link4'), ('panda_link1', 'panda_link5'), ('panda_link1', 'panda_link6'), ('panda_link1', 'panda_link7'), - ('panda_link1', 'panda_hand'), ('panda_link2', 'panda_link4'), ('panda_link2', 'panda_link5'), ('panda_link2', 'panda_link6'), ('panda_link2', 'panda_link7'), - ('panda_link2', 'panda_hand'), ('panda_link3', 'panda_link5'), ('panda_link3', 'panda_link6'), ('panda_link3', 'panda_link7'), ('panda_link3', 'panda_hand'), - ('panda_link4', 'panda_link6'), ('panda_link4', 'panda_link7'), ('panda_link4', 'panda_hand'), - # ('panda_link5', 'panda_hand'), # Enabling this results in very jaggy motion - ] - - # Approximate link geometries for distance calculation - self.link_geometries = { - 'panda_link1': {'radius': 0.08, 'length': 0.15, 'offset': np.array([0, 0, 0.075])}, - 'panda_link2': {'radius': 0.08, 'length': 0.12, 'offset': np.array([0, 0, 0])}, - 'panda_link3': {'radius': 0.07, 'length': 0.32, 'offset': np.array([0, 0, 0.16])}, - 'panda_link4': {'radius': 0.07, 'length': 0.28, 'offset': np.array([0, 0, -0.14])}, - 'panda_link5': {'radius': 0.06, 'length': 0.22, 'offset': np.array([0, 0, 0.11])}, - 'panda_link6': {'radius': 0.06, 'length': 0.12, 'offset': np.array([0, 0, 0.06])}, - 'panda_link7': {'radius': 0.05, 'length': 0.08, 'offset': np.array([0, 0, 0.04])}, - 'panda_hand': {'radius': 0.07, 'length': 0.07, 'offset': np.array([0, 0, 0.035])}, - } - - # Gamepad control parameters - self.speed_scale = 0.5 - self.deadzone = 0.05 - - # Gripper control parameters - self.gripper_range = [0, 255] - self.gripper_state = 0.0 - - # Robot state - self.current_joint_pos = None - self.target_pos = np.array([0.55, 0.0, 0.6]) - self.target_rpy = [180.0, 0.0, 90.0] - self.home_pos = np.array([0, 0, 0, -1.57079, 0, 1.57079, -0.7853]) - - # Collision state tracking - self.is_in_collision_risk = False - self.collision_force_applied_count = 0 - self.current_collision_info = "" - - # print("Franka Collision Controller initialized") - # print(f"Chain DOF: {len(self.chain.get_joint_parameter_names())}") - # print(f"Using device: {self.device}") - - def calculate_collision_repulsive_forces(self, joint_angles): - """Calculate repulsive forces to avoid collisions between robot links.""" - if not self.collision_check_enabled: - return np.zeros(7), False, "" - - # Get forward kinematics for all links - q_tensor = torch.tensor(joint_angles, device=self.device, dtype=torch.float32).unsqueeze(0) - fk_results = self.chain.forward_kinematics(q_tensor, end_only=False) - - # Initialize total repulsive force in joint space - total_repulsive_forces = np.zeros(7) - collision_detected = False - collision_info_list = [] - - # Check each collision pair - for link1, link2 in self.collision_pairs: - if link1 not in fk_results or link2 not in fk_results: - continue - - # Get link transforms - transform1 = fk_results[link1].get_matrix().squeeze(0).cpu().numpy() - transform2 = fk_results[link2].get_matrix().squeeze(0).cpu().numpy() - - # Get link center positions - link1_info, link2_info = self.link_geometries[link1], self.link_geometries[link2] - offset1, offset2 = link1_info['offset'], link2_info['offset'] - radius1, radius2 = link1_info['radius'], link2_info['radius'] - - # Transform offsets to world coordinates - pos1 = transform1[:3, 3] + transform1[:3, :3] @ offset1 - pos2 = transform2[:3, 3] + transform2[:3, :3] @ offset2 - - # Calculate distance between link centers - distance = np.linalg.norm(pos1 - pos2) - - # Check if within influence range - min_safe_distance = radius1 + radius2 + self.min_link_distance - influence_distance = min_safe_distance + self.collision_influence_distance - - if distance < influence_distance and distance > 1e-6: - collision_detected = True - - # Calculate repulsive force magnitude - if distance < min_safe_distance: - force_magnitude = self.collision_force_gain * (1.0/max(distance, 0.01) - 1.0/min_safe_distance) - else: - force_magnitude = 0 - - # Direction of repulsive force - if distance > 1e-6: - force_direction = (pos1 - pos2) / distance - else: - force_direction = np.array([1, 0, 0]) - - # Convert Cartesian repulsive force to joint space forces - link1_forces = self.cartesian_to_joint_forces(link1, pos1, force_direction * force_magnitude, joint_angles) - link2_forces = self.cartesian_to_joint_forces(link2, pos2, -force_direction * force_magnitude, joint_angles) - - total_repulsive_forces += link1_forces + link2_forces - - # Store collision info - collision_info_list.append(f"{link1}<->{link2}: {distance:.2f}m") - - collision_info = "; ".join(collision_info_list) if collision_info_list else "No collision" - return total_repulsive_forces, collision_detected, collision_info - - def cartesian_to_joint_forces(self, link_name, link_position, cartesian_force, joint_angles): - """Convert Cartesian force at a link to joint space forces.""" - # Get numerical Jacobian for this link position - jacobian = self.get_link_jacobian_simplified(link_name, joint_angles, link_position) - - # Convert Cartesian force to joint torques using Jacobian transpose - joint_forces = jacobian.T @ cartesian_force[:3] - - return joint_forces - - def get_link_jacobian_simplified(self, link_name, joint_angles, link_position): - """Get link Jacobian using numerical differentiation.""" - epsilon = 1e-6 - jacobian = np.zeros((3, 7)) - - # Calculate numerical Jacobian - for i in range(7): - # Perturb joint i - perturbed_joints = joint_angles.copy() - perturbed_joints[i] += epsilon - - # Get perturbed link position - q_perturbed = torch.tensor(perturbed_joints, device=self.device, dtype=torch.float32).unsqueeze(0) - fk_perturbed = self.chain.forward_kinematics(q_perturbed, end_only=False) - - if link_name in fk_perturbed: - transform_perturbed = fk_perturbed[link_name].get_matrix().squeeze(0).cpu().numpy() - offset = self.link_geometries[link_name]['offset'] - perturbed_pos = transform_perturbed[:3, 3] + transform_perturbed[:3, :3] @ offset - - # Numerical derivative - jacobian[:, i] = (perturbed_pos - link_position) / epsilon - - return jacobian - - def apply_deadzone(self, value): - """Apply deadzone to joystick input.""" - return 0.0 if abs(value) < self.deadzone else value - - def get_current_ee_pose(self, joint_angles): - """Get current end-effector pose.""" - q = torch.tensor(joint_angles, device=self.device, dtype=torch.float32).unsqueeze(0) - tf = self.chain.forward_kinematics(q) - transform_matrix = tf.get_matrix().squeeze(0).cpu().numpy() - position = transform_matrix[:3, 3] - rotation_matrix = transform_matrix[:3, :3] - return position, rotation_matrix - - def compute_jacobian_pytorch(self, joint_angles): - """Compute Jacobian using PyTorch Kinematics.""" - q = torch.tensor(joint_angles, device=self.device, dtype=torch.float32).unsqueeze(0) - J = self.chain.jacobian(q) - return J.squeeze(0).cpu().numpy() - - def process_gamepad_input(self, raw_control): - """Process gamepad input.""" - - axes = raw_control["axes"] - buttons = raw_control["buttons"] - - # Reset position with START button - if len(buttons) > 9 and buttons[9]: - if self.current_joint_pos is not None: - home_ee_pos, _ = self.get_current_ee_pose(self.home_pos) - self.target_pos = home_ee_pos.copy() - print("Reset target to home position") - - # Gripper control with X and Y buttons - if len(buttons) > 0 and buttons[0]: - self.gripper_state = 0.0 - print("Gripper: CLOSED") - elif len(buttons) > 3 and buttons[3]: - self.gripper_state = 1.0 - print("Gripper: OPEN") - - # Speed scaling with bumpers (LB/RB) - if len(buttons) > 4 and buttons[4]: - self.speed_scale = max(0.1, self.speed_scale - 0.1) - print(f"Speed: {self.speed_scale:.1f}") - if len(buttons) > 5 and buttons[5]: - self.speed_scale = min(1.0, self.speed_scale + 0.1) - print(f"Speed: {self.speed_scale:.1f}") - - # Get cartesian control inputs with deadzone - dx = self.apply_deadzone(axes[0]) * self.speed_scale * 0.1 - dy = -self.apply_deadzone(axes[1]) * self.speed_scale * 0.1 - dz = -self.apply_deadzone(axes[3]) * self.speed_scale * 0.1 - - # Update target position incrementally - if abs(dx) > 0 or abs(dy) > 0 or abs(dz) > 0: - self.target_pos += np.array([dx, dy, dz]) - - def get_target_rotation_matrix(self): - """Convert RPY to rotation matrix.""" - roll_rad, pitch_rad, yaw_rad = np.radians(self.target_rpy) - desired_rot = Rotation.from_euler('ZYX', [yaw_rad, pitch_rad, roll_rad]) - return desired_rot.as_matrix() - - def apply_cartesian_control(self, current_joints): - """Apply Cartesian control with collision avoidance using repulsive forces.""" - self.current_joint_pos = current_joints[:7] - - # Get current end-effector pose - current_ee_pos, current_ee_rot = self.get_current_ee_pose(self.current_joint_pos) - - # Calculate position error - pos_error = self.target_pos - current_ee_pos - - # Construct 6D twist - twist = np.zeros(6) - twist[:3] = self.Kpos * pos_error / self.integration_dt - - # Orientation control - current_rot = Rotation.from_matrix(current_ee_rot) - desired_rot = Rotation.from_matrix(self.get_target_rotation_matrix()) - rot_error = (desired_rot * current_rot.inv()).as_rotvec() - twist[3:] = self.Kori * rot_error / self.integration_dt - - # Get Jacobian - jac = self.compute_jacobian_pytorch(self.current_joint_pos) - - # Damped least squares inverse kinematics - diag = self.damping * np.eye(6) - dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist) - - # Nullspace control - jac_pinv = np.linalg.pinv(jac) - N = np.eye(7) - jac_pinv @ jac - dq_null = self.Kn * (self.home_pos - self.current_joint_pos) - dq += N @ dq_null - - # Collision avoidance - collision_forces, collision_detected, collision_info = self.calculate_collision_repulsive_forces(self.current_joint_pos) - - if collision_detected: - dq += collision_forces * self.integration_dt - self.is_in_collision_risk = True - self.current_collision_info = collision_info - # print(f"Active collisions: {collision_info}") - else: - if self.is_in_collision_risk: - print("Collision forces cleared - normal motion resumed") - self.is_in_collision_risk = False - self.current_collision_info = "Safe" - - # Limit joint velocities - dq_abs_max = np.abs(dq).max() - if dq_abs_max > self.max_angvel: - dq *= self.max_angvel / dq_abs_max - - # Integrate to get new joint positions - new_joints = self.current_joint_pos + dq * self.integration_dt - - # Apply joint limits - joint_limits = np.array([[-2.8973, 2.8973], [-1.7628, 1.7628], [-2.8973, 2.8973], [-3.0718, -0.0698], [-2.8973, 2.8973], [-0.0175, 3.7525], [-2.8973, 2.8973]]) - - for i in range(7): - new_joints[i] = np.clip(new_joints[i], joint_limits[i][0], joint_limits[i][1]) - - # Return commands - full_commands = np.zeros(8) - full_commands[:7] = new_joints - full_commands[7] = self.gripper_range[0] if self.gripper_state < 0.5 else self.gripper_range[1] - - return full_commands - - -def main(): - """Main controller loop.""" - node = Node("franka_collision_controller") - controller = FrankaCollisionController() - - print("Franka Collision Controller Node Started") - - for event in node: - if event["type"] == "INPUT": - - if event["id"] == "joint_positions": - joint_positions = event["value"].to_numpy() - full_commands = controller.apply_cartesian_control(joint_positions) - - node.send_output( - "joint_commands", - pa.array(full_commands, type=pa.float64()), - metadata={"timestamp": time.time()} - ) - - if controller.current_joint_pos is not None: - current_ee_pos, _ = controller.get_current_ee_pose(controller.current_joint_pos) - node.send_output( - "ee_position", - pa.array(current_ee_pos, type=pa.float64()), - metadata={"timestamp": time.time()} - ) - - # Send collision status - collision_status = { - "collision_risk": controller.is_in_collision_risk, - "collision_info": controller.current_collision_info, - "force_applied_count": controller.collision_force_applied_count - } - node.send_output( - "collision_status", - pa.array([1.0 if controller.is_in_collision_risk else 0.0], type=pa.float64()), - metadata={"collision_status": json.dumps(collision_status)} - ) - - elif event["id"] == "raw_control": - raw_control = json.loads(event["value"].to_pylist()[0]) - controller.process_gamepad_input(raw_control) - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/examples/franka-mujoco-control/README.md b/examples/franka-mujoco-control/README.md deleted file mode 100644 index aef13e42..00000000 --- a/examples/franka-mujoco-control/README.md +++ /dev/null @@ -1,46 +0,0 @@ -# Franka MuJoCo Control Tutorial - -This comprehensive tutorial demonstrates how to build a modular robot control system using Dora's dataflow architecture with the [`dora-mujoco`](../../node-hub/dora-mujoco) simulation node and robot-specific control logic. - -## Tutorial Structure - -### [01. Basic Simulation](01_basic_simulation/) -Load and run a Franka Panda simulation using the `dora-mujoco` node. -- Learn the fundamentals of MuJoCo simulation in Dora -- Understand the simulation node architecture -- See how robot descriptions are loaded automatically - -### [02. Target Pose Control](02_target_pose_control/) -Add robot-specific control logic with programmatic pose commands. -- Implement Cartesian space control with inverse kinematics -- Create a dedicated Franka controller node -- Send target poses programmatically - -### [03. Gamepad Control](03_gamepad_control/) -Connect a gamepad for real-time interactive control. -- Integrate with the existing `gamepad` node -- Process raw gamepad input into robot commands -- Demonstrate real-time teleoperation - - -## Quick Start - -```bash -cd examples/franka-mujoco-control - -# 1. Basic simulation -cd 01_basic_simulation -dora build basic.yml -dora run basic.yml - -# 2. Target pose control -cd ../02_target_pose_control -dora build target_pose_control.yml -dora run target_pose_control.yml - -# 3. Gamepad control -cd ../03_gamepad_control -dora build gamepad_control.yml -dora run gamepad_control.yml -``` - diff --git a/examples/mujoco-sim/README.md b/examples/mujoco-sim/README.md new file mode 100644 index 00000000..4aa38255 --- /dev/null +++ b/examples/mujoco-sim/README.md @@ -0,0 +1,21 @@ +# Franka MuJoCo Control Tutorial + +This comprehensive tutorial demonstrates how to build a robot control system using Dora with the `dora-mujoco` simulation node and control logic. + +## Tutorial Structure + +### [01. Basic Simulation](basic_simulation/) +Load and run a Franka Panda simulation using the `dora-mujoco` node. +- Learn the fundamentals of MuJoCo simulation in Dora +- Understand the simulation node architecture +- See how robot descriptions are loaded automatically + +### [02. Target Pose Control](target_pose_control/) +Add control logic with pose commands as target. +- Implement Cartesian space control. +- Create generic controller node that is able to control any robotic arm by using `dora-pytorch-kinematics` + +### [03. Gamepad Control](gamepad_control/) +Connect a gamepad for real-time interactive control. +- Integrate with dora's `gamepad` node +- Demonstrate real-time teleoperation diff --git a/examples/franka-mujoco-control/franka_panda/panda.urdf b/examples/mujoco-sim/URDF/franka_panda/panda.urdf similarity index 100% rename from examples/franka-mujoco-control/franka_panda/panda.urdf rename to examples/mujoco-sim/URDF/franka_panda/panda.urdf diff --git a/examples/franka-mujoco-control/kuka_iiwa/model.urdf b/examples/mujoco-sim/URDF/kuka_iiwa/model.urdf similarity index 100% rename from examples/franka-mujoco-control/kuka_iiwa/model.urdf rename to examples/mujoco-sim/URDF/kuka_iiwa/model.urdf diff --git a/examples/franka-mujoco-control/01_basic_simulation/README.md b/examples/mujoco-sim/basic_simulation/README.md similarity index 71% rename from examples/franka-mujoco-control/01_basic_simulation/README.md rename to examples/mujoco-sim/basic_simulation/README.md index 828bef00..a3709ab0 100644 --- a/examples/franka-mujoco-control/01_basic_simulation/README.md +++ b/examples/mujoco-sim/basic_simulation/README.md @@ -6,11 +6,6 @@ This example demonstrates the simplest possible setup: loading and running a Fra - See how robot models are loaded from `robot-descriptions` - Learn the basic dataflow for physics simulation -## Architecture - -``` -[Timer] → [MuJoCo Sim] → [Joint Positions, Velocities, Sensor Data] -``` The simulation runs at 500Hz and outputs: - Joint positions for all robot joints @@ -27,13 +22,12 @@ dora run basic.yml ``` You should see: -1. MuJoCo viewer window opens with Franka Panda robot +1. MuJoCo viewer window opens with GO2 robot 2. Robot is effected by gravity (enabled by default) -3. Console output showing node activity ## What's Happening -1. **Model Loading**: The `dora-mujoco` node loads the Franka model using `load_robot_description("panda_mj_description")` +1. **Model Loading**: The `dora-mujoco` node loads the Franka model using `load_robot_description("go2_mj_description")` 2. **Physics Loop**: Timer triggers simulation steps at 500Hz (This is default step time for Mujoco) 3. **Data Output**: Joint states are published 4. **Visualization**: MuJoCo viewer shows real-time simulation @@ -41,6 +35,6 @@ You should see: ## Configuration Details The `basic.yml` configures: -- Model name: `"panda"` (resolved to `panda_mj_description`) -- Update rate: 2ms (500Hz) +- Model name: `"go2"` you change this to other robots name +- Update rate: 2ms (500Hz) - Outputs: Joint positions, velocities, and sensor data \ No newline at end of file diff --git a/examples/franka-mujoco-control/01_basic_simulation/basic.yml b/examples/mujoco-sim/basic_simulation/basic.yml similarity index 78% rename from examples/franka-mujoco-control/01_basic_simulation/basic.yml rename to examples/mujoco-sim/basic_simulation/basic.yml index ffa08d05..3bb8c3e3 100644 --- a/examples/franka-mujoco-control/01_basic_simulation/basic.yml +++ b/examples/mujoco-sim/basic_simulation/basic.yml @@ -9,4 +9,4 @@ nodes: - joint_velocities - sensor_data env: - MODEL_NAME: "panda" # Load Franka Panda from robot-descriptions \ No newline at end of file + MODEL_NAME: "go2" # Load GO2 \ No newline at end of file diff --git a/examples/mujoco-sim/gamepad_control/README.md b/examples/mujoco-sim/gamepad_control/README.md new file mode 100644 index 00000000..ecc22728 --- /dev/null +++ b/examples/mujoco-sim/gamepad_control/README.md @@ -0,0 +1,75 @@ +# 03. Gamepad Control + +This example demonstrates real-time interactive control by connecting a gamepad to the controller. It builds upon the target pose control example by adding gamepad input processing. Shows how to integrate and implement teleoperation. + +### Gamepad Controls: +- **D-pad Vertical**: Move along X-axis (forward/backward) +- **D-pad Horizontal**: Move along Y-axis (left/right) +- **Right Stick Y**: Move along Z-axis (up/down) +- **LB/RB**: Decrease/Increase movement speed (0.1-1.0x scale) +- **START**: Reset to home position + +## Running the Example + +1. **Connect a gamepad** (Xbox/PlayStation controller via USB or Bluetooth) +2. **Run the simulation**: +```bash +cd 03_gamepad_control +dora build gamepad_control.yml +dora run gamepad_control.yml +``` + +You should see: +1. Robot responds to gamepad input in real-time +2. Smooth incremental movement based on D-pad/stick input +3. Speed control with bumper buttons +4. Reset functionality with START button +5. GPU-accelerated kinematics computation (if CUDA available) + +#### **Gamepad Node** (`gamepad`) +Built-in Dora node that interfaces with system gamepad drivers. +- **Outputs**: + - `cmd_vel`: 6DOF velocity commands `[linear_x, linear_y, linear_z, angular_x, angular_y, angular_z]` + - `raw_control`: Gamepad Input in Json format + +#### **GamePad Controller** (`gamepad_controller.py`) +This node processes gamepad input and translates it into target positions for the robot end-effector. + +- Receives continuous movement commands (`cmd_vel`) from D-pad and analog sticks +- Processes discrete button presses (`raw_control`) for special functions + +**Controlling the End-Effector with Gamepad**: + +The controller takes the first 3 values (X, Y, Z movement) from the gamepad `cmd_vel`, updates Target Position continuously + +**Button Commands**: + - **START button**: resets end-effector to home position [0.4, 0.0, 0.3] + - **Left Bumper (LB)**: Decreases movement speed + - **Right Bumper (RB)**: Increases movement speed + +The end-effector moves smoothly as you hold the controls, with position updates sent to the inverse kinematics solver to calculate required joint angles. + +## Troubleshooting + +**"Gamepad not responding"** +```bash +# Check if gamepad is connected +ls /dev/input/js* +# Test gamepad input +jstest /dev/input/js0 +``` + +**"Robot doesn't move with D-pad"** +- Check `cmd_vel` output: should show non-zero values when D-pad pressed +- Verify controller processes `cmd_vel` events +- Ensure your gamepad has correct mapping. + + +## Real Robot Deployment + +For actual robot control: +1. **Replace MuJoCo**: Use real robot drivers +2. **Safety Limits**: Add emergency stops and workspace bounds +3. **Force Control**: Integrate force/torque feedback +4. **Network Latency**: Consider wireless controller delay +5. **Deadman Switch**: Require constant button hold for safety diff --git a/examples/franka-mujoco-control/03_gamepad_control/gamepad_control.yml b/examples/mujoco-sim/gamepad_control/gamepad_control.yml similarity index 76% rename from examples/franka-mujoco-control/03_gamepad_control/gamepad_control.yml rename to examples/mujoco-sim/gamepad_control/gamepad_control.yml index e62cac4b..bfbd0c19 100644 --- a/examples/franka-mujoco-control/03_gamepad_control/gamepad_control.yml +++ b/examples/mujoco-sim/gamepad_control/gamepad_control.yml @@ -13,7 +13,7 @@ nodes: path: dora-mujoco inputs: tick: dora/timer/millis/2 # 500 Hz simulation - control_input: franka_gamepad_controller/joint_commands + control_input: gamepad_controller/joint_commands outputs: - joint_positions - joint_velocities @@ -22,8 +22,8 @@ nodes: env: MODEL_NAME: "kuka_iiwa14" - - id: franka_gamepad_controller - path: nodes/franka_gamepad_controller.py + - id: gamepad_controller + path: nodes/gamepad_controller.py inputs: joint_positions: mujoco_sim/joint_positions joint_velocities: mujoco_sim/joint_velocities @@ -40,12 +40,12 @@ nodes: build: pip install -e ../../../node-hub/dora-pytorch-kinematics path: dora-pytorch-kinematics inputs: - fk_request: franka_gamepad_controller/fk_request - jacobian_request: franka_gamepad_controller/jacobian_request + fk_request: gamepad_controller/fk_request + jacobian_request: gamepad_controller/jacobian_request outputs: - fk_request - jacobian_request env: - URDF_PATH: "/home/shashwatgpatil/Dora-rs/dora/examples/franka-mujoco-control/kuka_iiwa/model.urdf" + URDF_PATH: "../URDF/kuka_iiwa/model.urdf" END_EFFECTOR_LINK: "lbr_iiwa_link_7" TRANSFORM: "0. 0. 0. 1. 0. 0. 0." diff --git a/examples/franka-mujoco-control/03_gamepad_control/nodes/franka_gamepad_controller.py b/examples/mujoco-sim/gamepad_control/nodes/gamepad_controller.py similarity index 95% rename from examples/franka-mujoco-control/03_gamepad_control/nodes/franka_gamepad_controller.py rename to examples/mujoco-sim/gamepad_control/nodes/gamepad_controller.py index ae8ba8e2..478c1bbb 100644 --- a/examples/franka-mujoco-control/03_gamepad_control/nodes/franka_gamepad_controller.py +++ b/examples/mujoco-sim/gamepad_control/nodes/gamepad_controller.py @@ -12,10 +12,10 @@ class GamepadFrankaController: self.damping = 1e-4 self.Kpos = 0.95 # Position gain self.Kori = 0.95 # Orientation gain - self.max_angvel = 1.57 # Max joint velocity (rad/s) # Gamepad control parameters self.speed_scale = 0.5 + self.max_angvel = 3.14 * self.speed_scale # Max joint velocity (rad/s) # Robot state variables self.dof = None @@ -39,14 +39,13 @@ class GamepadFrankaController: self.home_pos = np.zeros(self.dof) def process_cmd_vel(self, cmd_vel): - print(f"Processing cmd_vel: {cmd_vel}") - dx = cmd_vel[0] * self.speed_scale * 0.1 - dy = cmd_vel[1] * self.speed_scale * 0.1 - dz = cmd_vel[2] * self.speed_scale * 0.1 + # print(f"Processing cmd_vel: {cmd_vel}") + delta = cmd_vel[:3] * 0.03 + dx, dy, dz = delta # Update target position incrementally if abs(dx) > 0 or abs(dy) > 0 or abs(dz) > 0: - self.target_pos += np.array([dx, dy, dz]) + self.target_pos += np.array([dx, -dy, dz]) def process_gamepad_input(self, raw_control): buttons = raw_control["buttons"] diff --git a/examples/mujoco-sim/target_pose_control/README.md b/examples/mujoco-sim/target_pose_control/README.md new file mode 100644 index 00000000..0c68c91e --- /dev/null +++ b/examples/mujoco-sim/target_pose_control/README.md @@ -0,0 +1,99 @@ +# 02. Target Pose Control + +This example demonstrates Cartesian space control by creating a Controller node that processes target pose commands and outputs joint commands using inverse kinematics using **dora-pytorch-kinematics**. + +## Running the Example + +```bash +cd 02_target_pose_control +dora build target_pose_control_pytorch.yml +dora run target_pose_control_pytorch.yml +``` + +You should see: +1. Robot moves to predefined target poses automatically +2. Smooth Cartesian space motion with differential inverse kinematics +3. End-effector following target positions accurately + + +### Nodes + +#### 1. **Pose Publisher Script** (`pose_publisher.py`) +```python +class PosePublisher: + def __init__(self): + # Predefined sequence of target poses [x, y, z, roll, pitch, yaw] + self.target_poses = [ + [0.5, 0.5, 0.3, 180.0, 0.0, 90.0], # Position + RPY orientation + [0.6, 0.2, 0.5, 180.0, 0.0, 45.0], # Different orientation + # ... more poses + ] +``` + +- Sends target poses every 10 seconds +- Cycles through predefined positions and orientations +- Can be replaced with path planning, user input, or any pose generation logic +- Outputs `target_pose` array `[x, y, z, roll, pitch, yaw]` + +#### 2. **Controller Script** (`controller.py`) +```yaml + - id: controller + path: nodes/controller.py + inputs: + joint_positions: mujoco_sim/joint_positions + joint_velocities: mujoco_sim/joint_velocities + target_pose: pose_publisher/target_pose + fk_result: pytorch_kinematics/fk_request + jacobian_result: pytorch_kinematics/jacobian_request + outputs: + - joint_commands + - fk_request + - jacobian_request +``` +- **Input**: Takes target poses `[x, y, z, roll, pitch, yaw]` and current joint positions from `dora-mujoco` +- **Processing**: Differential inverse kinematics using `dora-pytorch-kinematics` to calculate actuator commands +- **Output**: Control/Actuator commands + +#### 3. **PyTorch Kinematics Node** (`dora-pytorch-kinematics`) + + +```yaml +- id: pytorch_kinematics + build: pip install -e ../../../node-hub/dora-pytorch-kinematics + path: dora-pytorch-kinematics + inputs: + fk_request: controller/fk_request + jacobian_request: controller/jacobian_request + outputs: + - fk_request + - jacobian_request + env: + URDF_PATH: "path to the robot urdf" # URDF is used to create the kinematics model for the robot + END_EFFECTOR_LINK: "name of the end effector" + TRANSFORM: "0. 0. 0. 1. 0. 0. 0." # Pytorch kinematics uses wxyz format. Robot transform from world frame + +``` + + Joint states performs Forward Kinematics, and returns End-effector pose along with jacobian matrix + +#### 4. **MuJoCo Simulation Node** (`dora-mujoco`) + +```yaml + + - id: mujoco_sim + build: pip install -e ../../../node-hub/dora-mujoco + path: dora-mujoco + inputs: + tick: dora/timer/millis/2 + control_input: controller/joint_commands + outputs: + - joint_positions + - joint_velocities + - actuator_controls + - sensor_data + env: + MODEL_NAME: "panda" # Name of the robot you want to load +``` +- **Input**: Joint commands from controller +- **Processing**: Physics simulation, rendering +- **Output**: Joint positions, velocities, sensor data \ No newline at end of file diff --git a/examples/franka-mujoco-control/02_target_pose_control/nodes/franka_controller_pytorch.py b/examples/mujoco-sim/target_pose_control/nodes/controller.py similarity index 100% rename from examples/franka-mujoco-control/02_target_pose_control/nodes/franka_controller_pytorch.py rename to examples/mujoco-sim/target_pose_control/nodes/controller.py diff --git a/examples/franka-mujoco-control/02_target_pose_control/nodes/pose_publisher.py b/examples/mujoco-sim/target_pose_control/nodes/pose_publisher.py similarity index 100% rename from examples/franka-mujoco-control/02_target_pose_control/nodes/pose_publisher.py rename to examples/mujoco-sim/target_pose_control/nodes/pose_publisher.py diff --git a/examples/franka-mujoco-control/02_target_pose_control/target_pose_control_pytorch.yml b/examples/mujoco-sim/target_pose_control/target_pose_control.yml similarity index 71% rename from examples/franka-mujoco-control/02_target_pose_control/target_pose_control_pytorch.yml rename to examples/mujoco-sim/target_pose_control/target_pose_control.yml index aa9d8fa5..e96871a1 100644 --- a/examples/franka-mujoco-control/02_target_pose_control/target_pose_control_pytorch.yml +++ b/examples/mujoco-sim/target_pose_control/target_pose_control.yml @@ -4,7 +4,7 @@ nodes: path: dora-mujoco inputs: tick: dora/timer/millis/2 - control_input: kuka_controller_pytorch/joint_commands + control_input: controller/joint_commands outputs: - joint_positions - joint_velocities @@ -13,8 +13,8 @@ nodes: env: MODEL_NAME: "panda" - - id: kuka_controller_pytorch - path: nodes/franka_controller_pytorch.py + - id: controller + path: nodes/controller.py inputs: joint_positions: mujoco_sim/joint_positions joint_velocities: mujoco_sim/joint_velocities @@ -30,19 +30,19 @@ nodes: build: pip install -e ../../../node-hub/dora-pytorch-kinematics path: dora-pytorch-kinematics inputs: - fk_request: kuka_controller_pytorch/fk_request - jacobian_request: kuka_controller_pytorch/jacobian_request + fk_request: controller/fk_request + jacobian_request: controller/jacobian_request outputs: - fk_request - jacobian_request env: - URDF_PATH: "/home/shashwatgpatil/Dora-rs/dora/examples/franka-mujoco-control/franka_panda/panda.urdf" + URDF_PATH: "../URDF/franka_panda/panda.urdf" END_EFFECTOR_LINK: "panda_hand" TRANSFORM: "0. 0. 0. 1. 0. 0. 0." # Pytorch kinematics uses wxyz format for quaternion - id: pose_publisher path: nodes/pose_publisher.py inputs: - tick: dora/timer/millis/5000 + tick: dora/timer/millis/10000 outputs: - target_pose \ No newline at end of file diff --git a/node-hub/gamepad/README.md b/node-hub/gamepad/README.md index 6b0256ff..25e3bd16 100644 --- a/node-hub/gamepad/README.md +++ b/node-hub/gamepad/README.md @@ -9,6 +9,7 @@ A Dora framework node that provides universal gamepad control functionality. Whi - Configurable axis mapping for different controllers - Dead zone handling for precise control - Real-time velocity command output +- Raw gamepad state output for custom processing - Clean shutdown with automatic zero velocity - Customizable speed limits - Default configuration optimized for Logitech F710 @@ -20,6 +21,7 @@ A Dora framework node that provides universal gamepad control functionality. Whi - Python 3.8 or higher - Dora Framework 0.3.6 or higher - Any compatible USB/Wireless gamepad +- Pygame library for joystick handling ### Installation @@ -54,10 +56,18 @@ sudo chmod +x /dev/input/js* ### Controls -- **Left Stick Y-Axis**: Forward/Backward movement (±2.0 m/s) -- **Left Stick X-Axis**: Left/Right turning (±1.5 rad/s) +#### Default Control Mapping (cmd_vel output): +- **D-pad Vertical**: Linear X movement (forward/backward) +- **D-pad Horizontal**: Linear Y movement (left/right) +- **Right Stick Y-Axis**: Linear Z movement (up/down, ±0.1 m/s) +- **Right Stick X-Axis**: Angular Z rotation (turning, ±0.8 rad/s) +- **Left Stick Y-Axis**: Angular X rotation (±0.8 rad/s) +- **Left Stick X-Axis**: Angular Y rotation (±0.8 rad/s) - **Deadzone**: 5% to prevent drift +#### Raw Control Output: +The node also outputs complete gamepad state including all buttons, axes, and D-pad for custom processing. + ### YAML Specification ```yaml @@ -66,20 +76,55 @@ nodes: build: pip install -e . path: gamepad outputs: - - cmd_vel # Velocity commands [vx, vy, vz, wx, wy, wz] + - cmd_vel # Velocity commands [vx, vy, vz, wx, wy, wz] + - raw_control # Complete gamepad state (JSON) inputs: - tick: dora/timer/millis/10 # Update rate + tick: dora/timer/millis/10 # Update rate (100Hz) ``` ### Outputs -The node outputs `cmd_vel` as a 6-element array: -- `[0]`: Linear X velocity (forward/backward) -- `[1]`: Linear Y velocity (always 0) -- `[2]`: Linear Z velocity (always 0) -- `[3]`: Angular X velocity (always 0) -- `[4]`: Angular Y velocity (always 0) -- `[5]`: Angular Z velocity (turning) +#### 1. `cmd_vel` - 6-element velocity array: +- `[0]`: Linear X velocity (D-pad vertical) +- `[1]`: Linear Y velocity (D-pad horizontal) +- `[2]`: Linear Z velocity (right stick Y) +- `[3]`: Angular X velocity (left stick Y) +- `[4]`: Angular Y velocity (left stick X) +- `[5]`: Angular Z velocity (right stick X) + +#### 2. `raw_control` - Complete gamepad state (JSON): +```json +{ + "axes": [left_x, left_y, right_x, right_y, ...], + "buttons": [X, A, B, Y, LB, RB, LT, RT, BACK, START, LEFT_STICK, RIGHT_STICK], + "hats": [[dpad_x, dpad_y]], + "mapping": { + "axes": {"LEFT-X": 0, "LEFT-Y": 1, "RIGHT-X": 2, "RIGHT-Y": 3}, + "buttons": {"X": 0, "A": 1, "B": 2, "Y": 3, "LB": 4, "RB": 5, ...} + } +} +``` + +## Controller Configuration + +To use a different controller, modify the `Controller` class mapping: + +```python +class Controller: + def __init__(self): + # Customize for your controller + self.axisNames = { + 'LEFT-X': 0, + 'LEFT-Y': 1, + 'RIGHT-X': 2, + 'RIGHT-Y': 3, + } + self.buttonNames = { + 'X': 0, + 'A': 1, + # ... add your button mapping + } +``` ## Development @@ -97,6 +142,28 @@ Run tests with pytest: pytest . ``` +## Integration Examples + +### Simple Velocity Control: +```python +for event in node: + if event["id"] == "cmd_vel": + velocity = event["value"].to_numpy() + # Apply velocity to your robot/system + print(f"Velocity: {velocity}") +``` + +### Custom Button Processing: +```python +import json + +for event in node: + if event["id"] == "raw_control": + control_state = json.loads(event["value"].to_pylist()[0]) + buttons = control_state["buttons"] + axis = control_state[axis] +``` + ## Troubleshooting **No gamepad detected**: @@ -104,6 +171,16 @@ pytest . - Ensure controller is powered on - Verify mode switch position + +**Raw control output empty**: + - Verify gamepad is responding with `jstest /dev/input/js0` + - Check pygame initialization + +**Incorrect button mapping**: + - Different controllers have different mappings + - Use `jstest` to identify your controller's button/axis numbers + - Update the `Controller` class accordingly + **If port is not executable then run:** ```bash sudo chmod +x /dev/input/js* @@ -117,4 +194,5 @@ Released under the MIT License. See LICENSE file for details. - Uses Pygame for joystick handling - Built with the Dora framework -- Designed for Logitech F710 gamepad \ No newline at end of file +- Designed for Logitech F710 gamepad +- Supports any standard USB/Bluetooth gamepad \ No newline at end of file