added the example of franka panda

examples/franka-mujoco-control/01_basic_simulation/README.md

@@ -0,0 +1,46 @@
+# 01. Basic Simulation
+
+This example demonstrates the simplest possible setup: loading and running a Franka Panda robot simulation using the `dora-mujoco` node.
+
+- Understand how the `dora-mujoco` node works
+- 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
+- Joint velocities 
+- Sensor data (if available)
+- Current simulation time
+
+## Running the Example
+
+```bash
+cd 01_basic_simulation
+dora build basic.yml
+dora run basic.yml
+```
+
+You should see:
+1. MuJoCo viewer window opens with Franka Panda 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")`
+2. **Physics Loop**: Timer triggers simulation steps at 500Hz (This is dafault step time for Mujoco)
+3. **Data Output**: Joint states are published 
+4. **Visualization**: MuJoCo viewer shows real-time simulation
+
+## Configuration Details
+
+The `basic.yml` configures:
+- Model name: `"panda"` (resolved to `panda_mj_description`)
+- Update rate: 2ms (500Hz)
+- Outputs: Joint positions, velocities, and sensor data

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

@@ -0,0 +1,12 @@
+nodes:
+  - id: mujoco_sim
+    build: pip install -e ../../../node-hub/dora-mujoco
+    path: dora-mujoco
+    inputs:
+      tick: dora/timer/millis/2  # 500 Hz simulation
+    outputs:
+      - joint_positions
+      - joint_velocities 
+      - sensor_data
+    env:
+      MODEL_NAME: "go2"  # Load Franka Panda from robot-descriptions

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

@@ -0,0 +1,72 @@
+# 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]
+```
+
+## Running the Example
+
+```bash
+cd 02_target_pose_control
+dora build target_pose_control.yml
+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
+
+## 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()}
+)
+"
+```
+
+## Key Components
+
+### 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
+
+### 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
+
+## Controller Features
+
+- **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

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

@@ -0,0 +1,166 @@
+"""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
+import numpy as np
+import pyarrow as pa
+import mujoco
+from dora import Node
+from scipy.spatial import transform
+from robot_descriptions.loaders.mujoco import load_robot_description
+
+class FrankaController:
+    """Franka Panda robot controller using proven MuJoCo-based control."""
+    
+    def __init__(self):
+        # Load the same model to get proper kinematics
+        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)
+        self.integration_dt = 0.1
+        self.damping = 1e-4
+        self.Kpos = 0.95  # Position gain
+        self.Kori = 0.95  # Orientation gain
+        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)
+        
+        # Robot state
+        self.current_joint_pos = None
+        self.target_pos = np.array([0.4, 0.0, 0.3])  # Conservative default target
+        self.target_rpy = [180.0, 0.0, 90.0]  # Downward orientation
+        self.home_pos = np.array([0, 0, 0, -1.57079, 0, 1.57079, -0.7853])
+        
+        # Initialize simulation data with home position
+        self.data.qpos[:7] = self.home_pos
+        mujoco.mj_forward(self.model, self.data)
+        
+        # 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."""
+        if len(pose_array) >= 3:
+            self.target_pos = np.array(pose_array[:3])
+        
+        if len(pose_array) >= 6:
+            self.target_rpy = list(pose_array[3:6])
+    
+    def get_target_rotation_matrix(self):
+        """Convert RPY to rotation matrix."""
+        roll_rad, pitch_rad, yaw_rad = np.radians(self.target_rpy)
+        desired_rot = transform.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 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)
+        
+        # Get current end-effector pose
+        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)
+        twist[:3] = self.Kpos * pos_error / self.integration_dt
+        
+        # Orientation control
+        current_rot = transform.Rotation.from_matrix(current_ee_rot)
+        desired_rot = transform.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 for the hand body (exactly like dora-franka-mujoco)
+        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)
+        
+        # Extract only the arm joints (first 7 DOF)
+        jac = np.vstack((jacp[:, :7], jacr[:, :7]))
+        
+        # Damped least squares inverse kinematics
+        diag = self.damping * np.eye(6)
+        dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist)
+        
+        # Nullspace control - drive towards home position in nullspace
+        N = np.eye(7) - np.linalg.pinv(jac) @ jac  # Nullspace projection
+        dq_null = self.Kn * (self.home_pos - current_joints[:7])  # Nullspace velocity
+        dq += N @ dq_null  # Add nullspace movement
+        
+        # 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 = current_joints[:7] + dq * self.integration_dt
+        
+        # Apply joint limits (from MuJoCo model)
+        for i in range(7):
+            joint_range = self.model.jnt_range[i]
+            new_joints[i] = np.clip(new_joints[i], joint_range[0], joint_range[1])
+        
+        return new_joints
+
+def main():
+    node = Node("franka_controller")
+    controller = FrankaController()
+    
+    for event in node:
+        if event["type"] == "INPUT":
+            
+            if event["id"] == "joint_positions":
+                # Update current state and compute control commands
+                controller.current_joint_pos = event["value"].to_numpy()
+                
+                # Apply Cartesian control using proven method
+                joint_commands = controller.apply_cartesian_control(controller.current_joint_pos)
+                
+                # Send joint commands
+                node.send_output(
+                    "joint_commands",
+                    pa.array(joint_commands, type=pa.float64()),
+                    metadata={"timestamp": time.time()}
+                )
+                
+                # Send current end-effector position
+                if controller.hand_id is not None:
+                    ee_pos = controller.data.body(controller.hand_id).xpos.copy()
+                    node.send_output(
+                        "ee_position", 
+                        pa.array(ee_pos, type=pa.float64()),
+                        metadata={"timestamp": time.time()}
+                    )
+                
+            elif event["id"] == "target_pose":
+                # Process new target pose
+                pose_array = event["value"].to_numpy()
+                controller.set_target_pose(pose_array)
+
+if __name__ == "__main__":
+    main()

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

@@ -0,0 +1,57 @@
+"""Target pose publisher node.
+
+This node demonstrates how to send target poses programmatically.
+It cycles through predefined poses to show the robot moving.
+"""
+
+import time
+import numpy as np
+import pyarrow as pa
+from dora import Node
+
+class PosePublisher:
+    """Publishes target poses in sequence."""
+    
+    def __init__(self):
+        # Define a 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],   
+            [0.6, 0.2, 0.5, 180.0, 0.0, 45.0],  
+            [0.7, 0.1, 0.4, 90.0, 90.0, 90.0], 
+            [0.5, -0.3, 0.6, 180.0, 0.0, 135.0],   
+            [-0.3, -0.7, 0.2, 180.0, 0.0, 90.0],   
+        ]
+        
+        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."""
+        pose = self.target_poses[self.current_pose_index]
+        self.current_pose_index = (self.current_pose_index + 1) % len(self.target_poses)
+        return pose
+
+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()),
+                metadata={"timestamp": time.time()}
+            )
+
+if __name__ == "__main__":
+    main()

examples/franka-mujoco-control/02_target_pose_control/target_pose_control.yml

@@ -0,0 +1,30 @@
+nodes:
+  - 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_controller/joint_commands
+    outputs:
+      - joint_positions
+      - joint_velocities 
+      - actuator_controls
+      - sensor_data
+    env:
+      MODEL_NAME: "panda"
+
+  - id: franka_controller
+    path: nodes/franka_controller.py
+    inputs:
+      joint_positions: mujoco_sim/joint_positions
+      target_pose: pose_publisher/target_pose
+    outputs:
+      - joint_commands
+      - ee_position
+
+  - id: pose_publisher
+    path: nodes/pose_publisher.py
+    inputs:
+      tick: dora/timer/millis/10000  # New pose every 10 seconds
+    outputs:
+      - target_pose

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

@@ -0,0 +1,86 @@
+# 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)
+```
+
+## 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
+- **START**: Reset to home position
+- **X Button**: Close gripper
+- **Y Button**: Open gripper
+
+## Running the Example
+
+1. **Connect a gamepad** (Xbox/PlayStation controller)
+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
+
+## Key Features
+
+### 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
+
+### Input Priority
+- Gamepad input takes precedence when active
+- Target pose commands work when gamepad is idle
+- Smooth transitions between control modes
+
+## Gamepad Mapping
+
+The controller uses the raw gamepad data from the [`gamepad`](../../../node-hub/gamepad) node:
+
+```json
+{
+  "axes": [stick_values...],    // Analog stick positions
+  "buttons": [button_states...], // Button press states
+  "mapping": {...}              // Button/axis name mappings
+}
+```
+
+## Extension Ideas
+
+- Add orientation control to right stick
+- Implement force feedback
+- Create custom button mappings
+
+## Conclusion
+
+You've now built a complete modular robot control system! This architecture demonstrates:
+
+- **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
+
+This pattern scales to production robotics systems and can be adapted for any robot platform.

examples/franka-mujoco-control/03_gamepad_control/gamepad_control.yml

@@ -0,0 +1,41 @@
+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_gamepad_controller/joint_commands
+    outputs:
+      - joint_positions
+      - joint_velocities 
+      - actuator_controls
+      - sensor_data
+    env:
+      MODEL_NAME: "panda"
+
+  - id: franka_gamepad_controller
+    path: nodes/franka_gamepad_controller.py
+    inputs:
+      joint_positions: mujoco_sim/joint_positions
+      raw_control: gamepad/raw_control
+      # target_pose: pose_publisher/target_pose  # Optional
+    outputs:
+      - joint_commands
+      - ee_position
+
+  # Optional: uncomment to also have programmatic control
+  # - id: pose_publisher
+  #   path: ../02_target_pose_control/nodes/pose_publisher.py
+  #   inputs:
+  #     tick: dora/timer/millis/20000  # New pose every 20 seconds
+  #   outputs:
+  #     - target_pose

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

