improved example readme

examples/franka-mujoco-control/01_basic_simulation/basic.yml

@@ -9,4 +9,4 @@ nodes:
       - joint_velocities 
       - sensor_data
     env:
-      MODEL_NAME: "go2"  # Load Franka Panda from robot-descriptions
+      MODEL_NAME: "panda"  # Load Franka Panda from robot-descriptions

examples/franka-mujoco-control/02_target_pose_control/README.md

@@ -1,20 +1,6 @@
 # 02. Target Pose Control
 
-This example adds robot-specific control logic by creating a dedicated Franka controller node that processes target pose commands and outputs joint commands.
-
-## Learning Goals
-
-- Implement Cartesian space control with inverse kinematics
-- Create robot-specific controller nodes
-- Send programmatic target pose commands
-
-## Architecture
-
-```
-[Pose Publisher] → [Franka Controller] → [MuJoCo Sim] → [Outputs]
-     ↓                      ↓                  ↓
-[Target Poses]     [Joint Commands]    [Joint States]
-```
+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.
 
 ## Running the Example
 
@@ -27,6 +13,7 @@ dora run target_pose_control.yml
 You should see:
 1. Robot moves to predefined target poses automatically
 2. Smooth Cartesian space motion with inverse kinematics
+3. End-effector following target positions accurately
 
 ## Interactive Control
 
