Enhancement: Teleoperation & Code Readability

[ts-flake]

The issue/first experience

While assembling and walking through the original XLeRobot project, I was often confused about the connections between the software/codes (parameter names, control logic, etc.) and the hardware, and a bit afraid of moving my controllers, in fear of abrupt movements of the robot.

I am new to robotics, and thus I decided to spend more time looking into the code details (e.g., LeRobot). And the final solutions (journey logs) proposed are...

• LeRobot-styled teleoperation, i.e., using a Teleoperator and Robot Processors, thus this increases code reusability
• Better code readability with detailed comments and clear parameter names
• Easier ways to set configurations via TeleoperatorConfig or CLI
• Improved VR control experience with added keymaps for more actions such as 'back to zero/home', 'exit', and 'reset reference frame' (I found this is very useful when you drive the robot around and still be able to reposition yourself)
• Smoothier base movements. Using the EMAJointAction processor, the three-wheeled base now starts off gently and stops pointly
• (Optional) Support SO101-yaw arm (SO101 + 1 dof wrist yaw) [Hardware]

Note: I have completed and tested the codes for XLeRobot-yaw [GitHub]. The original SO101 platform is not tested but should work since minimal changes are made to adapt to SO101-yaw.

There are many changes I have made, and I would like to know if this sounds good or if you have suggestions!

Updates

The folder structure is the same as lerobot, for example:

software/
├── examples/
│   ├── xlerobot/
│   └── xlerobot_yaw/
└── src/
    ├── model/
    │   └── rr_kinematics.py
    ├── robots/
    │   ├── xlerobot/
    │   └── xlerobot_yaw/
    └── teleoperators/
        ├── xlerobot_vr/
        └── xlerobot_yaw_vr/

So, the installation is minimal; you only need to copy/move some files to the corresponding destinations in the LeRobot folder.

Inverse Kinematics

The RRKinematics class in software/src/model/rr_kinematics.py is essentially a refactoring of the SO101Kinematics from the XLeRobot, with a detailed comment on the angle definitions and better naming.

XLeVR

The wrist_yaw_deg is a newly added field to ControlGoal for the SO101-yaw (or any 6DoF) robot arm. For the SO101 arm, this value is ignored.

Significant changes to XLeVR/xlevr/inputs/vr_ws_server.py:

• VRControllerState now records the prev/curr-position/quaternion to facilitate delta position/quaternion computation
• VRWebSocketServer sends delta EE commands, in the robot's frame, i.e., forward (x), left (y), upward (z):
  • target_position: (dx, dy, dz)
  • wrist_roll_deg: x-axis rotation (angle directions follow the right-hand rule)
  • wrist_flex_deg: pitch, y-axis rotation
  • wrist_yaw_deg: z-axis rotation
• The original squeeze-to-teleoperate logic is pushed to downstreams, and all necessary information is stored in metadata. E.g., the user can decide the behavior of the controller when metadata['buttons']['squeeze'] is true.

Note: In the codes, the delta actions are expressed in the local/body frame (origin_quaternion) first and then converted to the robot's frame.

metadata = {
    ...,
    'trigger': float (0-1)
    'trigger_active': bool if trigger > 0.5
    'thumbstick': {'x': float (0-1), 'y': float (0-1)},
    'buttons': {
        'squeeze': bool,
         'x': bool,
         'y': bool,
         'a': bool,
         'b': bool
    }
}

Controller Keymaps: In Teleoperator

Below are the default keymaps for the Quest3 VR controller.

Left controller

• thumbstick: Reserved for recording actions, e.g., 'early exiting', 'stop recording', etc.
• trigger: Open/close left gripper (toggled by pressing button A)
• grip (squeeze) button: Freeze/unfreeze left arm (set by grip_to_activate in XLeRobotYawVRConfig)
• X button: Quit and move back to the initial position
• Y button: Move back to the zero position

Right controller

• thumbstick: Base motion control, e.g., 'forward', 'backward', 'turn left/right'
• trigger: Open/close gripper (toggled by pressing button A)
• grip (squeeze) button: Freeze/unfreeze right arm (set by grip_to_activate in XLeRobotYawVRConfig)
• A button: Toggle trigger state, i.e., open/close gripper
• B button: Reset the origin pose of both controllers. Press this button whenever you relocate/reorientate in space