@@ -0,0 +1,257 @@
+"""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
+import time
+import numpy as np
+import pyarrow as pa
+import mujoco
+from dora import Node
+from scipy.spatial import transform
+from robot_descriptions.loaders.mujoco import load_robot_description
+
+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
+        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)
+        self.integration_dt = 0.1
+        self.damping = 1e-4
+        self.Kpos = 0.95  # Position gain
+        self.Kori = 0.95  # Orientation gain
+        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)
+        self.speed_scale = 0.5
+        self.deadzone = 0.05
+        self.last_input_source = None
+        
+        # Gripper control parameters (from original)
+        self.gripper_range = [0, 255]
+        self.gripper_state = 0.0  # (0=closed, 1=open)
+        
+        # Robot state
+        self.current_joint_pos = None
+        self.target_pos = np.array([0.4, 0.0, 0.3])  # Conservative default target
+        self.target_rpy = [180.0, 0.0, 90.0]  # Downward orientation
+        self.home_pos = np.array([0, 0, 0, -1.57079, 0, 1.57079, -0.7853])
+        
+        # Initialize simulation data with home position
+        self.data.qpos[:7] = self.home_pos
+        mujoco.mj_forward(self.model, self.data)
+        
+        # Initialize target position to current end-effector position
+        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")
+    
+    def apply_deadzone(self, value, deadzone=None):
+        """Apply deadzone to joystick input."""
+        deadzone = deadzone or self.deadzone
+        return 0.0 if abs(value) < deadzone else value
+    
+    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
+        
+        # Reset position with START button
+        if len(buttons) > 9 and buttons[9]:
+            # Reset to home position and update target
+            self.data.qpos[:7] = self.home_pos
+            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.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.gripper_state = 1.0
+            print("Gripper: OPEN")
+        
+        # Speed scaling with bumpers (LB/RB)
+        if len(buttons) > 4 and buttons[4]:  # LB
+            self.speed_scale = max(0.1, self.speed_scale - 0.1)
+        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
+        
+        return False
+    
+    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."""
+        roll_rad, pitch_rad, yaw_rad = np.radians(self.target_rpy)
+        desired_rot = transform.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 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
+        
+        # Update our internal model state with current joint positions
+        self.data.qpos[:7] = current_joints[:7]
+        mujoco.mj_forward(self.model, self.data)
+        
+        # Get current end-effector pose
+        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
+
+        # Construct 6D twist (3 position + 3 orientation)
+        twist = np.zeros(6)
+        twist[:3] = self.Kpos * pos_error / self.integration_dt
+        
+        # Orientation control
+        current_rot = transform.Rotation.from_matrix(current_ee_rot)
+        desired_rot = transform.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 for the hand body (exactly like dora-franka-mujoco)
+        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)
+        
+        # Extract only the arm joints (first 7 DOF)
+        jac = np.vstack((jacp[:, :7], jacr[:, :7]))
+        
+        # Damped least squares inverse kinematics
+        diag = self.damping * np.eye(6)
+        dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist)
+        
+        # Nullspace control - drive towards home position in nullspace
+        N = np.eye(7) - np.linalg.pinv(jac) @ jac  # Nullspace projection
+        dq_null = self.Kn * (self.home_pos - current_joints[:7])  # Nullspace velocity
+        dq += N @ dq_null  # Add nullspace movement
+        
+        # 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 = current_joints[:7] + dq * self.integration_dt
+        
+        # Apply joint limits (from MuJoCo model)
+        for i in range(7):
+            joint_range = self.model.jnt_range[i]
+            new_joints[i] = np.clip(new_joints[i], joint_range[0], joint_range[1])
+    
+        # 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]  # Current gripper state
+    
+        return full_commands
+
+
+def main():
+    node = Node("franka_controller")
+    controller = EnhancedFrankaController()
+    
+    print("Enhanced Franka Controller Node Started")
+    print("\nGamepad Controls:")
+    print("  Left Stick X/Y: Move in X/Y plane")
+    print("  Right Stick Y: Move up/down (Z)")
+    print("  LB/RB: Decrease/Increase speed")
+    print("  START: Reset to home position")
+    print("  X: Close gripper")
+    print("  Y: Open gripper")
+    print("\nAlso accepts target_pose commands")
+    print("Ready to receive inputs...")
+    
+    for event in node:
+        if event["type"] == "INPUT":
+            
+            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()),
+                    metadata={"timestamp": time.time()}
+                )
+                
+                # Send current end-effector position
+                if controller.hand_id is not None:
+                    ee_pos = controller.data.body(controller.hand_id).xpos.copy()
+                    node.send_output(
+                        "ee_position",
+                        pa.array(ee_pos, type=pa.float64()),
+                        metadata={"timestamp": time.time()}
+                    )
+                
+            elif event["id"] == "raw_control":
+                raw_control = json.loads(event["value"].to_pylist()[0])
+                controller.process_gamepad_input(raw_control)
+                
+            elif event["id"] == "target_pose":
+                pose_array = event["value"].to_numpy()
+                controller.process_target_pose(pose_array)
+
+if __name__ == "__main__":
+    main()

examples/franka-mujoco-control/README.md

