added simple controller

examples/mujoco-sim/README.md

@@ -1,4 +1,4 @@
-# Franka MuJoCo Control Tutorial
+# MuJoCo Sim Tutorial
 
 This comprehensive tutorial demonstrates how to build a robot control system using Dora with the `dora-mujoco` simulation node and control logic.
 

examples/mujoco-sim/basic_simulation/README.md

@@ -13,7 +13,7 @@ The simulation runs at 500Hz and outputs:
 - Sensor data (if available)
 - Current simulation time
 
-## Running the Example
+### Running the Example
 
 ```bash
 cd basic_simulation
@@ -25,14 +25,14 @@ You should see:
 1. MuJoCo viewer window opens with GO2 robot
 2. Robot is effected by gravity (enabled by default)
 
-## What's Happening
+### What's Happening
 
-1. **Model Loading**: The `dora-mujoco` node loads the Franka model using `load_robot_description("go2_mj_description")`
+1. **Model Loading**: The `dora-mujoco` node loads the RoboDog (go2) model using `load_robot_description("go2_mj_description")`
 2. **Physics Loop**: Timer triggers simulation steps at 500Hz (This is default step time for Mujoco)
 3. **Data Output**: Joint states are published 
 4. **Visualization**: MuJoCo viewer shows real-time simulation
 
-## Configuration Details
+### Configuration Details
 
 The `basic.yml` configures:
 - Model name: `"go2"` you change this to other robots name

examples/mujoco-sim/gamepad_control/README.md

@@ -1,53 +1,117 @@
 # 03. Gamepad Control
 
-This example demonstrates real-time interactive control by connecting a gamepad to the controller. It builds upon the target pose control example by adding gamepad input processing. Shows how to integrate and implement teleoperation. 
+This example demonstrates real-time interactive control by connecting a gamepad to the controller. It builds upon the target pose control example by adding gamepad input processing for teleoperation of the robot arm.
+
+## Controller Types
+
+### 1. Basic Gamepad Control (`gamepad_control_basic.yml`)
+- **Direct IK approach**: Uses simple IK solver for immediate position updates
+- The movement fells jumpy
+
+### 2. Advanced Gamepad Control (`gamepad_control_advanced.yml`)
+- **Differential IK approach**: Smooth velocity-based control with Jacobian
+- **Smooth**: Continuous motion interpolation
+- **Use case**: Precise manipulation, smooth trajectories
+
+## Gamepad Controls
 
-### Gamepad Controls:
 - **D-pad Vertical**: Move along X-axis (forward/backward) 
 - **D-pad Horizontal**: Move along Y-axis (left/right)
 - **Right Stick Y**: Move along Z-axis (up/down)
 - **LB/RB**: Decrease/Increase movement speed (0.1-1.0x scale)
-- **START**: Reset to home position
+- **START**: Reset to home position [0.4, 0.0, 0.3]
 
-## Running the Example
+## Running the Examples
 
 1. **Connect a gamepad** (Xbox/PlayStation controller via USB or Bluetooth)
-2. **Run the simulation**:
+
+### Basic Gamepad Control
 ```bash
 cd gamepad_control
