diff --git a/examples/franka-mujoco-control/01_basic_simulation/README.md b/examples/franka-mujoco-control/01_basic_simulation/README.md
new file mode 100644
index 00000000..79ae2341
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/01_basic_simulation/basic.yml b/examples/franka-mujoco-control/01_basic_simulation/basic.yml
new file mode 100644
index 00000000..26104087
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/02_target_pose_control/README.md b/examples/franka-mujoco-control/02_target_pose_control/README.md
new file mode 100644
index 00000000..8b9b227a
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/02_target_pose_control/nodes/franka_controller.py b/examples/franka-mujoco-control/02_target_pose_control/nodes/franka_controller.py
new file mode 100644
index 00000000..a2123f68
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/02_target_pose_control/nodes/pose_publisher.py b/examples/franka-mujoco-control/02_target_pose_control/nodes/pose_publisher.py
new file mode 100644
index 00000000..e99b3273
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/02_target_pose_control/target_pose_control.yml b/examples/franka-mujoco-control/02_target_pose_control/target_pose_control.yml
new file mode 100644
index 00000000..5a122eb1
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/03_gamepad_control/README.md b/examples/franka-mujoco-control/03_gamepad_control/README.md
new file mode 100644
index 00000000..0b5be8bb
--- /dev/null
+++ b/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.
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/03_gamepad_control/gamepad_control.yml b/examples/franka-mujoco-control/03_gamepad_control/gamepad_control.yml
new file mode 100644
index 00000000..5fa4a493
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/03_gamepad_control/nodes/franka_gamepad_controller.py b/examples/franka-mujoco-control/03_gamepad_control/nodes/franka_gamepad_controller.py
new file mode 100644
index 00000000..6798d2aa
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/examples/franka-mujoco-control/README.md b/examples/franka-mujoco-control/README.md
new file mode 100644
index 00000000..1c4f4821
--- /dev/null
+++ b/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
+
diff --git a/node-hub/dora-franka-mujoco/README.md b/node-hub/dora-franka-mujoco/README.md
deleted file mode 100644
index 366b8d95..00000000
--- a/node-hub/dora-franka-mujoco/README.md
+++ /dev/null
@@ -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
diff --git a/node-hub/dora-franka-mujoco/demo.yml b/node-hub/dora-franka-mujoco/demo.yml
deleted file mode 100644
index e7098ffb..00000000
--- a/node-hub/dora-franka-mujoco/demo.yml
+++ /dev/null
@@ -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
\ No newline at end of file
diff --git a/node-hub/dora-franka-mujoco/dora_franka_mujoco/__init__.py b/node-hub/dora-franka-mujoco/dora_franka_mujoco/__init__.py
deleted file mode 100644
index cde7a377..00000000
--- a/node-hub/dora-franka-mujoco/dora_franka_mujoco/__init__.py
+++ /dev/null
@@ -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."
diff --git a/node-hub/dora-franka-mujoco/dora_franka_mujoco/__main__.py b/node-hub/dora-franka-mujoco/dora_franka_mujoco/__main__.py
deleted file mode 100644
index bcbfde6d..00000000
--- a/node-hub/dora-franka-mujoco/dora_franka_mujoco/__main__.py
+++ /dev/null
@@ -1,5 +0,0 @@
-from .main import main
-
-
-if __name__ == "__main__":
- main()
diff --git a/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/assets/names.txt b/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/assets/names.txt
deleted file mode 100644
index 7d36cefc..00000000
--- a/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/assets/names.txt
+++ /dev/null
@@ -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
diff --git a/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/panda.xml b/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/panda.xml
deleted file mode 100644
index 13838b95..00000000
--- a/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/panda.xml
+++ /dev/null
@@ -1,283 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/scene.xml b/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/scene.xml
deleted file mode 100644
index 493bdc2c..00000000
--- a/node-hub/dora-franka-mujoco/dora_franka_mujoco/franka_emika_panda/scene.xml
+++ /dev/null
@@ -1,38 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/node-hub/dora-franka-mujoco/dora_franka_mujoco/main.py b/node-hub/dora-franka-mujoco/dora_franka_mujoco/main.py
deleted file mode 100644
index 02e207f7..00000000
--- a/node-hub/dora-franka-mujoco/dora_franka_mujoco/main.py
+++ /dev/null
@@ -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()
\ No newline at end of file
diff --git a/node-hub/dora-franka-mujoco/dora_franka_mujoco/mesh_downloader.py b/node-hub/dora-franka-mujoco/dora_franka_mujoco/mesh_downloader.py
deleted file mode 100644
index e73c2a58..00000000
--- a/node-hub/dora-franka-mujoco/dora_franka_mujoco/mesh_downloader.py
+++ /dev/null
@@ -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()
\ No newline at end of file
diff --git a/node-hub/dora-franka-mujoco/pyproject.toml b/node-hub/dora-franka-mujoco/pyproject.toml
deleted file mode 100644
index cf4017c9..00000000
--- a/node-hub/dora-franka-mujoco/pyproject.toml
+++ /dev/null
@@ -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
-]
\ No newline at end of file
diff --git a/node-hub/dora-franka-mujoco/tests/test_dora_franka_mujoco.py b/node-hub/dora-franka-mujoco/tests/test_dora_franka_mujoco.py
deleted file mode 100644
index d3d93eed..00000000
--- a/node-hub/dora-franka-mujoco/tests/test_dora_franka_mujoco.py
+++ /dev/null
@@ -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()
diff --git a/node-hub/dora-mujoco-sim/dora_mujoco_sim/main.py b/node-hub/dora-mujoco-sim/dora_mujoco_sim/main.py
deleted file mode 100644
index 640ba098..00000000
--- a/node-hub/dora-mujoco-sim/dora_mujoco_sim/main.py
+++ /dev/null
@@ -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()
\ No newline at end of file
diff --git a/node-hub/dora-mujoco-sim/tests/test_dora_mujoco_sim.py b/node-hub/dora-mujoco-sim/tests/test_dora_mujoco_sim.py
deleted file mode 100644
index 4b862d87..00000000
--- a/node-hub/dora-mujoco-sim/tests/test_dora_mujoco_sim.py
+++ /dev/null
@@ -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()
diff --git a/node-hub/dora-mujoco-sim/README.md b/node-hub/dora-mujoco/README.md
similarity index 52%
rename from node-hub/dora-mujoco-sim/README.md
rename to node-hub/dora-mujoco/README.md
index 60344b99..e7f32ef6 100644
--- a/node-hub/dora-mujoco-sim/README.md
+++ b/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/`
- - 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
diff --git a/node-hub/dora-mujoco-sim/demo.yml b/node-hub/dora-mujoco/demo.yml
similarity index 76%
rename from node-hub/dora-mujoco-sim/demo.yml
rename to node-hub/dora-mujoco/demo.yml
index a3e67e65..e5825ad7 100644
--- a/node-hub/dora-mujoco-sim/demo.yml
+++ b/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
diff --git a/node-hub/dora-mujoco-sim/dora_mujoco_sim/__init__.py b/node-hub/dora-mujoco/dora_mujoco/__init__.py
similarity index 100%
rename from node-hub/dora-mujoco-sim/dora_mujoco_sim/__init__.py
rename to node-hub/dora-mujoco/dora_mujoco/__init__.py
diff --git a/node-hub/dora-mujoco-sim/dora_mujoco_sim/__main__.py b/node-hub/dora-mujoco/dora_mujoco/__main__.py
similarity index 100%
rename from node-hub/dora-mujoco-sim/dora_mujoco_sim/__main__.py
rename to node-hub/dora-mujoco/dora_mujoco/__main__.py
diff --git a/node-hub/dora-mujoco/dora_mujoco/main.py b/node-hub/dora-mujoco/dora_mujoco/main.py
new file mode 100644
index 00000000..fd2a9f7c
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/node-hub/dora-mujoco-sim/dora_mujoco_sim/robot_models.json b/node-hub/dora-mujoco/dora_mujoco/robot_models.json
similarity index 100%
rename from node-hub/dora-mujoco-sim/dora_mujoco_sim/robot_models.json
rename to node-hub/dora-mujoco/dora_mujoco/robot_models.json
diff --git a/node-hub/dora-mujoco-sim/pyproject.toml b/node-hub/dora-mujoco/pyproject.toml
similarity index 90%
rename from node-hub/dora-mujoco-sim/pyproject.toml
rename to node-hub/dora-mujoco/pyproject.toml
index 51e68b50..8ae35ffc 100644
--- a/node-hub/dora-mujoco-sim/pyproject.toml
+++ b/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 = [