@@ -52,21 +39,175 @@ node.send_output(
 "
 ```
 
-## Key Components
+## 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.py`)
+
+**Key Components**:
+
+```python
+class FrankaController:
+    def __init__(self):
+        # ⚠️ DUAL MUJOCO INSTANCE - loads separate model for kinematics
+        self.model = load_robot_description("panda_mj_description", variant="scene")
+        self.data = mujoco.MjData(self.model)
+```
+
+**What it does**:
+- **Input**: Target poses `[x, y, z, roll, pitch, yaw]` and current joint positions
+- **Processing**: Inverse kinematics using damped least squares with nullspace control
+- **Output**: Joint position commands for the robot
+
+**Control Algorithm**:
+```python
+def apply_cartesian_control(self, current_joints):
+    # 1. Update internal model with current joint state
+    self.data.qpos[:7] = current_joints[:7]
+    mujoco.mj_forward(self.model, self.data)
+    
+    # 2. Get current end-effector pose from kinematics
+    current_ee_pos = self.data.body(self.hand_id).xpos.copy()
+    current_ee_rot = self.data.body(self.hand_id).xmat.reshape(3, 3)
+    
+    # 3. Calculate position and orientation errors
+    pos_error = self.target_pos - current_ee_pos
+    rot_error = (desired_rot * current_rot.inv()).as_rotvec()
+    
+    # 4. 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
+    
+    # 5. Compute Jacobian matrix
+    jacp = np.zeros((3, self.model.nv))  # Position Jacobian
+    jacr = np.zeros((3, self.model.nv))  # Rotation Jacobian
+    mujoco.mj_jacBody(self.model, self.data, jacp, jacr, self.hand_id)
+    jac = np.vstack((jacp[:, :7], jacr[:, :7]))
+    
+    # 6. Solve inverse kinematics with damped least squares
+    diag = self.damping * np.eye(6)
+    dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist)
+    
+    # 7. 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 - current_joints[:7])
+    dq += N @ dq_null
+    
+    # 8. Integrate to get new joint positions
+    new_joints = current_joints[:7] + 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
+
+**The Problem**: This implementation runs **TWO separate MuJoCo instances**:
+
+1. **Main Simulation** (`dora-mujoco` node): Handles physics, rendering, what you see
+2. **Controller Kinematics** (`franka_controller.py`): Separate headless instance for inverse kinematics
+
+```python
+# In franka_controller.py - SEPARATE MUJOCO INSTANCE
+self.model = load_robot_description("panda_mj_description", variant="scene")
+self.data = mujoco.MjData(self.model)  # Independent physics state!
+```
+
+**Why I Did This**:
+- **Accurate Kinematics**: MuJoCo provides exact forward kinematics and Jacobian computation
+- **Smooth Control**: Custom geometric kinematics often have numerical issues
+<span style="color: #888888; opacity: 1.0;">I got Lazy 🙃: Easier than implementing analytical kinematics from scratch</span>
+
+#### Alternative Approaches: Pure Geometric Kinematics
+
+Replace the dual MuJoCo with analytical kinematics:
+
+```python
+class FrankaController:
+    def __init__(self):
+        # No MuJoCo model - just DH parameters
+        self.dh_params = self._get_franka_dh_parameters()
+    
+    def forward_kinematics(self, joint_angles):
+        """Compute FK using DH parameters"""
+        # Analytical forward kinematics implementation
+        
+    def compute_jacobian(self, joint_angles):
+        """Analytical or numerical differentiation"""
+```
+
+**Pros**: Single source of truth
+**Cons**: More complex to implement, potential numerical issues
+
+### RPY Orientation Control Issues
+
+**The Problem**: Roll-Pitch-Yaw (RPY) orientation control has limitations:
+
+```python
+target_pose = [0.5, 0.2, 0.6, 180.0, 0.0, 90.0]  # x, y, z, roll, pitch, yaw
+```
+
+**Issues**:
+- **Gimbal Lock**: Certain orientations become unreachable
+- **Order Dependency**: ZYX Euler angle convention may not match user expectations
+
+**Better Alternatives**:
+- **Quaternions**: `[x, y, z, qw, qx, qy, qz]` - no singularities (but not at all intuitive for humans)
+
+## Key Controller Features
+
+- **Cartesian Control**: Independent position and orientation control
+- **Joint Limits**: Respects Franka's mechanical constraints  
+- **Nullspace Control**: Returns to preferred joint configuration when possible
+- **Damped Least Squares**: Handles near-singular configurations gracefully
+
+
+## Production Recommendations
+
+For real robot deployment:
+1. Replace dual MuJoCo with analytical kinematics or robot manufacturer's libraries
+2. Use quaternion orientation representation  
+3. Add collision checking and joint limit monitoring
+4. Implement proper error handling and safety stops
+5. Add force/torque control capabilities
+
+## Extensions
 
-### Franka Controller Node
-- **Input**: Target poses `[x, y, z, roll, pitch, yaw]`
-- **Output**: Joint position commands  
-- **Algorithm**: Damped least squares inverse kinematics with nullspace control
+This example can be extended with:
+- **Path Planning**: Replace pose publisher with trajectory generation
+- **Obstacle Avoidance**: Add collision checking to controller
+- **Force Control**: Implement hybrid position/force control
+- **Multi-Robot**: Coordinate multiple robot arms
+- **Real Robot**: Replace MuJoCo with actual robot drivers
 
-### Pose Publisher Node
-- Sends predefined target poses in sequence
-- Demonstrates programmatic control
-- Can be replaced with your own pose generation logic or pose path planning
+## Troubleshooting
 
-## Controller Features
+**"Controller gets out of sync after GUI reset"**
+- This is due to dual MuJoCo instances - the controller's internal model doesn't know about GUI resets
+- Restart the entire pipeline after using GUI reset
 
-- **Cartesian Control**: Position and orientation control
-- **Joint Limits**: Respects Franka's joint constraints
-- **Nullspace Control**: Returns to preferred joint configuration
-- **Smooth Motion**: Velocity-limited for safety
+**"Robot moves erratically with certain orientations"**  
+- Certain RPY orientations sometimes cause issues
+- Try quaternion representation or avoid problematic orientations

examples/franka-mujoco-control/02_target_pose_control/nodes/franka_controller.py

@@ -1,7 +1,4 @@
 """Franka robot controller node for Dora.
-
-This controller uses the same proven control approach as the original 
-dora-franka-mujoco node, adapted for the modular architecture.
 """
 
 import time
@@ -16,14 +13,19 @@ class FrankaController:
     """Franka Panda robot controller using proven MuJoCo-based control."""
     
     def __init__(self):
-        # Load the same model to get proper kinematics
+        """
+        Initialize the Franka robot controller with MuJoCo simulation.
+        Sets up the robot model, simulation data, and control parameters for end-effector
+        pose control using operational space control with nullspace projection.
+        """
+
         self.model = load_robot_description("panda_mj_description", variant="scene")
         self.data = mujoco.MjData(self.model)
         
         # Get the hand body ID for end-effector control
         self.hand_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "hand")
         
-        # Control parameters (exactly from dora-franka-mujoco)
+        # Control parameters
         self.integration_dt = 0.1
         self.damping = 1e-4
         self.Kpos = 0.95  # Position gain
@@ -43,10 +45,7 @@ class FrankaController:
         
         # Initialize target position to current end-effector position
         self.target_pos = self.data.body(self.hand_id).xpos.copy()
-        
         print("Franka Controller initialized with MuJoCo model")
-        print(f"Hand body ID: {self.hand_id}")
-        print(f"Initial target position: {self.target_pos}")
     
     def set_target_pose(self, pose_array):
         """Set target pose from input array."""
@@ -64,9 +63,6 @@ class FrankaController:
     
     def apply_cartesian_control(self, current_joints):
         """Apply Cartesian control using the exact same method as dora-franka-mujoco."""
-        if current_joints is None or len(current_joints) < 7:
-            return self.home_pos
-        
         # Update our internal model state with current joint positions
         self.data.qpos[:7] = current_joints[:7]
         mujoco.mj_forward(self.model, self.data)
@@ -75,15 +71,7 @@ class FrankaController:
         current_ee_pos = self.data.body(self.hand_id).xpos.copy()
         current_ee_rot = self.data.body(self.hand_id).xmat.reshape(3, 3)
         
-        # Calculate position error
         pos_error = self.target_pos - current_ee_pos
-        pos_error_norm = np.linalg.norm(pos_error)
-        
-        # Log state periodically
-        # if pos_error_norm > 0.01:
-            # print(f"📍 Current: {current_ee_pos}")
-            # print(f"🎯 Target:  {self.target_pos}")
-            # print(f"📏 Error:   {pos_error_norm:.4f}m")
         
         # Construct 6D twist (3 position + 3 orientation)
         twist = np.zeros(6)
@@ -95,7 +83,7 @@ class FrankaController:
         rot_error = (desired_rot * current_rot.inv()).as_rotvec()
         twist[3:] = self.Kori * rot_error / self.integration_dt
         
-        # Get Jacobian for the hand body (exactly like dora-franka-mujoco)
+        # Get Jacobian for the hand body
         jacp = np.zeros((3, self.model.nv))  # Position Jacobian
         jacr = np.zeros((3, self.model.nv))  # Rotation Jacobian
         mujoco.mj_jacBody(self.model, self.data, jacp, jacr, self.hand_id)

examples/franka-mujoco-control/02_target_pose_control/nodes/pose_publisher.py

@@ -13,7 +13,8 @@ class PosePublisher:
     """Publishes target poses in sequence."""
     
     def __init__(self):
-        # Define a sequence of target poses [x, y, z, roll, pitch, yaw]
+        """Initialize pose publisher with predefined target poses sequence."""
+        # target poses [x, y, z, roll, pitch, yaw]
         self.target_poses = [
             [0.5, 0.5, 0.3, 180.0, 0.0, 90.0],   
             [0.6, 0.2, 0.5, 180.0, 0.0, 45.0],  
@@ -24,7 +25,6 @@ class PosePublisher:
         
         self.current_pose_index = 0
         print("Pose Publisher initialized")
-        print(f"Will cycle through {len(self.target_poses)} target poses")
     
     def get_next_pose(self):
         """Get the next target pose in sequence."""
@@ -36,17 +36,11 @@ def main():
     node = Node("pose_publisher")
     publisher = PosePublisher()
     
-    print("Pose Publisher Node Started")
-    print("Publishing target poses every 10 seconds...")
-    # time.sleep(10)  # Allow time for node to initialize
     for event in node:
         if event["type"] == "INPUT" and event["id"] == "tick":
-            # Get next target pose
             target_pose = publisher.get_next_pose()
-            
             print(f"Publishing target pose: {target_pose}")
-            
-            # Send target pose
+
             node.send_output(
                 "target_pose",
                 pa.array(target_pose, type=pa.float64()),

examples/franka-mujoco-control/03_gamepad_control/README.md

@@ -1,35 +1,20 @@
 # 03. Gamepad Control
 
-This example demonstrates real-time interactive control by connecting a gamepad to the Franka controller. It shows how to integrate the existing [`gamepad`](../../../node-hub/gamepad) node with robot-specific control logic.
-
-## Learning Goals
-
-- Integrate multiple input sources (gamepad + target poses)
-- Process raw gamepad input into robot commands
-- Implement real-time teleoperation
-- Understand modular input processing
-
-## Architecture
-
-```
-[Gamepad] → [Franka Controller] → [MuJoCo Sim] → [Outputs]
-             ↑
-    [Target Pose Publisher] (optional)
-```
+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 maintaining the same dual MuJoCo architecture. Shows how to integrate multiple input sources and implement teleoperation.
 
 ## Controls
 
-- **Left Stick X**: Move along X-axis  
-- **Left Stick Y**: Move along Y-axis
-- **Right Stick Y**: Move along Z-axis
-- **LB/RB**: Decrease/Increase movement speed
+- **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)
+1. **Connect a gamepad** (Xbox/PlayStation controller via USB or Bluetooth)
 2. **Run the simulation**:
 ```bash
 cd 03_gamepad_control