@@ -0,0 +1,53 @@
+# 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
+```
+
+## 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
+

node-hub/dora-franka-mujoco/README.md

@@ -1,235 +0,0 @@
-# dora-franka-mujoco
-
-A MuJoCo-based Franka Emika Panda robot simulation node for the Dora dataflow framework. This node provides high-fidelity physics simulation with real-time control capabilities, supporting both gamepad and programmatic control interfaces.
-
-## Features
-
-- **High-Fidelity Simulation**: Physics-based simulation using MuJoCo with detailed Franka Panda model
-- **Dual Control Modes**: Support for both gamepad control and target pose commands
-- **Cartesian Control**: End-effector position and orientation control with nullspace joint control
-- **Real-time Feedback**: Joint positions and end-effector position streaming
-- **Automatic Mesh Management**: Automatic download of required mesh files from Hugging Face
-- **Interactive Visualization**: Built-in MuJoCo viewer for real-time monitoring
-
-## Robot Model
-
-- **Robot**: Franka Emika Panda (7-DOF arm + 2-DOF gripper)
-- **Environment**: Table workspace with movable objects
-- **Physics**: Full dynamics simulation with gravity and collision detection
-- **Control**: Position control with damped least squares inverse kinematics
-
-## Getting Started
-
-### Installation
-
-```bash
-# Create virtual environment
-uv venv -p 3.11 --seed
-
-# Install the package
-uv pip install -e .
-```
-
-### Quick Start
-
-1. **Run with gamepad control**:
-```bash
-dora build demo.yml
-# run the node 
-dora run demo.yml
-```
-
-2. **Connect a gamepad** (Xbox/PlayStation controller) and use the controls below
-
-### Controls
-
-#### Gamepad Mapping
-- **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
-- **START**: Reset robot to home position
-- **X Button**: Close gripper
-- **Y Button**: Open gripper
-
-#### Programmatic Control
-Send target poses as `[x, y, z, roll, pitch, yaw]` arrays to the `target_pose` input.
-
-## YAML Specification
-
-### Node Configuration
-```yaml
-nodes:
-  - id: mujoco_franka
-    build: pip install -e .
-    path: dora-franka-mujoco
-    inputs:
-      raw_control: gamepad/raw_control  # Gamepad input (optional)
-      target_pose: controller/target_pose  # Target pose commands (optional)
-      tick: dora/timer/millis/10  # Simulation tick rate
-    outputs:
-      - joint_positions  # 7-DOF joint angles
-      - ee_position     # End-effector position [x, y, z]
-```
-
-### Input Specifications
-
-| Input | Type | Description | Format |
-|-------|------|-------------|---------|
-| `raw_control` | `pa.string()` | Gamepad input (handled by gamepad node) | `{"axes": [float], "buttons": [bool]}` |
-| `target_pose` | `pa.array(float64)` | Target pose command | `[x, y, z, roll, pitch, yaw]` (position in meters, orientation in degrees) |
-| `tick` | Timer | Simulation step trigger | Timer event |
-
-### Output Specifications
-
-| Output | Type | Description | Metadata |
-|--------|------|-------------|----------|
-| `joint_positions` | `pa.array(float64)` | 7-DOF joint angles (radians) | `{"type": "joint_positions"}` |
-| `ee_position` | `pa.array(float64)` | End-effector position [x, y, z] (meters) | `{"type": "ee_position"}` |
-
-## Examples
-
-### Basic Simulation
-```yaml
-# Minimal setup for physics simulation
-nodes:
-  - id: mujoco_franka
-    build: pip install -e .
-    path: dora-franka-mujoco
-    inputs:
-      tick: dora/timer/millis/10
-    outputs:
-      - joint_positions
-      - ee_position
-```
-
-### Gamepad Control
-```yaml
-# Full gamepad control setup
-nodes:
-  - id: gamepad
-    build: pip install -e ../gamepad
-    path: gamepad
-    outputs:
-      - raw_control
-    inputs:
-      tick: dora/timer/millis/10
-
-  - id: mujoco_franka
-    build: pip install -e .
-    path: dora-franka-mujoco
-    inputs:
-      raw_control: gamepad/raw_control
-      tick: dora/timer/millis/10
-    outputs:
-      - joint_positions
-      - ee_position
-```
-
-### Programmatic Control
-```python
-# Send target pose commands
-import pyarrow as pa
-from dora import Node
-
-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()}
-)
-```
-
-## Technical Details
-
-### Control System
-- **Inverse Kinematics**: Damped least squares with nullspace control
-- **Target Orientation**: Default downward-facing gripper (configurable)
-- **Joint Limits**: Enforced according to Franka specifications
-- **Velocity Limits**: Maximum 0.785 rad/s per joint
-
-
-### Mesh Files
-The node automatically downloads required mesh files from Hugging Face Hub:
-- **Repository**: `SGPatil/Mujoco_franka_meshes`
-- **Cache Location**: `~/.cache/dora-mujoco-franka/meshes/`
-- **File Types**: STL and OBJ mesh files for visual and collision geometry
-
-### Simulation Parameters
-- **Timestep**: 0.002s (500Hz physics simulation)
-- **Integration**: 0.1s control integration time
-- **Damping**: 1e-4 for numerical stability
-- **Position Gains**: Kpos=0.95, Kori=0.95
-- **Nullspace Gains**: [10, 10, 10, 10, 5, 5, 5]
-
-## Troubleshooting
-
-### Common Issues
-
-1. **Mesh files not downloading**:
-   ```
-   Error downloading mesh files: [error details]
-   ```
-   - Check internet connection
-   - Verify Hugging Face Hub access
-   - Clear cache: `rm -rf ~/.cache/dora-mujoco-franka/`
-
-2. **Gamepad not detected**:
-   - Ensure gamepad is connected and recognized by OS
-   - Test with `js_test` on Linux or similar tools
-   - Check gamepad node configuration
-
-3. **Robot not responding to commands**:
-   - Verify input connections in YAML
-   - Check that timer is triggering simulation steps
-   - Use MuJoCo viewer to monitor robot state
-
-4. **Simulation running slowly**:
-   - Reduce timer frequency (increase interval)
-   - Close other applications using GPU/CPU
-   - Consider headless mode for better performance
-
-
-## Development
-
-### Code Quality
-```bash
-# Format code
-uv run ruff check . --fix
-
-# Lint code  
-uv run ruff check .
-
-# Run tests
-uv pip install pytest
-uv run pytest .
-```
-
-### Adding New Features
-1. Modify the `RobotController` class for new control modes
-2. Update input/output specifications
-3. Add corresponding tests
-4. Update documentation
-
-## Contributing
-
-Contributions are welcome! Please:
-
-1. Follow the existing code style (ruff formatting)
-2. Add tests for new features
-3. Update documentation as needed
-4. Submit pull requests with clear descriptions
-
-## License
-
-This project is released under the MIT License. See the LICENSE file for details.
-
-## Related Projects
-
-- [Dora-rs](https://github.com/dora-rs/dora) - The dataflow framework
-- [MuJoCo](https://github.com/google-deepmind/mujoco) - Physics simulation engine
-- [Franka Control Interface](https://frankaemika.github.io/docs/) - Official Franka documentation

node-hub/dora-franka-mujoco/demo.yml

@@ -1,24 +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
-    env:
-      VIRTUAL_ENV: path_to_your/venv
-
-  - id: mujoco_franka
-    build: pip install -e ../../node-hub/dora-franka-mujoco
-    path: dora-franka-mujoco
-    inputs:
-      raw_control: gamepad/raw_control
-      # target_pose: target_pose_publisher/target_pose 
-      tick: dora/timer/millis/10
-    outputs:
-      - joint_positions
-      - ee_position
-    env:
-      VIRTUAL_ENV: path_to_your/venv

node-hub/dora-franka-mujoco/dora_franka_mujoco/__init__.py

@@ -1,11 +0,0 @@
-import os
-
-# Define the path to the README file relative to the package directory
-readme_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "README.md")
-
-# Read the content of the README file
-try:
-    with open(readme_path, encoding="utf-8") as f:
-        __doc__ = f.read()
-except FileNotFoundError:
-    __doc__ = "README file not found."

node-hub/dora-franka-mujoco/dora_franka_mujoco/__main__.py

@@ -1,5 +0,0 @@
-from .main import main
-
-
-if __name__ == "__main__":
-    main()

node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/assets/names.txt

@@ -1,67 +0,0 @@
-finger_0.obj
-finger_1.obj
-hand_0.obj
-hand_1.obj
-hand_2.obj
-hand_3.obj
-hand_4.obj
-hand.stl
-link0_0.obj
-link0_10.obj
-link0_11.obj
-link0_1.obj
-link0_2.obj
-link0_3.obj
-link0_4.obj
-link0_5.obj
-link0_7.obj
-link0_8.obj
-link0_9.obj
-link0.stl
-link1.obj
-link1.stl
-link2.obj
-link2.stl
-link3_0.obj
-link3_1.obj
-link3_2.obj
-link3_3.obj
-link3.stl
-link4_0.obj
-link4_1.obj
-link4_2.obj
-link4_3.obj
-link4.stl
-link5_0.obj
-link5_1.obj
-link5_2.obj
-link5_collision_0.obj
-link5_collision_1.obj
-link5_collision_2.obj
-link6_0.obj
-link6_10.obj
-link6_11.obj
-link6_12.obj
-link6_13.obj
-link6_14.obj
-link6_15.obj
-link6_16.obj
-link6_1.obj
-link6_2.obj
-link6_3.obj
-link6_4.obj
-link6_5.obj
-link6_6.obj
-link6_7.obj
-link6_8.obj
-link6_9.obj
-link6.stl
-link7_0.obj
-link7_1.obj
-link7_2.obj
-link7_3.obj
-link7_4.obj
-link7_5.obj
-link7_6.obj
-link7_7.obj
-link7.stl

node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/panda.xml

@@ -1,283 +0,0 @@
-<mujoco model="panda">
-  <compiler angle="radian" meshdir="assets" autolimits="true"/>
-
-  <option integrator="implicitfast"/>
-
-  <default>
-    <default class="panda">
-      <material specular="0.5" shininess="0.25"/>
-      <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
-      <general dyntype="none" biastype="affine" ctrlrange="-2.8973 2.8973" forcerange="-87 87"/>
-      <default class="finger">
-        <joint axis="0 1 0" type="slide" range="0 0.04"/>
-      </default>
-
-      <default class="visual">
-        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
-      </default>
-      <default class="collision">
-        <geom type="mesh" group="3"/>
-        <default class="fingertip_pad_collision_1">
-          <geom type="box" size="0.0085 0.004 0.0085" pos="0 0.0055 0.0445"/>
-        </default>
-        <default class="fingertip_pad_collision_2">
-          <geom type="box" size="0.003 0.002 0.003" pos="0.0055 0.002 0.05"/>
-        </default>
-        <default class="fingertip_pad_collision_3">
-          <geom type="box" size="0.003 0.002 0.003" pos="-0.0055 0.002 0.05"/>
-        </default>
-        <default class="fingertip_pad_collision_4">
-          <geom type="box" size="0.003 0.002 0.0035" pos="0.0055 0.002 0.0395"/>
-        </default>
-        <default class="fingertip_pad_collision_5">
-          <geom type="box" size="0.003 0.002 0.0035" pos="-0.0055 0.002 0.0395"/>
-        </default>
-      </default>
-    </default>
-  </default>
-
-  <asset>
-    <material class="panda" name="white" rgba="1 1 1 1"/>
-    <material class="panda" name="off_white" rgba="0.901961 0.921569 0.929412 1"/>
-    <material class="panda" name="black" rgba="0.25 0.25 0.25 1"/>
-    <material class="panda" name="green" rgba="0 1 0 1"/>
-    <material class="panda" name="light_blue" rgba="0.039216 0.541176 0.780392 1"/>
-
-    <!-- Collision meshes -->
-    <mesh name="link0_c" file="link0.stl"/>
-    <mesh name="link1_c" file="link1.stl"/>
-    <mesh name="link2_c" file="link2.stl"/>
-    <mesh name="link3_c" file="link3.stl"/>
-    <mesh name="link4_c" file="link4.stl"/>
-    <mesh name="link5_c0" file="link5_collision_0.obj"/>
-    <mesh name="link5_c1" file="link5_collision_1.obj"/>
-    <mesh name="link5_c2" file="link5_collision_2.obj"/>
-    <mesh name="link6_c" file="link6.stl"/>
-    <mesh name="link7_c" file="link7.stl"/>
-    <mesh name="hand_c" file="hand.stl"/>
-
-    <!-- Visual meshes -->
-    <mesh file="link0_0.obj"/>
-    <mesh file="link0_1.obj"/>
-    <mesh file="link0_2.obj"/>
-    <mesh file="link0_3.obj"/>
-    <mesh file="link0_4.obj"/>
-    <mesh file="link0_5.obj"/>
-    <mesh file="link0_7.obj"/>
-    <mesh file="link0_8.obj"/>
-    <mesh file="link0_9.obj"/>
-    <mesh file="link0_10.obj"/>
-    <mesh file="link0_11.obj"/>
-    <mesh file="link1.obj"/>
-    <mesh file="link2.obj"/>
-    <mesh file="link3_0.obj"/>
-    <mesh file="link3_1.obj"/>
-    <mesh file="link3_2.obj"/>
-    <mesh file="link3_3.obj"/>
-    <mesh file="link4_0.obj"/>
-    <mesh file="link4_1.obj"/>
-    <mesh file="link4_2.obj"/>
-    <mesh file="link4_3.obj"/>
-    <mesh file="link5_0.obj"/>
-    <mesh file="link5_1.obj"/>
-    <mesh file="link5_2.obj"/>
-    <mesh file="link6_0.obj"/>
-    <mesh file="link6_1.obj"/>
-    <mesh file="link6_2.obj"/>
-    <mesh file="link6_3.obj"/>
-    <mesh file="link6_4.obj"/>
-    <mesh file="link6_5.obj"/>
-    <mesh file="link6_6.obj"/>
-    <mesh file="link6_7.obj"/>
-    <mesh file="link6_8.obj"/>
-    <mesh file="link6_9.obj"/>
-    <mesh file="link6_10.obj"/>
-    <mesh file="link6_11.obj"/>
-    <mesh file="link6_12.obj"/>
-    <mesh file="link6_13.obj"/>
-    <mesh file="link6_14.obj"/>
-    <mesh file="link6_15.obj"/>
-    <mesh file="link6_16.obj"/>
-    <mesh file="link7_0.obj"/>
-    <mesh file="link7_1.obj"/>
-    <mesh file="link7_2.obj"/>
-    <mesh file="link7_3.obj"/>
-    <mesh file="link7_4.obj"/>
-    <mesh file="link7_5.obj"/>
-    <mesh file="link7_6.obj"/>
-    <mesh file="link7_7.obj"/>
-    <mesh file="hand_0.obj"/>
-    <mesh file="hand_1.obj"/>
-    <mesh file="hand_2.obj"/>
-    <mesh file="hand_3.obj"/>
-    <mesh file="hand_4.obj"/>
-    <mesh file="finger_0.obj"/>
-    <mesh file="finger_1.obj"/>
-  </asset>
-
-  <worldbody>
-    <light name="top" pos="0 0 2" mode="trackcom"/>
-    <body name="link0" childclass="panda" pos="0 0 0.42">
-      <inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
-        fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
-      <geom mesh="link0_0" material="off_white" class="visual"/>
-      <geom mesh="link0_1" material="black" class="visual"/>
-      <geom mesh="link0_2" material="off_white" class="visual"/>
-      <geom mesh="link0_3" material="black" class="visual"/>
-      <geom mesh="link0_4" material="off_white" class="visual"/>
-      <geom mesh="link0_5" material="black" class="visual"/>
-      <geom mesh="link0_7" material="white" class="visual"/>
-      <geom mesh="link0_8" material="white" class="visual"/>
-      <geom mesh="link0_9" material="black" class="visual"/>
-      <geom mesh="link0_10" material="off_white" class="visual"/>
-      <geom mesh="link0_11" material="white" class="visual"/>
-      <geom mesh="link0_c" class="collision"/>
-      <body name="link1" pos="0 0 0.333">
-        <inertial mass="4.970684" pos="0.003875 0.002081 -0.04762"
-          fullinertia="0.70337 0.70661 0.0091170 -0.00013900 0.0067720 0.019169"/>
-        <joint name="joint1"/>
-        <geom material="white" mesh="link1" class="visual"/>
-        <geom mesh="link1_c" class="collision"/>
-        <body name="link2" quat="1 -1 0 0">
-          <inertial mass="0.646926" pos="-0.003141 -0.02872 0.003495"
-            fullinertia="0.0079620 2.8110e-2 2.5995e-2 -3.925e-3 1.0254e-2 7.04e-4"/>
-          <joint name="joint2" range="-1.7628 1.7628"/>
-          <geom material="white" mesh="link2" class="visual"/>
-          <geom mesh="link2_c" class="collision"/>
-          <body name="link3" pos="0 -0.316 0" quat="1 1 0 0">
-            <joint name="joint3"/>
-            <inertial mass="3.228604" pos="2.7518e-2 3.9252e-2 -6.6502e-2"
-              fullinertia="3.7242e-2 3.6155e-2 1.083e-2 -4.761e-3 -1.1396e-2 -1.2805e-2"/>
-            <geom mesh="link3_0" material="white" class="visual"/>
-            <geom mesh="link3_1" material="white" class="visual"/>
-            <geom mesh="link3_2" material="white" class="visual"/>
-            <geom mesh="link3_3" material="black" class="visual"/>
-            <geom mesh="link3_c" class="collision"/>
-            <body name="link4" pos="0.0825 0 0" quat="1 1 0 0">
-              <inertial mass="3.587895" pos="-5.317e-2 1.04419e-1 2.7454e-2"
-                fullinertia="2.5853e-2 1.9552e-2 2.8323e-2 7.796e-3 -1.332e-3 8.641e-3"/>
-              <joint name="joint4" range="-3.0718 -0.0698"/>
-              <geom mesh="link4_0" material="white" class="visual"/>
-              <geom mesh="link4_1" material="white" class="visual"/>
-              <geom mesh="link4_2" material="black" class="visual"/>
-              <geom mesh="link4_3" material="white" class="visual"/>
-              <geom mesh="link4_c" class="collision"/>
-              <body name="link5" pos="-0.0825 0.384 0" quat="1 -1 0 0">
-                <inertial mass="1.225946" pos="-1.1953e-2 4.1065e-2 -3.8437e-2"
-                  fullinertia="3.5549e-2 2.9474e-2 8.627e-3 -2.117e-3 -4.037e-3 2.29e-4"/>
-                <joint name="joint5"/>
-                <geom mesh="link5_0" material="black" class="visual"/>
-                <geom mesh="link5_1" material="white" class="visual"/>
-                <geom mesh="link5_2" material="white" class="visual"/>
-                <geom mesh="link5_c0" class="collision"/>
-                <geom mesh="link5_c1" class="collision"/>
-                <geom mesh="link5_c2" class="collision"/>
-                <body name="link6" quat="1 1 0 0">
-                  <inertial mass="1.666555" pos="6.0149e-2 -1.4117e-2 -1.0517e-2"
-                    fullinertia="1.964e-3 4.354e-3 5.433e-3 1.09e-4 -1.158e-3 3.41e-4"/>
-                  <joint name="joint6" range="-0.0175 3.7525"/>
-                  <geom mesh="link6_0" material="off_white" class="visual"/>
-                  <geom mesh="link6_1" material="white" class="visual"/>
-                  <geom mesh="link6_2" material="black" class="visual"/>
-                  <geom mesh="link6_3" material="white" class="visual"/>
-                  <geom mesh="link6_4" material="white" class="visual"/>
-                  <geom mesh="link6_5" material="white" class="visual"/>
-                  <geom mesh="link6_6" material="white" class="visual"/>
-                  <geom mesh="link6_7" material="light_blue" class="visual"/>
-                  <geom mesh="link6_8" material="light_blue" class="visual"/>
-                  <geom mesh="link6_9" material="black" class="visual"/>
-                  <geom mesh="link6_10" material="black" class="visual"/>
-                  <geom mesh="link6_11" material="white" class="visual"/>
-                  <geom mesh="link6_12" material="green" class="visual"/>
-                  <geom mesh="link6_13" material="white" class="visual"/>
-                  <geom mesh="link6_14" material="black" class="visual"/>
-                  <geom mesh="link6_15" material="black" class="visual"/>
-                  <geom mesh="link6_16" material="white" class="visual"/>
-                  <geom mesh="link6_c" class="collision"/>
-                  <body name="link7" pos="0.088 0 0" quat="1 1 0 0">
-                    <inertial mass="7.35522e-01" pos="1.0517e-2 -4.252e-3 6.1597e-2"
-                      fullinertia="1.2516e-2 1.0027e-2 4.815e-3 -4.28e-4 -1.196e-3 -7.41e-4"/>
-                    <joint name="joint7"/>
-                    <geom mesh="link7_0" material="white" class="visual"/>
-                    <geom mesh="link7_1" material="black" class="visual"/>
-                    <geom mesh="link7_2" material="black" class="visual"/>
-                    <geom mesh="link7_3" material="black" class="visual"/>
-                    <geom mesh="link7_4" material="black" class="visual"/>
-                    <geom mesh="link7_5" material="black" class="visual"/>
-                    <geom mesh="link7_6" material="black" class="visual"/>
-                    <geom mesh="link7_7" material="white" class="visual"/>
-                    <geom mesh="link7_c" class="collision"/>
-                    <body name="hand" pos="0 0 0.107" quat="0.9238795 0 0 -0.3826834">
-                      <inertial mass="0.73" pos="-0.01 0 0.03" diaginertia="0.001 0.0025 0.0017"/>
-                      <geom mesh="hand_0" material="off_white" class="visual"/>
-                      <geom mesh="hand_1" material="black" class="visual"/>
-                      <geom mesh="hand_2" material="black" class="visual"/>
-                      <geom mesh="hand_3" material="white" class="visual"/>
-                      <geom mesh="hand_4" material="off_white" class="visual"/>
-                      <geom mesh="hand_c" class="collision"/>
-                      <body name="left_finger" pos="0 0 0.0584">
-                        <inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
-                        <joint name="finger_joint1" class="finger"/>
-                        <geom mesh="finger_0" material="off_white" class="visual"/>
-                        <geom mesh="finger_1" material="black" class="visual"/>
-                        <geom mesh="finger_0" class="collision"/>
-                        <geom class="fingertip_pad_collision_1"/>
-                        <geom class="fingertip_pad_collision_2"/>
-                        <geom class="fingertip_pad_collision_3"/>
-                        <geom class="fingertip_pad_collision_4"/>
-                        <geom class="fingertip_pad_collision_5"/>
-                      </body>
-                      <body name="right_finger" pos="0 0 0.0584" quat="0 0 0 1">
-                        <inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
-                        <joint name="finger_joint2" class="finger"/>
-                        <geom mesh="finger_0" material="off_white" class="visual"/>
-                        <geom mesh="finger_1" material="black" class="visual"/>
-                        <geom mesh="finger_0" class="collision"/>
-                        <geom class="fingertip_pad_collision_1"/>
-                        <geom class="fingertip_pad_collision_2"/>
-                        <geom class="fingertip_pad_collision_3"/>
-                        <geom class="fingertip_pad_collision_4"/>
-                        <geom class="fingertip_pad_collision_5"/>
-                      </body>
-                    </body>
-                  </body>
-                </body>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-    </body>
-  </worldbody>
-
-  <tendon>
-    <fixed name="split">
-      <joint joint="finger_joint1" coef="0.5"/>
-      <joint joint="finger_joint2" coef="0.5"/>
-    </fixed>
-  </tendon>
-
-  <equality>
-    <joint joint1="finger_joint1" joint2="finger_joint2" solimp="0.95 0.99 0.001" solref="0.005 1"/>
-  </equality>
-
-  <actuator>
-    <general class="panda" name="actuator1" joint="joint1" gainprm="4500" biasprm="0 -4500 -450"/>
-    <general class="panda" name="actuator2" joint="joint2" gainprm="4500" biasprm="0 -4500 -450"
-      ctrlrange="-1.7628 1.7628"/>
-    <general class="panda" name="actuator3" joint="joint3" gainprm="3500" biasprm="0 -3500 -350"/>
-    <general class="panda" name="actuator4" joint="joint4" gainprm="3500" biasprm="0 -3500 -350"
-      ctrlrange="-3.0718 -0.0698"/>
-    <general class="panda" name="actuator5" joint="joint5" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
-    <general class="panda" name="actuator6" joint="joint6" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"
-      ctrlrange="-0.0175 3.7525"/>
-    <general class="panda" name="actuator7" joint="joint7" gainprm="2000" biasprm="0 -2000 -200" forcerange="-12 12"/>
-    <!-- Remap original ctrlrange (0, 0.04) to (0, 255): 0.04 * 100 / 255 = 0.01568627451 -->
-    <general class="panda" name="actuator8" tendon="split" forcerange="-100 100" ctrlrange="0 255"
-      gainprm="0.01568627451 0 0" biasprm="0 -100 -10"/>
-  </actuator>
-
-  <!-- <keyframe>
-    <key name="home" qpos="0 0 0 -1.57079 0 1.57079 -0.7853 0.04 0.04" ctrl="0 0 0 -1.57079 0 1.57079 -0.7853 255"/>
-  </keyframe> -->
-</mujoco>

node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/scene.xml

@@ -1,38 +0,0 @@
-<mujoco model="panda scene">
-  <include file="panda.xml"/>
-
-  <statistic center="0.3 0 0.4" extent="1"/>
-
-  <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
-    <rgba haze="0.15 0.25 0.35 1"/>
-    <global azimuth="120" elevation="-20"/>
-  </visual>
-
-  <asset>
-    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
-    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8" width="300" height="300"/>
-    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
-    <material name="table" rgba="0.7 0.5 0.3 1" reflectance="0.1"/>
-  </asset>
-
-  <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
-    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
-    
-    <!-- Table -->
-    <body name="table" pos="0.3 0 0">
-      <geom name="tabletop" size="0.5 0.5 0.02" pos="0 0 0.4" type="box" material="table"/>
-      <geom name="leg1" size="0.025 0.025 0.2" pos="0.35 0.35 0.2" type="box" material="table"/>
-      <geom name="leg2" size="0.025 0.025 0.2" pos="0.35 -0.35 0.2" type="box" material="table"/>
-      <geom name="leg3" size="0.025 0.025 0.2" pos="-0.35 0.35 0.2" type="box" material="table"/>
-      <geom name="leg4" size="0.025 0.025 0.2" pos="-0.35 -0.35 0.2" type="box" material="table"/>
-    </body>
-    <!-- Movable Box -->
-    <body name="movable_box" pos="0.5 0.0 0.45">
-      <freejoint/>
-      <geom name="box_geom" type="box" size="0.025 0.025 0.025" rgba="0.8 0.2 0.2 1" mass="0.05"/>
-    </body>
-  </worldbody>
-</mujoco>

node-hub/dora-franka-mujoco/dora_franka_mujoco/main.py

@@ -1,257 +0,0 @@
-"""MuJoCo Franka simulation node for Dora.
-
-This module provides a Dora node that simulates a Franka robot using the MuJoCo physics engine.
-It handles raw gamepad input for robot control and target pose commands.
-"""
-
-import os
-import json
-import numpy as np
-import mujoco
-import mujoco.viewer
-import pyarrow as pa
-from dora import Node
-from scipy.spatial import transform
-from .mesh_downloader import check_mesh_files_exist, ensure_meshes
-
-class RobotController:
-    """Handles robot control modes and mappings."""
-    def __init__(self):
-        # Control parameters
-        self.dt = 0.002  # MuJoCo timestep
-        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])  # Nullspace gains
-        self.max_angvel = 0.785  # Maximum joint velocity (rad/s)
-        self.speed_scale = 0.5
-        self.deadzone = 0.05  # Joystick deadzone
-        self.gripper_range = [0, 255]
-        self.gripper_state = 0.0  # (0=closed, 1=open)
-        self.target_rot_z = 0.0 
-        self.rotation_scale = 0.2
-        self.last_input_source = None
-        
-        # Default RPY values for downward-facing gripper (in degrees)
-        self.target_rpy = [180.0, 0.0, 90.0]
-
-    def set_target_orientation_rpy(self, roll_deg, pitch_deg, yaw_deg):
-        """Set target orientation using roll, pitch, yaw in degrees."""
-        self.target_rpy = [roll_deg, pitch_deg, yaw_deg]
-
-    def get_target_rotation_matrix(self):
-        """convert RPY to rotation matrix."""
-        roll_rad, pitch_rad, yaw_rad = np.radians(self.target_rpy)
-        desired_rot = transform.Rotation.from_euler('ZYX', [yaw_rad, pitch_rad, roll_rad])
-        return desired_rot.as_matrix()
-
-    def initialize(self, model, data):
-        """Initialize controller with model specific parameters."""
-        # Get end effector (hand) body ID
-        self.hand_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "hand")  
-        
-        # Get gripper actuator IDs
-        self.gripper_actuator = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "actuator8")
-        
-        # Initial end effector position and orientation to set as home pose
-        self.home_pos = data.qpos.copy()
-        self.target_pos = data.body(self.hand_id).xpos.copy()
-
-        # Set gravity
-        # model.opt.gravity[2] = 0.0
-        model.opt.gravity[2] = -9.81
-
-    def reset_pose(self, model, data):
-        """Reset robot to home position."""
-        data.qpos[:] = self.home_pos
-        data.qvel[:] = 0.0
-        mujoco.mj_forward(model, data)
-        
-        # Reset target position and Orientation
-        self.target_pos = data.body(self.hand_id).xpos.copy()
-        # self.initial_rot = transform.Rotation.from_matrix(data.body(self.hand_id).xmat.reshape(3, 3))
-
-        print("Robot reset to home position")
-
-    def process_target_pose(self, target_pose, model, data):
-        """Process target pose input (x, y, z, roll, pitch, yaw)."""
-
-        self.target_pos = np.array(target_pose[:3])
-        # If 6 elements provided, update orientation as well
-        if len(target_pose) >= 6:
-            roll, pitch, yaw = target_pose[3:6]
-            self.set_target_orientation_rpy(roll, pitch, yaw)
-
-        self._apply_cartesian_control(model, data)
-        self.last_input_source = "target_pose"
-
-    def process_gamepad_input(self, raw_control, model, data):
-        """Process raw gamepad input into robot controls."""
-        axes = raw_control["axes"]
-        buttons = raw_control["buttons"]
-        
-        # Reset position with START button
-        if buttons[9]:
-            self.reset_pose(model, data)
-            return
-        # Gripper control with X and Y buttons
-        if buttons[0]:  # X button - Close gripper
-            data.ctrl[self.gripper_actuator] = self.gripper_range[0]  # Close (0)
-            self.gripper_state = 0.0
-        elif buttons[3]:  # Y button - Open gripper
-            data.ctrl[self.gripper_actuator] = self.gripper_range[1]  # Open (255)
-            self.gripper_state = 1.0
-        # Speed scaling with bumpers
-        if buttons[4]:  # LB
-            self.speed_scale = max(0.1, self.speed_scale - 0.1)
-        if buttons[5]:  # RB
-            self.speed_scale = min(1.0, self.speed_scale + 0.1)
-            
-        # 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
-        
-        # Update target position incrementally for gamepad control
-        self.target_pos += np.array([dx, dy, dz])
-        self._apply_cartesian_control(model, data)
-        
-        # # Debug output 
-        # if np.any(np.abs([dx, dy, dz]) > 0):
-        #     print(f"Target: {self.target_pos}")
-        #     print(f"Current: {data.body(self.hand_id).xpos}")
-        #     print(f"Speed: {self.speed_scale:.1f}")
-        #     print(f"Gripper: {'Open' if self.gripper_state > 0.5 else 'Closed'}")
-        self.last_input_source = "gamepad"
-
-    def _apply_cartesian_control(self, model, data):
-        """Apply cartesian control to move the robot towards target position."""
-        twist = np.zeros(6)  # 3 for position, 3 for orientation
-        twist[:3] = self.Kpos * (self.target_pos - data.body(self.hand_id).xpos) / self.integration_dt
-        
-        current_rot_mat = data.body(self.hand_id).xmat.reshape(3, 3)
-        desired_rot_mat = self.get_target_rotation_matrix()
-        current_rot = transform.Rotation.from_matrix(current_rot_mat)
-        desired_rot = transform.Rotation.from_matrix(desired_rot_mat)
-        
-        rot_error = (desired_rot * current_rot.inv()).as_rotvec()
-        twist[3:] = self.Kori * rot_error / self.integration_dt
-        
-        # Get Jacobian for the hand body
-        jacp = np.zeros((3, model.nv))
-        jacr = np.zeros((3, model.nv))
-        mujoco.mj_jacBody(model, data, jacp, jacr, self.hand_id)
-        
-        # Extract the relevant part of the Jacobian (just the 7 arm joints)
-        jac = np.vstack((jacp[:, :7], jacr[:, :7]))
-        
-        # Damped least squares
-        diag = self.damping * np.eye(6)
-        dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist)
-        
-        # Nullspace control
-        N = np.eye(7) - np.linalg.pinv(jac) @ jac  # Nullspace projection matrix
-        dq_null = self.Kn * (self.home_pos[:7] - data.qpos[:7])  # Joint space velocity
-        dq += N @ dq_null  # Add nullspace movement
-        
-        # Clamp maximum joint velocity
-        dq_abs_max = np.abs(dq).max()
-        if dq_abs_max > self.max_angvel:
-            dq *= self.max_angvel / dq_abs_max
-            
-        # Integrate joint velocities
-        q = data.qpos[:7].copy()  # Only get the 7 arm joints
-        q += dq * self.integration_dt
-        
-        # Apply joint limits
-        np.clip(q, model.jnt_range[:7, 0], model.jnt_range[:7, 1], out=q)
-        
-        # Set control signals for arm joints
-        data.ctrl[:7] = q
-            
-    def apply_deadzone(self, value, deadzone):
-        """Apply deadzone to joystick input."""
-        if abs(value) < deadzone:
-            return 0.0
-        return value
-
-def main():
-    node = Node("mujoco_franka")
-    print("Initializing MuJoCo Franka simulation...")
-    
-    # Check if all required mesh files are present
-    print("Checking mesh files...")
-    if not check_mesh_files_exist():
-        print("Some mesh files are missing. Downloading required files...")
-        if not ensure_meshes():
-            print("Error: Failed to download all required mesh files")
-            return
-    
-    # Load the MuJoCo model
-    model_path = os.path.join(os.path.dirname(__file__), "franka_emika_panda/scene.xml")
-    model = mujoco.MjModel.from_xml_path(model_path)
-
-    
-    data = mujoco.MjData(model)
-    data.qpos[:7] = np.array([0, 0, 0, -1.57079, 0, 1.57079, -0.7853]) # home position
-    mujoco.mj_forward(model, data)
-    
-    controller = RobotController()
-    controller.initialize(model, data)
-    
-    with mujoco.viewer.launch_passive(model, data) as viewer:
-        print("Simulation initialized successfully")
-        print("\nGamepad control mapping:")
-        print("  Left stick X: Move along X axis")
-        print("  Left stick Y: Move along Y axis")
-        print("  Right stick Y: Move along Z axis")
-        print("  LB/RB: Decrease/Increase speed")
-        print("  START: Reset position")
-        print("  X: Close gripper")
-        print("  Y: Open gripper")
-
-        input_received = False
-        
-        for event in node:
-            if event["type"] == "INPUT":
-                input_received = True
-                
-                if event["id"] == "raw_control":
-                    raw_control = json.loads(event["value"].to_pylist()[0])
-                    controller.process_gamepad_input(raw_control, model, data)
-                    
-                elif event["id"] == "target_pose":
-                    target_pose = event["value"].to_pylist()
-                    # print(f"Received target pose: {target_pose}")
-                    controller.process_target_pose(target_pose, model, data)
-            
-            if input_received:
-                if controller.last_input_source == "target_pose":
-                    controller._apply_cartesian_control(model, data)
-                
-                # Step simulation and update viewer
-                mujoco.mj_step(model, data)
-                viewer.sync()
-                
-                # Send feedback
-                node.send_output(
-                    "joint_positions",
-                    data=pa.array(data.qpos[:7].tolist(), type=pa.float64()),
-                    metadata={"type": "joint_positions"}
-                )
-                
-                node.send_output(
-                    "ee_position",
-                    data=pa.array(data.body(controller.hand_id).xpos.tolist(), 
-                                    type=pa.float64()),
-                    metadata={"type": "ee_position"}
-                )
-            
-            else:
-                mujoco.mj_step(model, data)
-                viewer.sync()
-
-
-if __name__ == "__main__":
-    main()

node-hub/dora-franka-mujoco/dora_franka_mujoco/mesh_downloader.py

@@ -1,67 +0,0 @@
-# filepath: /dora-mujoco-sim/src/dora_mujoco_sim/mesh_downloader.py
-"""Mesh file downloader for MuJoCo simulation."""
-
-from pathlib import Path
-from huggingface_hub import hf_hub_download
-
-def get_required_mesh_files():
-    """Get list of required mesh files from names.txt"""
-    names_file = Path(__file__).parent / "franka_emika_panda" / "assets" / "names.txt"
-    
-    with open(names_file) as f:
-        return [line.strip() for line in f if line.strip() and (line.strip().endswith('.stl') or line.strip().endswith('.obj'))]
-    
-MESH_FILES = get_required_mesh_files()
-REPO_ID = "SGPatil/Mujoco_franka_meshes"
-REPO_TYPE = "dataset"
-
-def get_cache_dir():
-    cache_dir = Path.home() / ".cache" / "dora-mujoco-franka" / "meshes"
-    cache_dir.mkdir(parents=True, exist_ok=True)
-    return cache_dir
-
-def check_mesh_files_exist():
-    """Check if all required mesh files exist locally"""
-    mesh_dir = Path(__file__).parent / "franka_emika_panda" / "assets"
-    
-    missing_files = []
-    for filename in MESH_FILES:
-        mesh_file = mesh_dir / filename
-        if not mesh_file.exists():
-            missing_files.append(filename)
-    
-    if missing_files:
-        print(f"Missing {len(missing_files)} mesh files")
-        return False
-    
-    print(f"All {len(MESH_FILES)} mesh files are present")
-    return True
-
-def ensure_meshes():
-    """Download and install required mesh files for the simulation if they are not already present in the local cache."""
-    mesh_dir = Path(__file__).parent / "franka_emika_panda" / "assets"
-    mesh_dir.mkdir(parents=True, exist_ok=True)
-    
-    print("Checking mesh files...")
-    try:
-        for filename in MESH_FILES:
-            # Download file from Hugging Face Hub
-            downloaded_path = hf_hub_download(
-                repo_id=REPO_ID,
-                filename=filename,
-                repo_type=REPO_TYPE,
-                cache_dir=get_cache_dir()
-            )
-            
-            mesh_file = mesh_dir / filename
-            if not mesh_file.exists():
-                mesh_file.write_bytes(Path(downloaded_path).read_bytes())
-                
-    except Exception as e:
-        print(f"Error downloading mesh files: {e}")
-        raise e
-    
-    print("All mesh files are ready")
-
-if __name__ == "__main__":
-    ensure_meshes()

node-hub/dora-franka-mujoco/pyproject.toml

@@ -1,39 +0,0 @@
-[project]
-name = "dora-franka-mujoco"
-version = "0.1.0"
-authors = [{ name = "Your Name", email = "email@email.com" }]
-description = "MuJoCo-based Franka Emika Panda robot simulation node for Dora"
-license = { text = "MIT" }
-readme = "README.md"
-requires-python = ">=3.8"
-
-dependencies = [
-    "dora-rs >= 0.3.9",
-    "mujoco >= 3.1.6",
-    "numpy >= 1.21.0",
-    "pyarrow >= 14.0.1",
-    "scipy >= 1.7.0",
-    "huggingface_hub >= 0.16.0",
-]
-
-[dependency-groups]
-dev = ["pytest >=8.1.1", "ruff >=0.9.1"]
-
-[project.scripts]
-dora-franka-mujoco = "dora_franka_mujoco.main:main"
-
-[tool.setuptools.package-data]
-dora_franka_mujoco = [
-    "franka_emika_panda/**/*",
-    "franka_emika_panda/assets/*",
-    "franka_emika_panda/assets/names.txt",
-]
-
-[tool.ruff.lint]
-extend-select = [
-  "UP",   # Ruff's UP rule
-  "PERF", # Ruff's PERF rule
-  "RET",  # Ruff's RET rule
-  "RSE",  # Ruff's RSE rule
-  "NPY",  # Ruff's NPY rule
-]

node-hub/dora-franka-mujoco/tests/test_dora_franka_mujoco.py

@@ -1,9 +0,0 @@
-import pytest
-
-
-def test_import_main():
-    from dora_franka_mujoco.main import main
-
-    # Check that everything is working, and catch dora Runtime Exception as we're not running in a dora dataflow.
-    with pytest.raises(RuntimeError):
-        main()

node-hub/dora-mujoco-sim/dora_mujoco_sim/main.py

@@ -1,154 +0,0 @@
-"""MuJoCo simulation node for Dora with robot descriptions support."""
-
-import numpy as np
-import pyarrow as pa
-import mujoco
-import mujoco.viewer
-from dora import Node
-import json
-from pathlib import Path
-import time
-from typing import Dict, Any
-from robot_descriptions.loaders.mujoco import load_robot_description
-
-
-class MuJoCoSimulator:
-    """MuJoCo simulator for Dora."""
-    
-    def __init__(self, model_path_or_name: str = "go2"):
-        """Initialize the MuJoCo simulator."""
-        self.model_path_or_name = model_path_or_name
-        self.model = None
-        self.data = None
-        self.viewer = None
-        self.state_data = {}
-        self.model_mapping = self._load_model_mapping()
-        
-    def _load_model_mapping(self) -> dict:
-        """Load robot model mapping from JSON file."""
-        config_path = Path(__file__).parent / "robot_models.json"
-
-        with open(config_path) as f:
-            mapping_data = json.load(f)
-        
-        model_mapping = {}
-        for models in mapping_data.values():
-            model_mapping.update(models)
-            
-        return model_mapping
-            
-
-    def load_model(self) -> bool:
-        """Load MuJoCo model from path or robot description name."""
-        model_path = Path(self.model_path_or_name)
-        if model_path.exists() and model_path.suffix == '.xml':
-            # print(f"Loading model from direct path: {model_path}")
-            self.model = mujoco.MjModel.from_xml_path(str(model_path))
-            
-        else:
-            # Treat as model name - robot_descriptions 
-            description_name = self.model_mapping.get(
-                self.model_path_or_name, 
-                f"{self.model_path_or_name}_mj_description"
-            )
-            self.model = load_robot_description(description_name, variant="scene")
-            
-        # Initialize simulation data
-        self.data = mujoco.MjData(self.model)
-        
-        # Set control to neutral position
-        if self.model.nkey > 0:
-            mujoco.mj_resetDataKeyframe(self.model, self.data, 0)
-        else:
-            mujoco.mj_resetData(self.model, self.data)
-            
-        # Initialize state data
-        self._update_state_data()
-        return True
-
-
-    def step_simulation(self):
-        """Step the simulation forward."""
-        mujoco.mj_step(self.model, self.data)
-        self._update_state_data()
-    
-    def _update_state_data(self):
-        """Update state data that can be sent via Dora."""
-        self.state_data = {
-            "time": self.data.time,                    # Current simulation time
-            "qpos": self.data.qpos.copy(),            # Joint positions  
-            "qvel": self.data.qvel.copy(),            # Joint velocities
-            "qacc": self.data.qacc.copy(),            # Joint accelerations
-            "ctrl": self.data.ctrl.copy(),            # Control inputs/actuator commands
-            "qfrc_applied": self.data.qfrc_applied.copy(),  # External forces applied to joints
-            "sensordata": self.data.sensordata.copy() if self.model.nsensor > 0 else np.array([])  # Sensor readings
-        }
-    
-    def get_state(self) -> Dict[str, Any]:
-        """Get current simulation state."""
-        return self.state_data.copy()
-    
-
-
-def main():
-    """Run the main Dora node function."""
-    node = Node()
-    
-    # Configuration - can be either a model name or direct path
-    model_path_or_name = "go2"  # Examples: "go2", "franka_panda", "/path/to/scene.xml"
-    
-    # Initialize simulator
-    simulator = MuJoCoSimulator(model_path_or_name)
-    
-    # Load model
-    if not simulator.load_model():
-        print("Failed to load MuJoCo model")
-        return
-        
-    print("MuJoCo simulation node started")
-    
-    # Launch viewer (optional - comment out for headless)
-    with mujoco.viewer.launch_passive(simulator.model, simulator.data) as viewer:
-        print("Viewer launched")
-        
-        # Main Dora event loop
-        for event in node:
-            # Always step simulation
-            simulator.step_simulation()
-            viewer.sync()
-            
-            if event["type"] == "INPUT":
-                state = simulator.get_state()
-
-                # Get current time
-                current_time = state.get("time", time.time())
-                
-                # Send joint positions
-                if "qpos" in state:
-                    print(f"Sending joint positions: {state['qpos']}")
-                    node.send_output(
-                        "joint_positions", 
-                        pa.array(state["qpos"]), 
-                        {"timestamp": current_time}
-                    )
-                
-                # Send joint velocities
-                if "qvel" in state:
-                    node.send_output(
-                        "joint_velocities", 
-                        pa.array(state["qvel"]), 
-                        {"timestamp": current_time}
-                    )
-                
-                # Send sensor data if available
-                if "sensordata" in state and len(state["sensordata"]) > 0:
-                    node.send_output(
-                        "sensor_data", 
-                        pa.array(state["sensordata"]), 
-                        {"timestamp": current_time}
-                    )
-
-
-
-if __name__ == "__main__":
-    main()

node-hub/dora-mujoco-sim/tests/test_dora_mujoco_sim.py

@@ -1,13 +0,0 @@
-"""Test module for dora_mujoco_sim package."""
-
-import pytest
-
-
-def test_import_main():
-    """Test importing and running the main function."""
-    from dora_mujoco_sim.main import main
-
-    # Check that everything is working, and catch Dora RuntimeError
-    # as we're not running in a Dora dataflow.
-    with pytest.raises(RuntimeError):
-        main()

node-hub/dora-mujoco-sim/README.md → node-hub/dora-mujoco/README.md

@@ -1,6 +1,6 @@
-# dora-mujoco-sim
+# dora-mujoco
 
-A MuJoCo physics simulation node for the Dora dataflow framework. This node provides real-time physics simulation with support for a wide range of robots from the `robot_descriptions` package, as well as custom robot models.
+A MuJoCo physics simulation node for the Dora dataflow framework. This node provides real-time physics simulation with support for a wide range of robots from the `robot_descriptions` package, as well as custom robot models. Designed for modular robotics control architectures.
 
 ## Features
 
@@ -8,6 +8,7 @@ A MuJoCo physics simulation node for the Dora dataflow framework. This node prov
 - **Flexible Model Loading**: Load robots by name (via robot_descriptions) or direct XML file paths
 - **Real-time Simulation**: Continuous physics simulation with configurable tick rates
 - **Live Visualization**: Optional MuJoCo viewer for real-time 3D visualization
+- **Generic Control Interface**: Accepts control commands for any robot type
 - **Rich Data Output**: Joint positions, velocities, accelerations, and sensor data
 
 
@@ -55,10 +56,35 @@ The simulator supports all models from the `robot_descriptions` package. Common
 
 - **Quadrupeds**: `go1`, `go2`, `a1`, `aliengo`, `anymal_b`, `anymal_c`, `spot`
 - **Humanoids**: `g1`, `h1`, `apollo`, `talos`, `jvrc`, `cassie`
-- **Arms**: `franka_panda`, `ur5e`, `ur10e`, `iiwa14`, `gen3`, `sawyer`
+- **Arms**: `panda`, `ur5e`, `ur10e`, `iiwa14`, `gen3`, `sawyer`
 - **Hands**: `allegro_hand`, `shadow_hand`, `leap_hand`, `robotiq_2f85`
 
-See [`robot_models.json`](dora_mujoco_sim/robot_models.json) for the complete list.
+See [`robot_models.json`](dora_mujoco/robot_models.json) for the complete list.
+
+## Architecture
+
+The `dora-mujoco` node is designed to be **robot-agnostic** and work with **robot-specific controllers**:
+
+```
+┌─────────────────┐    
+│  Command Node   │    target_pose/cmd_vel
+│  (Gamepad, etc) │─────────────┐
+└─────────────────┘             │
+                                ▼
+┌─────────────────┐    control_input   ┌─────────────────┐
+│ Robot Controller│───────────────────▶│  dora-mujoco    │
+│ (Franka, Husky) │                    │                 │
+└─────────────────┘                    │ ┌─────────────┐ │    joint_positions
+           ▲                           │ │  Simulator  │ │──────────────────▶
+           │        joint_positions    │ │             │ │    joint_velocities  
+           └───────────────────────────│ │  ┌────────┐ │ │──────────────────▶
+                                       │ │  │Renderer│ │ │    actuator_controls
+                                       │ │  └────────┘ │ │──────────────────▶
+                                       │ │             │ │    sensor_data
+                                       │ └─────────────┘ │──────────────────▶
+                                       └─────────────────┘
+the control nodes are optional (the robot will still spawn without them)
+```
 
 ## YAML Specification
 
@@ -67,7 +93,7 @@ See [`robot_models.json`](dora_mujoco_sim/robot_models.json) for the complete li
 nodes:
   - id: mujoco_sim
     build: pip install -e .
-    path: dora-mujoco-sim
+    path: dora-mujoco
     env:
       MODEL_NAME: "go2"  # Robot model name or file path
     inputs:
@@ -79,35 +105,41 @@ nodes:
 ```
 
 ### Input Specification