-dora build gamepad_control.yml
-dora run gamepad_control.yml
+dora build gamepad_control_basic.yml
+dora run gamepad_control_basic.yml
+```
+
+### Advanced Gamepad Control
+```bash
+cd gamepad_control
+dora build gamepad_control_advanced.yml
+dora run gamepad_control_advanced.yml
 ```
 
 You should see:
 1. Robot responds to gamepad input in real-time
-2. Smooth incremental movement based on D-pad/stick input
-3. Speed control with bumper buttons
-4. Reset functionality with START button
-5. GPU-accelerated kinematics computation (if CUDA available)
+2. **Basic**: Immediate position jumps based on gamepad input
+3. **Advanced**: Smooth incremental movement with velocity control
+4. Speed scaling with bumper buttons
+5. Reset functionality with START button
+
+
+### **Gamepad Node** (`gamepad`)
+Built-in Dora node that interfaces with system gamepad drivers using Pygame.
+
+```yaml
+- id: gamepad
+  build: pip install -e ../../../node-hub/gamepad
+  path: gamepad
+  outputs:
+    - cmd_vel     # 6DOF velocity commands
+    - raw_control # Raw button/stick states
+  inputs:
+    tick: dora/timer/millis/10  # 100Hz polling
+```
 
-#### **Gamepad Node** (`gamepad`)
-Built-in Dora node that interfaces with system gamepad drivers.
-- **Outputs**: 
-  - `cmd_vel`: 6DOF velocity commands `[linear_x, linear_y, linear_z, angular_x, angular_y, angular_z]`
-  - `raw_control`: Gamepad Input in Json format 
+- **`cmd_vel`**: 6DOF velocity array `[linear_x, linear_y, linear_z, angular_x, angular_y, angular_z]`
+  - Generated from D-pad and analog stick positions
+  - Continuous updates while controls are held
+  
+- **`raw_control`**: JSON format gamepad state
 
-#### **GamePad Controller** (`gamepad_controller.py`)
-This node processes gamepad input and translates it into target positions for the robot end-effector.
 
-- Receives continuous movement commands (`cmd_vel`) from D-pad and analog sticks
-- Processes discrete button presses (`raw_control`) for special functions
+### **Gamepad Controller Scripts**
 
-**Controlling the End-Effector with Gamepad**:
+**Basic Controller (`gamepad_controller_ik.py`)**:
+```
+Gamepad Input → Target Position Update → IK Request → Joint Commands
+```
+- Updates target position incrementally based on gamepad
+- Immediate position jumps
+
+**Advanced Controller (`gamepad_controller_differential_ik.py`)**:
+```
+Gamepad Input → Target Position → Pose Error → Velocity → Joint Commands
+```
+- Continuous pose error calculation and velocity control
+- Smooth interpolation between current and target poses
+- Real-time Jacobian-based control
+
+## Gamepad Mapping
 
-The controller takes the first 3 values (X, Y, Z movement) from the gamepad `cmd_vel`, updates Target Position continuously
+The controller expects standard gamepad layout: (The mapping may change based on controller)
 
-**Button Commands**:
-    - **START button**: resets end-effector to home position [0.4, 0.0, 0.3]
-    - **Left Bumper (LB)**: Decreases movement speed 
-    - **Right Bumper (RB)**: Increases movement speed 
+| Control | Function |
+|---------|----------|
+| D-pad Up/Down |  X-axis movement |
+| D-pad Left/Right |  Y-axis movement |
+| Right Stick Y |  Z-axis movement |
+| Left Bumper |  Decrease speed |
+| Right Bumper |  Increase speed |
+| START button |  Reset position |
 
-The end-effector moves smoothly as you hold the controls, with position updates sent to the inverse kinematics solver to calculate required joint angles.
+## Key Features
+
+**Real-time Teleoperation:**
+- Incremental position updates based on continuous input
+- Immediate feedback through robot motion
+
+**Speed Control:**
+- Dynamic speed scaling (0.1x to 1.0x)
+- Allows both coarse and fine manipulation
+
+**Features:**
+- Home position reset capability
+- Bounded movement through incremental updates
+- Working on collision avoidance
 
 ## Troubleshooting
 
@@ -55,21 +119,16 @@ The end-effector moves smoothly as you hold the controls, with position updates
 ```bash
 # Check if gamepad is connected
 ls /dev/input/js*
-# Test gamepad input
+# Test gamepad input  
 jstest /dev/input/js0
