dora/examples/mujoco-sim/target_pose_control

• 02. Target Pose Control
• • Controller Types
  • • 1. Direct IK Controller (control.yml)
    • 2. Differential IK Controller (control_advanced.yml)
  • Running the Examples
  • • Direct IK Control
    • Differential IK Control
    • Nodes
    • • 1. Pose Publisher Script (pose_publisher.py)
      • 2. Controller Scripts
      • • Direct IK Controller (controller_ik.py)
        • Differential IK Controller (controller_differential_ik.py)
        • 3. PyTorch Kinematics Node (dora-pytorch-kinematics)
      • 4. MuJoCo Simulation Node (dora-mujoco)
  • References

02. Target Pose Control

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.

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

cd target_pose_control
dora build control.yml
dora run control.yml

Differential IK Control

cd target_pose_control
dora build control_advanced.yml
dora run control_advanced.yml

You should see:

1. Robot moves to predefined target poses automatically
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

1. Pose Publisher Script (pose_publisher.py)

class PosePublisher:
    def __init__(self):
        # Predefined sequence of target poses [x, y, z, roll, pitch, yaw]
        self.target_poses = [
            [0.5, 0.5, 0.3, 180.0, 0.0, 90.0],   # Position + RPY orientation
            [0.6, 0.2, 0.5, 180.0, 0.0, 45.0],   # Different orientation
            # ... more poses
        ]

• Sends target poses every 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 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:

   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:

   # 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:

   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

3. PyTorch Kinematics Node (dora-pytorch-kinematics)

A dedicated kinematics computation node that provides three core robotic functions:

- 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."

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

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)

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)

• Process: Physics simulation, dynamics integration, rendering
• Output: Joint positions, velocities, sensor data

References

This controller design draws inspiration from the kinematic control strategies presented in mjctrl, specifically the differential ik control example.

The URDF model for the robotic arms was sourced from the PyBullet GitHub repository. Or you could google search the robot and get its urdf.