-- **TICK**: Timer events that trigger simulation steps and data output
-  - Format: `dora/timer/millis/<interval>`
-  - Example: `dora/timer/millis/1` for 1000Hz updates
+| Input | Type | Description |
+|-------|------|-------------|
+| `tick` | Timer | Triggers simulation steps and data output |
+| `control_input` | `pa.array(float64)` | Control commands for actuators |
+
+**Control Input Format**:
+- **Manipulator Arms**: Joint position commands `[q1, q2, ..., q7, gripper]`
+- **Mobile Robots**: Wheel velocity commands `[wheel1, wheel2, wheel3, wheel4]`
+- **Quadrupeds**: Joint position commands `[hip1, thigh1, calf1, ...]`
 
 ### Output Specification
 | Output | Type | Description | Metadata |
 |--------|------|-------------|----------|
 | `joint_positions` | `pa.array(float64)` | Joint positions (qpos) | `timestamp` |
 | `joint_velocities` | `pa.array(float64)` | Joint velocities (qvel) | `timestamp` |
-| `sensor_data` | `pa.array(float64)` | Sensor readings (yet to manage sensors, currently dumping raw output) | `timestamp` |
+| `actuator_controls` | `pa.array(float64)` | Current actuator commands | `timestamp` |
+| `sensor_data` | `pa.array(float64)` | Sensor readings (if available) | `timestamp` |
 