+# Grant permissions
+sudo chmod 666 /dev/input/js0
 ```
 
 **"Robot doesn't move with D-pad"**
 - Check `cmd_vel` output: should show non-zero values when D-pad pressed
-- Verify controller processes `cmd_vel` events
-- Ensure your gamepad has correct mapping.
-
-
-## Real Robot Deployment
+- Verify correct gamepad mapping for your controller model
 
-For actual robot control:
-1. **Replace MuJoCo**: Use real robot drivers
-2. **Safety Limits**: Add emergency stops and workspace bounds
-3. **Force Control**: Integrate force/torque feedback
-4. **Network Latency**: Consider wireless controller delay
-5. **Deadman Switch**: Require constant button hold for safety
+**"Movement too fast/slow"**
+- Use LB/RB buttons to adjust speed scale
+- Modify `delta = cmd_vel[:3] * 0.03` scaling factor in code if required 

examples/mujoco-sim/gamepad_control/gamepad_control.yml → examples/mujoco-sim/gamepad_control/gamepad_control_advanced.yml

@@ -23,7 +23,7 @@ nodes:
       MODEL_NAME: "kuka_iiwa14"
 
   - id: gamepad_controller
-    path: nodes/gamepad_controller.py
+    path: nodes/gamepad_controller_differential_ik.py
     inputs:
       joint_positions: mujoco_sim/joint_positions
       joint_velocities: mujoco_sim/joint_velocities

examples/mujoco-sim/gamepad_control/gamepad_control_basic.yml

@@ -0,0 +1,48 @@
+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: gamepad_controller/joint_commands
+    outputs:
+      - joint_positions
+      - joint_velocities 
+      - actuator_controls
+      - sensor_data
+    env:
+      MODEL_NAME: "kuka_iiwa14"
+
+  - id: gamepad_controller
+    path: nodes/gamepad_controller_ik.py
+    inputs:
+      joint_positions: mujoco_sim/joint_positions
+      raw_control: gamepad/raw_control
+      cmd_vel: gamepad/cmd_vel
+      ik_request: pytorch_kinematics/ik_request
+    outputs:
+      - joint_commands
+      - ik_request
+      - joint_state
+
+  - id: pytorch_kinematics
+    build: pip install -e ../../../node-hub/dora-pytorch-kinematics
+    path: dora-pytorch-kinematics
+    inputs:
+      ik_request: gamepad_controller/ik_request
+      joint_state: gamepad_controller/joint_state
+    outputs:
+      - ik_request
+    env:
+      URDF_PATH: "../URDF/kuka_iiwa/model.urdf"
+      END_EFFECTOR_LINK: "lbr_iiwa_link_7"
+      TRANSFORM: "0. 0. 0. 1. 0. 0. 0."

examples/mujoco-sim/gamepad_control/nodes/gamepad_controller.py → examples/mujoco-sim/gamepad_control/nodes/gamepad_controller_differential_ik.py

@@ -5,7 +5,7 @@ import pyarrow as pa
 from dora import Node
 from scipy.spatial.transform import Rotation
 
-class GamepadFrankaController:
+class GamepadController:
     def __init__(self):
         # Control parameters
         self.integration_dt = 0.1
@@ -30,7 +30,7 @@ class GamepadFrankaController:
         self.current_ee_pose = None  # End-effector pose
         self.current_jacobian = None  # Jacobian matrix
         
-        print("Gamepad Franka Controller initialized")
+        print("Gamepad Controller initialized")
     
     def _initialize_robot(self, positions, jacobian_dof=None):
         self.full_joint_count = len(positions)
@@ -104,14 +104,13 @@ class GamepadFrankaController:
         diag = self.damping * np.eye(6)
         dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, twist)
         
-        # Apply nullspace control if we have redundant DOF
-        if self.dof > 6:
-            current_arm = self.current_joint_pos[:self.dof]
-            jac_pinv = np.linalg.pinv(jac)
-            N = np.eye(self.dof) - jac_pinv @ jac
-            k_null = np.ones(self.dof) * 5.0
-            dq_null = k_null * (self.home_pos - current_arm)
-            dq += N @ dq_null
+        # # Apply nullspace control
+        # current_arm = self.current_joint_pos[:self.dof]
+        # jac_pinv = np.linalg.pinv(jac)
+        # N = np.eye(self.dof) - jac_pinv @ jac
+        # k_null = np.ones(self.dof) * 5.0
+        # dq_null = k_null * (self.home_pos - current_arm)
+        # dq += N @ dq_null
         
         # Limit joint velocities
         dq_abs_max = np.abs(dq).max()
@@ -127,9 +126,7 @@ class GamepadFrankaController:
 
 def main():
     node = Node()
-    controller = GamepadFrankaController()
-    
-    print("Gamepad Franka Controller Started")
+    controller = GamepadController()
     
     for event in node:
         if event["type"] == "INPUT":

examples/mujoco-sim/gamepad_control/nodes/gamepad_controller_ik.py

@@ -0,0 +1,106 @@
+import json
+import time
+import numpy as np
+import pyarrow as pa
+from dora import Node
+
+class GamepadController:
+    def __init__(self):
+        """
+        Initialize the simple gamepad controller
+        """
+        # Robot state variables
+        self.dof = None 
+        self.current_joint_pos = None
+        
+        # Target pose (independent of DOF)
+        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.ik_request_sent = True 
+
+        
+        print("Simple Gamepad Controller initialized")
+    
+    def _initialize_robot(self, positions):
+        """Initialize controller state with appropriate dimensions"""
+        self.full_joint_count = len(positions)
+        self.current_joint_pos = positions.copy()
+        if self.dof is None:
+            self.dof = self.full_joint_count
+    
+    def process_cmd_vel(self, cmd_vel):
+        """Process gamepad velocity commands to update target position"""
+        delta = cmd_vel[:3] * 0.03
+        dx, dy, dz = delta
+        
+        # Update target position incrementally
+        if abs(dx) > 0 or abs(dy) > 0 or abs(dz) > 0:
+            self.target_pos += np.array([dx, -dy, dz])
+            self.ik_request_sent = False 
+
+    
+    def process_gamepad_input(self, raw_control):
+        """Process gamepad button inputs"""
+        buttons = raw_control["buttons"]
+
+        # Reset position with START button
+        if len(buttons) > 9 and buttons[9]:
+            self.target_pos = np.array([0.4, 0.0, 0.3])
+            print("Reset target to home position")
+            self.ik_request_sent = False 
+    
+    def get_target_pose_array(self):
+        """Get target pose as 6D array [x, y, z, roll, pitch, yaw]"""
+        return np.concatenate([self.target_pos, self.target_rpy])
+
+def main():
+    node = Node()
+    controller = GamepadController()
+    
+    for event in node:
+        if event["type"] == "INPUT":
+            if event["id"] == "joint_positions":
+                joint_pos = event["value"].to_numpy()
+
+                if controller.current_joint_pos is None:
+                    controller._initialize_robot(joint_pos)
+                else:
+                    controller.current_joint_pos = joint_pos
+                
+                # Request IK solution directly
+                target_pose = controller.get_target_pose_array()
+                if not controller.ik_request_sent:
+                    node.send_output(
+                        "ik_request", 
+                        pa.array(target_pose, type=pa.float32()),
+                        metadata={"encoding": "xyzrpy", "timestamp": time.time()}
+                    )
+                    controller.ik_request_sent = True 
+
+                node.send_output(
+                    "joint_state", 
+                    pa.array(joint_pos, type=pa.float32()),
+                    metadata={"encoding": "jointstate", "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"] == "cmd_vel":
+                cmd_vel = event["value"].to_numpy()
+                controller.process_cmd_vel(cmd_vel)
+            
+            # Handle IK results and send directly as joint commands
+            elif event["id"] == "ik_request":
+                if event["metadata"]["encoding"] == "jointstate":
+                    ik_solution = event["value"].to_numpy()
+                    # Send IK solution directly as joint commands
+                    node.send_output(
+                        "joint_commands", 
+                        pa.array(ik_solution, type=pa.float32()),
+                        metadata={"timestamp": time.time()}
+                    )
+
+if __name__ == "__main__":
+    main()

examples/mujoco-sim/target_pose_control/README.md

@@ -1,19 +1,39 @@
 # 02. Target Pose Control
 
-This example demonstrates Cartesian space control by creating a Controller node that processes target pose commands and outputs joint commands using inverse kinematics using **dora-pytorch-kinematics**.
+This example demonstrates Cartesian space control using two different approaches: **Direct Inverse Kinematics(IK)**(Basic) and **Differential IK**(advance). Both create a Controller node that processes target pose commands and outputs joint commands using **dora-pytorch-kinematics**.
 
-## Running the Example
+## Controller Types
 
+### 1. Direct IK Controller (`control.yml`)
+- **Simple approach**: Directly passes target pose to IK solver
+- **Fast**: Single IK computation per target pose
+
+### 2. Differential IK Controller (`control_advanced.yml`)  
+- **Smooth approach**: Uses pose error feedback and Jacobian-based control
+- **Continuous**: Interpolates smoothly between current and target poses
+- **Use case**: Smooth trajectories
+
+## Running the Examples
+
+### Direct IK Control
+```bash
+cd target_pose_control
+dora build control.yml
+dora run control.yml
+```
+
+### Differential IK Control
 ```bash
 cd target_pose_control
