updated readme, restructured and renamed the files

7 months ago · a079bde6a2
--- a/examples/franka-mujoco-control/02_target_pose_control/README.md
+++ b/examples/franka-mujoco-control/02_target_pose_control/README.md
@@ -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
--- a/examples/franka-mujoco-control/03_gamepad_control/README.md
+++ b/examples/franka-mujoco-control/03_gamepad_control/README.md
@@ -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
--- a/examples/franka-mujoco-control/04_gamepad_collision_avoidance/README.md
+++ b/examples/franka-mujoco-control/04_gamepad_collision_avoidance/README.md
@@ -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

 <!-- ### Node Communication Flow
 ```
 ┌─────────────┐    raw_control      ┌─────────────────────────────┐
 │   gamepad   │ ──────────────────▶ │ franka_collision_controller │
 └─────────────┘                     │                             │
                                    │  • Processes gamepad        │
 ┌─────────────┐   joint_positions   │  • Calculates IK/FK         │
 │ mujoco_sim  │ ──────────────────▶ │  • Applies collision        │
 │             │                     │    avoidance                │
 │             │ ◀────────────────── │  • Outputs joint cmds       │
 └─────────────┘   joint_commands    └─────────────────────────────┘
 ``` -->

 ### 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 
--- a/examples/franka-mujoco-control/04_gamepad_collision_avoidance/collision_avoidance.yml
+++ b/examples/franka-mujoco-control/04_gamepad_collision_avoidance/collision_avoidance.yml
@@ -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
--- 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
@@ -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()
--- a/examples/franka-mujoco-control/README.md
+++ b/examples/franka-mujoco-control/README.md
@@ -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
 ```

--- a/examples/mujoco-sim/README.md
+++ 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
--- a/examples/franka-mujoco-control/franka_panda/panda.urdf
+++ b/examples/franka-mujoco-control/franka_panda/panda.urdf
--- a/examples/franka-mujoco-control/kuka_iiwa/model.urdf
+++ b/examples/franka-mujoco-control/kuka_iiwa/model.urdf
--- a/examples/franka-mujoco-control/01_basic_simulation/README.md
+++ b/examples/franka-mujoco-control/01_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
--- a/examples/franka-mujoco-control/01_basic_simulation/basic.yml
+++ b/examples/franka-mujoco-control/01_basic_simulation/basic.yml
@@ -9,4 +9,4 @@ nodes:
      - joint_velocities 
      - sensor_data
    env:
      MODEL_NAME: "panda"  # Load Franka Panda from robot-descriptions
      MODEL_NAME: "go2"  # Load GO2
--- a/examples/mujoco-sim/gamepad_control/README.md
+++ 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
--- a/examples/franka-mujoco-control/03_gamepad_control/gamepad_control.yml
+++ b/examples/franka-mujoco-control/03_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."
--- a/examples/franka-mujoco-control/03_gamepad_control/nodes/franka_gamepad_controller.py
+++ b/examples/franka-mujoco-control/03_gamepad_control/nodes/franka_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"]
--- a/examples/mujoco-sim/target_pose_control/README.md
+++ 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
--- a/examples/franka-mujoco-control/02_target_pose_control/nodes/franka_controller_pytorch.py
+++ b/examples/franka-mujoco-control/02_target_pose_control/nodes/franka_controller_pytorch.py
--- a/examples/franka-mujoco-control/02_target_pose_control/nodes/pose_publisher.py
+++ b/examples/franka-mujoco-control/02_target_pose_control/nodes/pose_publisher.py
--- a/examples/franka-mujoco-control/02_target_pose_control/target_pose_control_pytorch.yml
+++ b/examples/franka-mujoco-control/02_target_pose_control/target_pose_control_pytorch.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
--- 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
 - Designed for Logitech F710 gamepad
 - Supports any standard USB/Bluetooth gamepad