+## Configuration Options
 
-## Architecture
+### Environment Variables
+- `MODEL_NAME`: Robot model name or XML file path (default: "go2")
 
-```
-┌─────────────────┐    TICK    ┌─────────────────┐
-│  Dora Timer     │───────────▶│  MuJoCo Node    │
-└─────────────────┘            │                 │
-                               │ ┌─────────────┐ │    joint_positions
-                               │ │  Simulator  │ │──────────────────▶
-                               │ │             │ │    joint_velocities  
-                               │ │  ┌────────┐ │ │──────────────────▶
-                               │ │  │Renderer│ │ │    sensor_data
-                               │ │  └────────┘ │ │──────────────────▶
-                               │ │             │ │
-                               │ └─────────────┘ |
-                               └─────────────────┘
+### Custom Robot Models
+```python
+# Use direct XML file path
+env:
+  MODEL_NAME: "/path/to/my_robot/scene.xml"
 ```
 
+### Simulation Parameters
+- **Physics Timestep**: Fixed at MuJoCo default (0.002s = 500Hz)
+- **Output Rate**: Controlled by `tick` input frequency
+- **Control Rate**: Determined by `control_input` frequency
+
 ## Development
 
 ### Code Quality
@@ -126,7 +158,7 @@ uv run pytest .
 ### Adding New Models
 To add support for new robot models:
 
-1. Add the model mapping to [`robot_models.json`](dora_mujoco_sim/robot_models.json):
+1. Add the model mapping to [`robot_models.json`](dora_mujoco/robot_models.json):
 ```json
 {
   "category_name": {
@@ -137,12 +169,14 @@ To add support for new robot models:
 
 2. Ensure the model is available in `robot_descriptions` or provide the XML file path directly.
 
-## Configuration
+### Creating Robot Controllers
+
+Robot-specific controllers should:
+1. Subscribe to `joint_positions` from the simulation
+2. Implement robot-specific control logic (IK, dynamics, etc.)
+3. Publish `control_input` commands to the simulation
 
 
-### Simulation Parameters
-- **Timestep**: Fixed at MuJoCo default (0.002s = 500Hz physics)
-- **Output Rate**: Controlled by TICK input frequency
 
 ## Troubleshooting
 
@@ -163,9 +197,23 @@ To add support for new robot models:
    - Normal behavior - falls back to robot-only model
    - Scene variants include ground plane and lighting
 
-3. **Viewer issues**:
-   - Use `HEADLESS` for server environments
+3. **Control dimension mismatch**:
+   ```
+   WARNING: Control input size doesn't match actuators
+   ```
+   - Check that control commands match the robot's actuator count
+   - Use `print(model.nu)` to see expected control dimensions
+
+4. **Viewer issues**:
    - Ensure proper OpenGL/graphics drivers
+   - Use headless mode for server environments
+
+## Examples
+
+Complete working examples are available in:
+- `dora/examples/franka-mujoco-control/`
+  - Target pose control with Cartesian space control
+  - Gamepad control with real-time interaction
 
 ## License
 

node-hub/dora-mujoco-sim/demo.yml → node-hub/dora-mujoco/demo.yml

@@ -1,17 +1,17 @@
 nodes:
   - id: mujoco_sim
     build: pip install -e .
-    path: dora-mujoco-sim
+    path: dora-mujoco
     inputs:
       tick: dora/timer/millis/2  # 500 Hz data collection
         # control_input: controller/output
-        # render_request: visualizer/request
-        # change_model: config/model_change
     outputs:
       - joint_positions
       - joint_velocities 
+      - actuator_controls
       - sensor_data
-      - rendered_frame
+    env:
+      MODEL_NAME: "go2"  # Load Franka Panda from robot-descriptions
 
   # - id: controller
   #   # Your control logic node

node-hub/dora-mujoco/dora_mujoco/main.py

@@ -0,0 +1,222 @@
+"""MuJoCo simulation node for Dora with robot descriptions support."""
+
+import numpy as np
+import pyarrow as pa
+import mujoco
+import mujoco.viewer
+from dora import Node
+import json
+import os
+from pathlib import Path
+import time
+from typing import Dict, Any
+from robot_descriptions.loaders.mujoco import load_robot_description
+
+
+class MuJoCoSimulator:
+    """MuJoCo simulator for Dora."""
+    
+    def __init__(self, model_path_or_name: str = None):
+        """Initialize the MuJoCo simulator."""
+        # Check environment variable first, then use parameter, then default
+        self.model_path_or_name = (
+            os.getenv("MODEL_NAME") or 
+            model_path_or_name or 
+            "go2"
+        )
+        
+        self.model = None
+        self.data = None
+        self.viewer = None
+        self.state_data = {}
+        self.model_mapping = self._load_model_mapping()
+        
+        print(f"MuJoCo Simulator initialized with model: {self.model_path_or_name}")
+        
+    def _load_model_mapping(self) -> dict:
+        """Load robot model mapping from JSON file."""
+        config_path = Path(__file__).parent / "robot_models.json"
+
+        with open(config_path) as f:
+            mapping_data = json.load(f)
+        
+        model_mapping = {}
+        for models in mapping_data.values():
+            model_mapping.update(models)
+            
+        return model_mapping
+            
+
+    def load_model(self) -> bool:
+        """Load MuJoCo model from path or robot description name."""
+        try:
+            model_path = Path(self.model_path_or_name)
+            if model_path.exists() and model_path.suffix == '.xml':
+                print(f"Loading model from direct path: {model_path}")
+                self.model = mujoco.MjModel.from_xml_path(str(model_path))
+                
+            else:
+                # Treat as model name - robot_descriptions 
+                description_name = self.model_mapping.get(
+                    self.model_path_or_name, 
+                    f"{self.model_path_or_name}_mj_description"
+                )
+                print(f"Loading model '{self.model_path_or_name}' using robot description: {description_name}")
+                self.model = load_robot_description(description_name, variant="scene")
+                
+        except Exception as e:
+            print(f"Error loading model '{self.model_path_or_name}': {e}")
+            print("Available models:")
+            for category, models in self._get_available_models().items():
+                print(f"  {category}: {', '.join(models.keys())}")
+            return False
+            
+        # Initialize simulation data
+        self.data = mujoco.MjData(self.model)
+        
+        # Set control to neutral position
+        if self.model.nkey > 0:
+            mujoco.mj_resetDataKeyframe(self.model, self.data, 0)
+        else:
+            mujoco.mj_resetData(self.model, self.data)
+        
+        # Print model info for debugging
+        print(f"Model loaded successfully:")
+        print(f"  DOF (nq): {self.model.nq}")
+        print(f"  Velocities (nv): {self.model.nv}")
+        print(f"  Actuators (nu): {self.model.nu}")
+        print(f"  Control inputs: {len(self.data.ctrl)}")
+        
+        # Print actuator info
+        if self.model.nu > 0:
+            print("Actuators:")
+            for i in range(self.model.nu):
+                actuator_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
+                joint_id = self.model.actuator_trnid[i, 0]  # First transmission joint
+                joint_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) if joint_id >= 0 else "N/A"
+                ctrl_range = self.model.actuator_ctrlrange[i]
+                print(f"  [{i}] {actuator_name or f'actuator_{i}'} -> joint: {joint_name} | range: [{ctrl_range[0]:.2f}, {ctrl_range[1]:.2f}]")
+            
+        # Initialize state data
+        self._update_state_data()
+        return True
+
+    def apply_control(self, control_input: np.ndarray):
+        """Apply control input to the simulation.
+        
+        Args:
+            control_input: Control values for actuators
+        """
+        if control_input is None or len(control_input) == 0:
+            return
+            
+        # Ensure we don't exceed the number of actuators
+        n_controls = min(len(control_input), self.model.nu)
+        
+        # Apply control directly to actuators 
+        for i in range(n_controls):
+            # Apply joint limits if available
+            ctrl_range = self.model.actuator_ctrlrange[i]
+            if ctrl_range[0] < ctrl_range[1]:  # Valid range
+                control_value = np.clip(control_input[i], ctrl_range[0], ctrl_range[1])
+            else:
+                control_value = control_input[i]
+            
+            self.data.ctrl[i] = control_value
+
+    def _get_available_models(self) -> dict:
+        """Get available models from the mapping file."""
+        config_path = Path(__file__).parent / "robot_models.json"
+        with open(config_path) as f:
+            return json.load(f)
+
+    def step_simulation(self):
+        """Step the simulation forward."""
+        mujoco.mj_step(self.model, self.data)
+        self._update_state_data()
+    
+    def _update_state_data(self):
+        """Update state data that can be sent via Dora."""
+        self.state_data = {
+            "time": self.data.time,                    # Current simulation time
+            "qpos": self.data.qpos.copy(),            # Joint positions  
+            "qvel": self.data.qvel.copy(),            # Joint velocities
+            "qacc": self.data.qacc.copy(),            # Joint accelerations
+            "ctrl": self.data.ctrl.copy(),            # Control inputs/actuator commands
+            "qfrc_applied": self.data.qfrc_applied.copy(),  # External forces applied to joints
+            "sensordata": self.data.sensordata.copy() if self.model.nsensor > 0 else np.array([])  # Sensor readings
+        }
+    
+    def get_state(self) -> Dict[str, Any]:
+        """Get current simulation state."""
+        return self.state_data.copy()
+
+
+def main():
+    """Run the main Dora node function."""
+    node = Node()
+    
+    # Initialize simulator
+    simulator = MuJoCoSimulator()
+    
+    # Load model
+    if not simulator.load_model():
+        print("Failed to load MuJoCo model")
+        return
+        
+    print("MuJoCo simulation node started")
+    
+    # Launch viewer
+    with mujoco.viewer.launch_passive(simulator.model, simulator.data) as viewer:        
+        print("MuJoCo viewer launched - simulation running")
+        
+        # Main Dora event loop
+        for event in node:
+            if event["type"] == "INPUT":
+                # Handle control input
+                if event["id"] == "control_input":
+                    control_array = event["value"].to_numpy()
+                    simulator.apply_control(control_array)
+                    # print(f"Applied control: {control_array[:7]}")  # Show first 7 values
+            
+            # Step simulation once per loop iteration
+            simulator.step_simulation()
+            viewer.sync()
+            
+            # Send outputs after stepping
+            if event["type"] == "INPUT":
+                state = simulator.get_state()
+                current_time = state.get("time", time.time())
+                
+                # Send joint positions
+                node.send_output(
+                    "joint_positions", 
+                    pa.array(state["qpos"]), 
+                    {"timestamp": current_time}
+                )
+                
+                # Send joint velocities
+                node.send_output(
+                    "joint_velocities", 
+                    pa.array(state["qvel"]), 
+                    {"timestamp": current_time}
+                )
+                
+                # Send actuator controls
+                node.send_output(
+                    "actuator_controls",
+                    pa.array(state["ctrl"]),
+                    {"timestamp": current_time}
+                )
+                
+                # Send sensor data if available
+                if len(state["sensordata"]) > 0:
+                    node.send_output(
+                        "sensor_data", 
+                        pa.array(state["sensordata"]), 
+                        {"timestamp": current_time}
+                    )
+
+
+if __name__ == "__main__":
+    main()

node-hub/dora-mujoco-sim/pyproject.toml → node-hub/dora-mujoco/pyproject.toml

@@ -1,5 +1,5 @@
 [project]
-name = "dora-mujoco-sim"
+name = "dora-mujoco"
 version = "0.1.0"
 authors = [{ name = "Your Name", email = "email@email.com" }]
 description = "MuJoCo simulation node for Dora"
@@ -19,7 +19,7 @@ dependencies = [
 dev = ["pytest >=8.1.1", "ruff >=0.9.1"]
 
 [project.scripts]
-dora-mujoco-sim = "dora_mujoco_sim.main:main"
+dora-mujoco = "dora_mujoco.main:main"
 
 [tool.ruff.lint]
 extend-select = [