-dora build target_pose_control.yml
-dora run target_pose_control.yml
+dora build control_advanced.yml
+dora run control_advanced.yml
 ```
 
 You should see:
 1. Robot moves to predefined target poses automatically
-2. Smooth Cartesian space motion with differential inverse kinematics
-3. End-effector following target positions accurately
+2. **Direct IK**: Immediate jumps to target poses
+3. **Differential IK**: Smooth Cartesian space motion with continuous interpolation
+4. End-effector following target positions accurately
 
 
 ### Nodes
@@ -30,70 +50,118 @@ class PosePublisher:
         ]
 ```
 
-- Sends target poses every 10 seconds
+- Sends target poses every 5 seconds
 - Cycles through predefined positions and orientations
 - Can be replaced with path planning, user input, or any pose generation logic
 - Outputs `target_pose` array `[x, y, z, roll, pitch, yaw]` 
 
-#### 2. **Controller Script** (`controller.py`)
+#### 2. **Controller Scripts**
+
+##### Direct IK Controller (`controller_ik.py`)
+
+**How it works:**
+1. **Target Input**: Receives new target pose `[x, y, z, roll, pitch, yaw]`
+2. **IK Request**: Sends target pose directly to `dora-pytorch-kinematics`
+3. **Joint Solution**: Receives complete joint configuration for target pose
+4. **Direct Application**: Passes IK solution directly as joint commands to robot *(sometimes for certain target pose there is no IK solution)*
+
+**Advantages:**
+- Simple and fast0
+- Minimal computation
+- Direct pose-to-joint mapping
+
+**Disadvantages:**
+- Sudden jumps between poses
+- No trajectory smoothing
+- May cause joint velocity spikes 
+
+##### Differential IK Controller (`controller_differential_ik.py`)
+
+**How it works:**
+1. **Pose Error Calculation**: Computes difference between target and current end-effector pose
+2. **Velocity Command**: Converts pose error to desired end-effector velocity using PD control:
+   ```python
+   pos_error = target_pos - current_ee_pos
+   twist[:3] = Kpos * pos_error / integration_dt  # Linear velocity
+   twist[3:] = Kori * rot_error / integration_dt  # Angular velocity
+   ```
+3. **Jacobian Inverse**: Uses robot Jacobian to map end-effector velocity to joint velocities:
+   ```python
+   # Damped least squares to avoid singularities
+   dq = J^T @ (J @ J^T + λI)^(-1) @ twist
+   ```
+4. **Interpolation**: Integrates joint velocities to get next joint positions:
+   ```python
+   new_joints = current_joints + dq * dt
+   ```
+5. **Nullspace Control** (optional): Projects secondary objectives (like joint limits avoidance) into the nullspace
+
+
+**Advantages:**
+- Smooth, continuous motion
+- Velocity-controlled approach
+- Handles robot singularities 
+- Real-time reactive control
+
+<!-- **Jacobian Role:**
+- **Forward Kinematics**: Maps joint space to Cartesian space
+- **Jacobian Matrix**: Linear mapping between joint velocities and end-effector velocities
+- **Inverse Mapping**: Converts desired end-effector motion to required joint motions
+- **Singularity Handling**: Damped least squares prevents numerical instability near singularities -->
+
+##### 3. **PyTorch Kinematics Node** (`dora-pytorch-kinematics`)
+
+A dedicated kinematics computation node that provides three core robotic functions:
+
 ```yaml
-  - id: controller
-    path: nodes/controller.py
-    inputs:
-      joint_positions: mujoco_sim/joint_positions
-      joint_velocities: mujoco_sim/joint_velocities
-      target_pose: pose_publisher/target_pose
-      fk_result: pytorch_kinematics/fk_request
-      jacobian_result: pytorch_kinematics/jacobian_request
-    outputs:
-      - joint_commands
-      - fk_request
-      - jacobian_request
+- id: pytorch_kinematics
+  build: pip install -e ../../../node-hub/dora-pytorch-kinematics
+  path: dora-pytorch-kinematics
+  inputs:
+    ik_request: controller/ik_request           # For inverse kinematics
+    fk_request: controller/fk_request           # For forward kinematics  
+    jacobian_request: controller/jacobian_request  # For Jacobian computation
+  outputs:
+    - ik_request      # Joint solution for target pose
+    - fk_request      # End-effector pose for joint configuration
+    - jacobian_request # Jacobian matrix for velocity mapping
+  env:
+    URDF_PATH: "../URDF/franka_panda/panda.urdf"
+    END_EFFECTOR_LINK: "panda_hand"
+    TRANSFORM: "0. 0. 0. 1. 0. 0. 0."
 ```