@@ -41,46 +26,193 @@ 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
 
-## Key Features
+## How It Works
 
-### Enhanced Controller
-The Franka controller now supports:
-- **Dual Input Sources**: Both gamepad and target pose commands
-- **Incremental Control**: Gamepad moves relative to current position
-- **Speed Scaling**: Adjustable movement speed
-- **Dead Zone**: Prevents controller drift
+### Node Breakdown
 
-### Input Priority
-- Gamepad input takes precedence when active
-- Target pose commands work when gamepad is idle
-- Smooth transitions between control modes
+#### 1. **Gamepad Node** (`gamepad`)
+Built-in Dora node that interfaces with system gamepad drivers.
 
-## Gamepad Mapping
-
-The controller uses the raw gamepad data from the [`gamepad`](../../../node-hub/gamepad) node:
+**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_values...],    // Analog stick positions
-  "buttons": [button_states...], // Button press states
-  "mapping": {...}              // Button/axis name mappings
+  "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.
+
+```python
+class EnhancedFrankaController:
+    def __init__(self):
+        # ⚠️ SAME DUAL MUJOCO ISSUE as target pose control
+        self.model = load_robot_description("panda_mj_description", variant="scene")
+        self.data = mujoco.MjData(self.model)
+        
+        # Gamepad-specific parameters
+        self.speed_scale = 0.5      # Movement speed multiplier
+        self.deadzone = 0.05        # Joystick deadzone threshold
+        self.gripper_range = [0, 255]  # Gripper control range
+```
+
+**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 robot to home position
+        self.data.qpos[:7] = self.home_pos
+        self.target_pos = self.data.body(self.hand_id).xpos.copy()
+    
+    # 2. Gripper control
+    if buttons[0]:      # X button - Close gripper
+        self.data.ctrl[self.gripper_actuator] = self.gripper_range[0]
+    elif buttons[3]:    # Y button - Open gripper  
+        self.data.ctrl[self.gripper_actuator] = self.gripper_range[1]
+    
+    # 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
+    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])
+```
+
+**Control Flow**:
+```python
+def apply_cartesian_control(self, current_joints):
+    # Same inverse kinematics as target pose control
+    # BUT now returns 8D commands: [7 arm joints + 1 gripper]
+    
+    # ... (same IK algorithm as before) ...
+    
+    # Return 8-dimensional control: 7 arm joints + gripper
+    full_commands = np.zeros(8)
+    full_commands[:7] = new_joints
+    full_commands[7] = self.data.ctrl[self.gripper_actuator]  # Gripper state
+    
+    return full_commands
+```
+
+#### 3. **MuJoCo Simulation Node** (`dora-mujoco`)
+Same as target pose control - handles physics, rendering, and outputs joint states.
+
+## Technical Implementation Details
+
+### Control Modes Comparison
+
+| Feature | Target Pose Control | Gamepad Control |
+|---------|-------------------|-----------------|
+| **Input Type** | Absolute positions | Incremental movements |
+| **Update Rate** | Configurable | Real-time (100Hz) |
+| **Control Precision** | Exact target poses | Human-guided 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
 
-- Add orientation control to right stick
-- Implement force feedback
-- Create custom button mappings
+### 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**:
+   ```python
+   # Prevent robot from leaving safe workspace
+   workspace_bounds = {
+       'x': [0.2, 0.8], 'y': [-0.5, 0.5], 'z': [0.1, 0.7]
+   }
+   ```
+
+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 (honestly not sure how to do this, but should be possible and fun)
+2. **Multi-Robot**: Control multiple arms with different controllers
+3. **Recording/Playback**: Record gamepad sessions for replay (Data Collection)
+
+## 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
 
-## Conclusion
+## Next Steps
 
-You've now built a complete modular robot control system! This architecture demonstrates:
+This gamepad control example demonstrates a complete teleoperation system. Consider exploring:
 
-- **Separation of Concerns**: Simulation, control, and input are separate
-- **Reusability**: Controller can work with different simulators
-- **Extensibility**: Easy to add new input methods or robots
-- **Maintainability**: Clear, testable components
+- **Direct Robot Control**: Replace MuJoCo with real robot drivers
+- **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
 
-This pattern scales to production robotics systems and can be adapted for any robot platform.
+<span style="color: #888888; opacity: 1.0;">All of these are really cool addons that can be implemented</span>

examples/franka-mujoco-control/03_gamepad_control/nodes/franka_gamepad_controller.py

@@ -1,7 +1,4 @@
 """Enhanced Franka controller with gamepad support.
-
-This controller uses the same proven control approach as the original 
-dora-franka-mujoco node, adapted for gamepad and target pose control.
 """
 
 import json
@@ -17,17 +14,16 @@ class EnhancedFrankaController:
     """Franka controller with gamepad and target pose support using proven MuJoCo control."""
     
     def __init__(self):
-        # Load the same model to get proper kinematics
+        """Init"""
         self.model = load_robot_description("panda_mj_description", variant="scene")
         self.data = mujoco.MjData(self.model)
         
         # Get the hand body ID for end-effector control
         self.hand_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "hand")
-        
         # Get gripper actuator ID (like in original)
         self.gripper_actuator = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, "actuator8")
         
-        # Control parameters (exactly from dora-franka-mujoco)
+        # Control parameters
         self.integration_dt = 0.1
         self.damping = 1e-4
         self.Kpos = 0.95  # Position gain
@@ -35,12 +31,12 @@ class EnhancedFrankaController:
         self.Kn = np.array([10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0])  # Nullspace gains
         self.max_angvel = 0.785  # Max joint velocity (rad/s)
         
-        # Gamepad control parameters (from original dora-franka-mujoco)
+        # Gamepad control parameters
         self.speed_scale = 0.5
         self.deadzone = 0.05
         self.last_input_source = None
         
-        # Gripper control parameters (from original)
+        # Gripper control parameters
         self.gripper_range = [0, 255]
         self.gripper_state = 0.0  # (0=closed, 1=open)
         
@@ -58,10 +54,8 @@ class EnhancedFrankaController:
         self.target_pos = self.data.body(self.hand_id).xpos.copy()
         
         print("Enhanced Franka Controller initialized with MuJoCo model")
-        print(f"Hand body ID: {self.hand_id}")
-        print(f"Gripper actuator ID: {self.gripper_actuator}")
-        print(f"Initial target position: {self.target_pos}")
-        print("Supports both gamepad and target pose control")
+        # print(f"Hand body ID: {self.hand_id}")
+        # print(f"Gripper actuator ID: {self.gripper_actuator}")
     
     def apply_deadzone(self, value, deadzone=None):
         """Apply deadzone to joystick input."""
@@ -70,12 +64,9 @@ class EnhancedFrankaController:
     
     def process_gamepad_input(self, raw_control):
         """Process gamepad input exactly like the original dora-franka-mujoco."""
-        try:
-            axes = raw_control["axes"]
-            buttons = raw_control["buttons"]
-        except (KeyError, TypeError):
-            return False
-        
+        axes = raw_control["axes"]
+        buttons = raw_control["buttons"]
+
         # Reset position with START button
         if len(buttons) > 9 and buttons[9]:
             # Reset to home position and update target
@@ -83,15 +74,14 @@ class EnhancedFrankaController:
             mujoco.mj_forward(self.model, self.data)
             self.target_pos = self.data.body(self.hand_id).xpos.copy()
             print("Reset to home position")
-            return True
         
         # Gripper control with X and Y buttons (exactly like original)
         if len(buttons) > 0 and buttons[0]:  # X button - Close gripper
-            self.data.ctrl[self.gripper_actuator] = self.gripper_range[0]  # Close (0)
+            self.data.ctrl[self.gripper_actuator] = self.gripper_range[0]  
             self.gripper_state = 0.0
             print("Gripper: CLOSED")
         elif len(buttons) > 3 and buttons[3]:  # Y button - Open gripper
-            self.data.ctrl[self.gripper_actuator] = self.gripper_range[1]  # Open (255)
+            self.data.ctrl[self.gripper_actuator] = self.gripper_range[1]  
             self.gripper_state = 1.0
             print("Gripper: OPEN")
         
@@ -101,38 +91,32 @@ class EnhancedFrankaController:
         if len(buttons) > 5 and buttons[5]:  # RB
             self.speed_scale = min(1.0, self.speed_scale + 0.1)
         
-        # Get cartesian control inputs with deadzone (exactly like original)
-        if len(axes) >= 4:
-            dx = self.apply_deadzone(axes[0], self.deadzone) * self.speed_scale * 0.1
-            dy = -self.apply_deadzone(axes[1], self.deadzone) * self.speed_scale * 0.1
-            dz = -self.apply_deadzone(axes[3], self.deadzone) * self.speed_scale * 0.1
-            
-            # Update target position incrementally (like original gamepad control)
-            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"
-                
-                # Debug output (like original)
-                print(f"Gamepad control: dx={dx:.3f}, dy={dy:.3f}, dz={dz:.3f}")
-                print(f"New target: {self.target_pos}")
-                print(f"Speed: {self.speed_scale:.1f}")
-                print(f"Gripper: {'Open' if self.gripper_state > 0.5 else 'Closed'}")
-                return True
+        # Get cartesian control inputs with deadzone 
+        dx = self.apply_deadzone(axes[0], self.deadzone) * self.speed_scale * 0.1
+        dy = -self.apply_deadzone(axes[1], self.deadzone) * self.speed_scale * 0.1
+        dz = -self.apply_deadzone(axes[3], self.deadzone) * self.speed_scale * 0.1
         
-        return False
-    
+        # 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"
+            
+            # Debug output 
+            # print(f"Gamepad control: dx={dx:.3f}, dy={dy:.3f}, dz={dz:.3f}")
+            # print(f"New target: {self.target_pos}")
+            # print(f"Speed: {self.speed_scale:.1f}")
+            # print(f"Gripper: {'Open' if self.gripper_state > 0.5 else 'Closed'}")
+
+
     def process_target_pose(self, pose_array):
         """Process target pose command."""
         if len(pose_array) >= 3:
-            old_target = self.target_pos.copy()
             self.target_pos = np.array(pose_array[:3])
-            distance_moved = np.linalg.norm(self.target_pos - old_target)
         
         if len(pose_array) >= 6:
             self.target_rpy = list(pose_array[3:6])
         
         self.last_input_source = "target_pose"
-        return True
     
     def get_target_rotation_matrix(self):
         """Convert RPY to rotation matrix."""
@@ -143,7 +127,7 @@ class EnhancedFrankaController:
     def apply_cartesian_control(self, current_joints):
         """Apply Cartesian control using the exact same method as dora-franka-mujoco."""
         if current_joints is None or len(current_joints) < 7:
-            return np.concatenate([self.home_pos, [0]])  # Include gripper in return
+            return np.concatenate([self.home_pos, [0]])  # Include gripper
         
         # Update our internal model state with current joint positions
         self.data.qpos[:7] = current_joints[:7]
@@ -225,11 +209,8 @@ def main():
             if event["id"] == "joint_positions":
                 # Update current state and compute commands
                 controller.current_joint_pos = event["value"].to_numpy()
-                
-                # Apply Cartesian control 
                 full_commands = controller.apply_cartesian_control(controller.current_joint_pos)
                 
-                # Send joint commands 
                 node.send_output(
                     "joint_commands",
                     pa.array(full_commands, type=pa.float64()),

examples/franka-mujoco-control/README.md

@@ -44,10 +44,3 @@ dora build gamepad_control.yml
 dora run gamepad_control.yml
 ```
 
-## Next Steps
-
-After completing this tutorial, you'll understand how to:
-- Build modular robotics applications with Dora
-- Create robot-specific controllers
-- Design scalable dataflow architectures
-