-- **Input**: Takes target poses `[x, y, z, roll, pitch, yaw]` and current joint positions from `dora-mujoco` 
-- **Processing**: Differential inverse kinematics using `dora-pytorch-kinematics` to calculate actuator commands
-- **Output**: Control/Actuator commands
 
-#### 3. **PyTorch Kinematics Node** (`dora-pytorch-kinematics`)
+1. **Inverse Kinematics (IK)**
+   - **Input**: Target pose `[x, y, z, roll, pitch, yaw]` or `[x, y, z, qw, qx, qy, qz]` + current joint state
+   - **Output**: Complete joint configuration to achieve target pose
+   - **Use case**: Convert Cartesian target to joint angles
 
+2. **Forward Kinematics (FK)**  
+   - **Input**: Joint positions array
+   - **Output**: Current end-effector pose `[x, y, z, qw, qx, qy, qz]`
+   - **Use case**: Determine end-effector position from joint angles
 
-```yaml
-- id: pytorch_kinematics
-    build: pip install -e ../../../node-hub/dora-pytorch-kinematics
-    path: dora-pytorch-kinematics
-    inputs:
-      fk_request: controller/fk_request
-      jacobian_request: controller/jacobian_request
-    outputs:
-      - fk_request  
-      - jacobian_request  
-    env:
-      URDF_PATH: "path to the robot urdf" # URDF is used to create the kinematics model for the robot 
-      END_EFFECTOR_LINK: "name of the end effector"
-      TRANSFORM: "0. 0. 0. 1. 0. 0. 0."  # Pytorch kinematics uses wxyz format. Robot transform from world frame
+3. **Jacobian Computation**
+   - **Input**: Current joint positions  
+   - **Output**: 6×N Jacobian matrix (N = number of joints)
+   - **Use case**: Map joint velocities to end-effector velocities
 
-```
+**Configuration:**
+- **URDF_PATH**: Robot model definition file
+- **END_EFFECTOR_LINK**: Target link for pose calculations
+- **TRANSFORM**: Optional transform offset (position + quaternion wxyz format)
 
- Joint states performs Forward Kinematics, and returns End-effector pose along with jacobian matrix
+**Usage in Controllers:**
+- **Direct IK**: Uses only `ik_request` → `ik_result`
+- **Differential IK**: Uses `fk_request` → `fk_result` and `jacobian_request` → `jacobian_result`
 
 #### 4. **MuJoCo Simulation Node** (`dora-mujoco`)
 
-```yaml
+- **Process**: Physics simulation, dynamics integration, rendering
+- **Output**: Joint positions, velocities, sensor data
 
-  - id: mujoco_sim
-    build: pip install -e ../../../node-hub/dora-mujoco
-    path: dora-mujoco
-    inputs:
-      tick: dora/timer/millis/2
-      control_input: controller/joint_commands
-    outputs:
-      - joint_positions
-      - joint_velocities 
-      - actuator_controls
-      - sensor_data
-    env:
-      MODEL_NAME: "panda" # Name of the robot you want to load
-```
-- **Input**: Joint commands from controller
-- **Processing**: Physics simulation, rendering
-- **Output**: Joint positions, velocities, sensor data
+## References
+
+This controller design draws inspiration from the kinematic control strategies presented in [mjctrl](https://github.com/kevinzakka/mjctrl), specifically the [differntial ik control example](https://github.com/kevinzakka/mjctrl/blob/main/diffik.py).
+
+The URDF model for the robotic arms was sourced from the [PyBullet GitHub repository](https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data). Or you could google search the robot and get its urdf.

examples/mujoco-sim/target_pose_control/control.yml

@@ -0,0 +1,46 @@
+nodes:
+  - id: mujoco_sim
+    build: pip install -e ../../../node-hub/dora-mujoco
+    path: dora-mujoco
+    inputs:
+      tick: dora/timer/millis/2
+      control_input: controller/joint_commands
+    outputs:
+      - joint_positions
+      - joint_velocities 
+      - actuator_controls
+      - sensor_data
+    env:
+      MODEL_NAME: "panda"
+
+  - id: controller
+    path: nodes/controller_ik.py
+    inputs:
+      joint_positions: mujoco_sim/joint_positions
+      target_pose: pose_publisher/target_pose
+      ik_request: pytorch_kinematics/ik_request
+    outputs:
+      - joint_commands
+      - ik_request
+      - joint_state
+
+  - id: pytorch_kinematics
+    build: pip install -e ../../../node-hub/dora-pytorch-kinematics
+    path: dora-pytorch-kinematics
+    inputs:
+      ik_request: controller/ik_request
+      joint_state: controller/joint_state
+    outputs:
+      - ik_request
+      - joint_state
+    env:
+      URDF_PATH: "../URDF/franka_panda/panda.urdf"
+      END_EFFECTOR_LINK: "panda_hand"
+      TRANSFORM: "0. 0. 0. 1. 0. 0. 0."  # Pytorch kinematics uses wxyz format for quaternion
+
+  - id: pose_publisher
+    path: nodes/pose_publisher.py
+    inputs:
+      tick: dora/timer/millis/5000
+    outputs:
+      - target_pose

examples/mujoco-sim/target_pose_control/target_pose_control.yml → examples/mujoco-sim/target_pose_control/control_advanced.yml

@@ -14,7 +14,7 @@ nodes:
       MODEL_NAME: "panda"
 
   - id: controller
-    path: nodes/controller.py
+    path: nodes/controller_differential_ik.py
     inputs:
       joint_positions: mujoco_sim/joint_positions
       joint_velocities: mujoco_sim/joint_velocities
@@ -43,6 +43,6 @@ nodes:
   - id: pose_publisher
     path: nodes/pose_publisher.py
     inputs:
-      tick: dora/timer/millis/10000
+      tick: dora/timer/millis/5000
     outputs:
       - target_pose

examples/mujoco-sim/target_pose_control/nodes/controller.py → examples/mujoco-sim/target_pose_control/nodes/controller_differential_ik.py

@@ -4,10 +4,10 @@ import pyarrow as pa
 from dora import Node
 from scipy.spatial.transform import Rotation
 
-class FrankaController:
+class Controller:
     def __init__(self):
         """
-        Initialize the Franka controller
+        Initialize the controller
         """
         # Control parameters
         self.integration_dt = 0.1
@@ -27,7 +27,7 @@ class FrankaController:
         self.current_ee_pose = None  # End-effector pose
         self.current_jacobian = None  # Jacobian matrix
         
-        print("FrankaController initialized")
+        print("Controller initialized")
     
     def _initialize_robot(self, positions, jacobian_dof=None):
         """Initialize controller state with appropriate dimensions"""
@@ -115,7 +115,7 @@ class FrankaController:
 
 def main():
     node = Node()
-    controller = FrankaController()
+    controller = Controller()
     
     for event in node:
         if event["type"] == "INPUT":

examples/mujoco-sim/target_pose_control/nodes/controller_ik.py

@@ -0,0 +1,89 @@
+import numpy as np
+import time
+import pyarrow as pa
+from dora import Node
+
+class Controller:
+    def __init__(self):
+        """
+        Initialize the simple controller
+        """
+        # State variables
+        self.dof = None 
+        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.ik_request_sent = True 
+        
+        print("Simple Controller initialized")
+    
+    def _initialize_robot(self, positions):
+        """Initialize controller state with appropriate dimensions"""
+        self.full_joint_count = len(positions)
+        self.current_joint_pos = positions.copy()
+        if self.dof is None:
+            self.dof = self.full_joint_count
+    
+    def set_target_pose(self, pose_array):
+        """Set target pose from input array."""
+        self.target_pos = np.array(pose_array[:3])
+        if len(pose_array) == 6:
+            self.target_rpy = list(pose_array[3:6])
+        else:
+            self.target_rpy = [180.0, 0.0, 90.0] # Default orientation if not provided
+        self.ik_request_sent = False
+    
+    def get_target_pose_array(self):
+        """Get target pose as 6D array [x, y, z, roll, pitch, yaw]"""
+        return np.concatenate([self.target_pos, self.target_rpy])
+
+def main():
+    node = Node()
+    controller = Controller()
+    
+    for event in node:
+        if event["type"] == "INPUT":
+            if event["id"] == "joint_positions":
+                joint_pos = event["value"].to_numpy()
+
+                if controller.current_joint_pos is None:
+                    controller._initialize_robot(joint_pos)
+                else:
+                    controller.current_joint_pos = joint_pos
+                
+                # Request IK solution directly
+                target_pose = controller.get_target_pose_array()
+                if not controller.ik_request_sent:
+                    node.send_output(
+                        "ik_request", 
+                        pa.array(target_pose, type=pa.float32()),
+                        metadata={"encoding": "xyzrpy", "timestamp": time.time()}
+                    )
+                    controller.ik_request_sent = True
+                
+                node.send_output(
+                    "joint_state", 
+                    pa.array(joint_pos, type=pa.float32()),
+                    metadata={"encoding": "jointstate", "timestamp": time.time()}
+                )
+
+            # Handle target pose updates
+            if event["id"] == "target_pose":
+                target_pose = event["value"].to_numpy()
+                controller.set_target_pose(target_pose)
+            
+            # Handle IK results and send directly as joint commands
+            if event["id"] == "ik_request":
+                if event["metadata"]["encoding"] == "jointstate":
+                    ik_solution = event["value"].to_numpy()
+                    # print("ik_solution", ik_solution)
+                    # Send IK solution directly as joint commands
+                    node.send_output(
+                        "joint_commands", 
+                        pa.array(ik_solution, type=pa.float32()),
+                        metadata={"timestamp": time.time()}
+                    )
+
+if __name__ == "__main__":
+    main()

examples/mujoco-sim/target_pose_control/nodes/pose_publisher.py

@@ -28,7 +28,7 @@ class PosePublisher:
 def main():
     node = Node("pose_publisher")
     publisher = PosePublisher()
-    
+    time.sleep(3)  # Allow time for simulation to start
     for event in node:
         if event["type"] == "INPUT" and event["id"] == "tick":
             target_pose = publisher.get_next_pose()

node-hub/dora-mujoco/README.md

@@ -211,7 +211,7 @@ Robot-specific controllers should:
 ## Examples
 
 Complete working examples are available in:
-- `dora/examples/franka-mujoco-control/`
+- `dora/examples/mujoco-sim/`
   - Target pose control with Cartesian space control
   - Gamepad control with real-time interaction
 

node-hub/dora-mujoco/pyproject.toml

@@ -23,11 +23,9 @@ dora-mujoco = "dora_mujoco.main:main"
 
 [tool.ruff.lint]
 extend-select = [
-  "D",   # pydocstyle
   "UP",  # pyupgrade
   "PERF", # Ruff's PERF rule
   "RET",  # Ruff's RET rule
   "RSE",  # Ruff's RSE rule
-  "NPY",  # Ruff's NPY rule
   "N",    # Ruff's N rule
 ]

node-hub/dora-pytorch-kinematics/dora_pytorch_kinematics/main.py

@@ -241,6 +241,7 @@ class RobotKinematics:
             torch.Tensor: Jacobian matrix of shape (B, 6, num_joints) or (6, num_joints)
                          where rows are [linear_vel_x, linear_vel_y, linear_vel_z, 
                                        angular_vel_x, angular_vel_y, angular_vel_z]
+
         """
         angles_tensor = self._prepare_joint_tensor(
             joint_angles, batch_dim_required=False
@@ -273,6 +274,7 @@ class RobotKinematics:
                          
         Returns:
             np.ndarray: Jacobian matrix as numpy array of shape (6, num_joints) or (B, 6, num_joints)
+
         """
         J = self.compute_jacobian(joint_angles)
         return J.cpu().numpy()
@@ -325,7 +327,7 @@ def main():
                             continue
 
                         solution = solution.numpy().ravel()
-                        delta_angles = solution - last_known_state
+                        delta_angles = solution - last_known_state[:len(solution)] # match with dof
 
                         valid = np.all(
                             (delta_angles >= -np.pi) & (delta_angles <= np.pi),