Teleoperation Development Example

The PICO upper-body teleoperation video or Bilibili demonstrates the DR02 Pro simulation-to-hardware workflow. See DR02 video tutorials for related resources.

Project Overview

dr02_tele is the upper-body Pico teleoperation package for the DR02 Pro, enabling real-time remote control of the robot’s upper body through a Pico headset and controllers. The program publishes position PD commands via /JOINTS_CMD, controlling the waist and dual arms, while leg motion is handled by the robot’s internal policy.

Code repository: DeepRoboticsLab/deep-robotics-teleoperate

System Architecture

Pico Headset/Controllers
    ↓ (PXREA protocol)
Robotics Service (Local)
    ↓ (JSON data parsing)
PicoDataReceiver
    ↓
PicoTeleoperationController (IK + Filtering + Protection)
    ↓
TeleoperationState::Run()
    ↓
DR2ProInterface::SetJointCommand()
    ↓
/JOINTS_CMD (31 DOF PD commands)
    ↓
Robot Firmware Execution

Startup Sequence

main.cpp

int main() {
  rclcpp::init(0, 0);

  // Create state machine (DR02 Pro)
  auto fsm = std::make_shared<HumanoidStateMachine>(RobotName::DR2PRO);

  // Initialize interfaces, create state controllers, start safety monitoring
  fsm->Start();

  // Enter 2ms control loop
  fsm->Run();

  // Stop all threads
  fsm->Stop();

  rclcpp::shutdown();
  return 0;
}

Start() executes the following in sequence:

1. Create ROS 2 node sdk_deploy
2. Initialize UserCommandInterface (subscribe to /GAMEPAD_KEY)
3. Initialize DR2ProInterface (31 DOF)
4. Create ControlParameters and SafeController
5. Build 4 state controllers (Emergency, ZeroPos, Teleoperation, Down)
6. Wait 3 seconds for safety initialization
7. Start all interfaces and safety monitoring threads
8. Enter initial state Emergency

[!NOTE]

The dr02_tele state machine is based on the same HumanoidStateMachine framework, but replaces the default RLControl and Idle states with the Teleoperation state.

For general state machine information, refer to:

• State Machine Overview and Implementation - Architecture overview, control cycle, state transitions, safety monitoring, etc.
• ROS 2/DDS Communication - Topic interface descriptions

Pico Teleoperation Controller

PicoTeleoperationController

Responsibility: Encapsulates the Pico SDK, processes controller and headset data, computes IK targets, and applies filtering and protection.

Interface Definition:

class PicoTeleoperationController {
public:
    static constexpr int kDof = 31;

    struct Command {
        std::array<float, kDof> position{};
        std::array<float, kDof> velocity{};
        bool ready = false;  // false indicates Pico stream lost, maintain last target
    };

    int Start();                              // Start PXREA
    void Stop();                              // Shutdown PXREA
    void SeedFromCurrent(const std::array<float, kDof>& current);  // Initialize from current position
    Command Update(const std::array<float, kDof>& current);         // Execute one teleoperation update
    void ResetTargetsToCurrent(const std::array<float, kDof>& current);  // Reset targets to current
    bool IsConnected() const;                 // Check PXREA connection status
};

PicoDataReceiver

Responsibility: Connects to Robotics Service, receives and parses Pico JSON data.

Data Structure:

struct PicoControllerState {
    bool valid{false};
    bool primary{false};          // Primary button pressed
    bool secondary{false};        // Secondary button pressed
    bool axis_click{false};       // Joystick pressed
    float grip{0.0f};             // Grip force
    float trigger{0.0f};          // Trigger value (0.5 triggers anchor lock)
    std::array<float, 2> axis{0.0f, 0.0f};  // Joystick direction
    std::array<double, 3> pos{0.0, 0.0, 0.0};     // Position
    std::array<double, 4> quat{0.0, 0.0, 0.0, 1.0}; // Quaternion (x, y, z, w)
};

struct PicoHeadState {
    bool valid{false};
    std::array<double, 3> pos{0.0, 0.0, 0.0};
    std::array<double, 4> quat{0.0, 0.0, 0.0, 1.0};
};

struct PicoState {
    PicoControllerState left;
    PicoControllerState right;
    PicoHeadState head;
    uint64_t received_steady_ns{0};      // Local receive time (monotonic clock)
    uint64_t valid_packet_count{0};       // Valid packet count
    uint64_t parse_error_count{0};        // Parse error count
    uint64_t state_sequence{0};           // State sequence number
};

Controller-Controlled Dual Arms

Trigger Anchor Mechanism:

1. When one side’s trigger reaches threshold 0.5, record that controller’s pose and the corresponding robot end-effector pose as the anchor
2. The first frame after trigger press only establishes the anchor; no displacement is computed
3. Subsequent frames use the relative-to-anchor translation and rotation to generate 6D end-effector targets
4. Releasing the trigger clears that side’s anchor; the next press re-establishes the anchor from the current robot command pose

Controller Mapping Parameters:

Parameter	Value	Description
Trigger Threshold	0.5	Triggers anchor lock
Translation Dead Zone	0.015 m	Below this value, considered no movement
Controller Position Scaling	1.2	Enlarges controller movement range
Translation Ratio	1.2 × 1.2 = 1.44	Final translation gain
Max Relative Anchor Translation	0.5 m	Limits maximum translation distance
Rotation Dead Zone	0.035 rad	Below this value, considered no rotation
Max Relative Anchor Rotation	1.45 rad	Limits maximum rotation angle

Headset-Controlled Waist

Anchor Mechanism:

Upon entering Teleoperation, the first valid headset pose establishes the headset-waist anchor. Subsequent control uses relative pose:

Headset Change	Waist Target	Ratio
pitch	Joint 0 (WaistZ)	+0.8
yaw	Joint 1 (WaistX)	-0.9
roll	Joint 2 (WaistY)	-0.6

Headset Angle Dead Zone: 0.006 rad

First-Order Filter Coefficient: alpha = 0.88

Final Limits: The waist also passes through software limits and per-feedback-frame rate limiting (0.006 rad)

Pico Stream Loss Protection

Detection Mechanism:

Uses the monotonic clock at the time Pico data is received locally to determine data freshness, not treated as a network latency measurement.

Protection Behavior:

• If no valid Pico data has been received, or the most recent data is older than 200 ms, enter stale hold
• Stale hold freezes the last target actually sent to the robot; it does not continue approaching undelivered old IK targets
• Clears controller and headset anchors on stream loss, and requires all triggers to be released first
• After data recovery and detection that all triggers are released, re-pressing triggers is allowed to establish new anchors

If controllers are still gripped during recovery, arm control will not resume immediately. This is expected protective behavior.

IK and Command Protection Pipeline

IK Configuration:

Loads the default URDF upon entering Teleoperation, or loads the file specified by DR02_PRO_TELEOP_URDF. In the full 31 DOF model, locks 17 joints for waist, neck, and dual legs, retaining only 14 DOF for dual arms for joint solving.

Protection and Shaping Sequence:

1. Controller Displacement Limiting: Constrains controller relative translation and rotation to the allowed range near the anchor
2. Joint IK Solving: Simultaneously optimizes dual arm position and orientation, adding a weighted 5.0 outward soft cost when the elbow enters the cylindrical region within 0.25 m radius of the torso
3. URDF Limit Clipping: Each IK iteration clips results using URDF joint position upper/lower limits
4. Four-Frame Weighted Average: 14-dimensional arm target is averaged with weights {0.1, 0.2, 0.3, 0.4}
5. Second-Order Smoother: Critically damped second-order smoothing, omega = 30
6. Software Limits: Waist and dual arm targets pass through the software limit table (see below)
7. Output Rate Limiting: Final output change relative to the last sent command is limited to 0.006 rad per feedback frame

Software Limit Table:

Index	Joint	Software Range (rad)
0	waist z	[-0.6, 0.6]
1	waist x	[-0.4, 0.4]
2	waist y	[-0.4, 0.4]
3 / 10	left / right shoulder y	[-1.2, 1.4]
4	left shoulder x	[-0.7, 1.8]
11	right shoulder x	[-1.8, 0.7]
5 / 12	left / right shoulder z	[-1.6, 1.6]
6 / 13	left / right elbow	[-0.2, 1.8]
7 / 14	left / right wrist z	[-2.6, 2.6]
8 / 15	left / right wrist y	[-1.45, 1.45]
9 / 16	left / right wrist x	[-1.45, 1.45]

[!WARNING]

The elbow cylinder cost is a soft constraint, not a mesh collision detection. It cannot detect dual arm mutual collisions, nor does it protect the robot from collisions with the environment or personnel.

Control Frequency

The state machine main loop wakes every 2 ms, but only executes state control and publishes new commands when the local receive timestamp of /JOINTS_DATA has changed. Therefore, the actual control frequency is determined by the joint feedback frequency, not a fixed 500 Hz.

Gamepad Control

Gamepad buttons are transmitted via the /GAMEPAD_KEY topic:

Button	Valid State	Result
L1	Emergency, Down	Request to enter ZeroPos
L2	ZeroPos	Request to enter Teleoperation
R1	Any state	Request to enter Emergency
R2	Emergency, Teleoperation	Request to enter Down

HandleKey Implementation:

void HandleKey(const std_msgs::msg::String::SharedPtr msg) {
    static const unordered_map<string, KeyCode> key_map = {
        {"G20_KEY_L1", L1}, {"G20_KEY_L2", L2},
        {"G20_KEY_R1", R1}, {"G20_KEY_R2", R2}};

    KeyCode code = key_map.find(msg->data)->second;
    RobotMotionState current_state = GetCurrentState();

    switch (code) {
        case L1:
            if (current_state == Emergency || current_state == DownMode) {
                usr_cmd_->target_mode = ZeroPos;
            } else {
                cout << "[GAMEPAD] L1 ignored: ZeroPos is available from Emergency or Down" << endl;
            }
            break;
        // ...
    }
}

If the corresponding button is pressed in a disallowed current state, the state machine will not execute the transition and will output the reason for ignoring the request in the terminal.

Build and Deployment

Environment Requirements

source /opt/ros/<ros-distro>/setup.bash
cd ~/your_project/deep-robotics-msg
# Follow the repository README to complete the build
source install/setup.bash

Environment Preparation:

source /opt/ros/<ros-distro>/setup.bash
# Refer to [Resource Downloads](../../sdk/appendix/resources.md) to obtain the dependency message library

Build for Real Robot (ARM On-Board Device):

source /opt/ros/<ros-distro>/setup.bash
source ~/your_project/deep-robotics-msg/install/setup.bash
cd ~/your_project/deep-robotics-teleoperate
colcon build --packages-up-to dr02_tele \
  --cmake-args -DBUILD_PLATFORM=arm -DBUILD_SIM=OFF
source install/setup.bash

Build for Simulation (x86 Host):

source /opt/ros/<ros-distro>/setup.bash
source ~/your_project/deep-robotics-msg/install/setup.bash
cd ~/your_project/deep-robotics-teleoperate
colcon build --packages-up-to dr02_tele \
  --cmake-args -DBUILD_PLATFORM=x86 -DBUILD_SIM=ON
source install/setup.bash

[!WARNING]

Building for the real robot must use BUILD_PLATFORM=arm and keep BUILD_SIM=OFF. Enabling BUILD_SIM=ON disables the runtime SafeController monitoring for real robot operation and must not be used on the real robot.

BUILD_SIM=ON only disables the real robot runtime SafeController monitoring; it does not start a simulator, nor does it modify ROS Topics or Pico control logic.

Simulation build results are for simulation only and must not be used to control the real robot.

If using a new terminal to build dr02_tele, you must first reload the environment in the following order:

source /opt/ros/<ros-distro>/setup.bash
source ~/your_project/deep-robotics-msg/install/setup.bash

Troubleshooting

Symptom	Cause and Solution
Continuously shows wait data update	No data on /JOINTS_DATA, check ROS 2 environment, drdds and DDS configuration
PXREA initialization failed	Robotics Service is not running, or using x86 dependencies instead of ARM
URDF not found	deep_robotics_model/ in project root is empty, or DR02_PRO_TELEOP_URDF path is incorrect

Check Joint Data:

ros2 topic echo /JOINTS_DATA

Pico Connection Issues

Symptom	Cause and Solution
No Pico data	Pico and Orin not on the same WiFi, receive address error, three send switches not enabled, headset in sleep mode
PXREAInit returned 0	PXREA initialization call succeeded
PXREA server connected	State machine connected to local Robotics Service
device found	Robotics Service has discovered the Pico device
first state JSON	First Pico state received and parsed

Control Issues

Symptom	Cause and Solution
Pico data present but arms don’t move	Not in Teleoperation state, trigger not reaching 0.5 threshold, need to release trigger first
trigger latched	Anchor just established on that side, continue moving the controller to generate relative targets
Arms jumping	IK target unreachable, anchor outdated, release trigger and re-establish anchor
Safety triggered (safe_control_mode != 0)	Joint data anomaly or motor over-temperature, state machine auto-recovers from Emergency after resolving the issue

Diagnostic Information

Teleoperation outputs diagnostics once per second:

[PicoTeleoperation] diag pico_age_ms=... packets=...(total=...)
parse_fail=...(total=...) feedback_hz=... ik_ms(avg/max)=.../...
control_ms(avg/max)=.../... trigger(L/R)=.../... stale=...

Field	Meaning and Check Direction
pico_age_ms	Local receive time of most recent valid Pico data; exceeding 200 indicates stream loss protection
packets	Valid state packets received in the last second; if 0, check Pico Send and Robotics Service
parse_fail	JSON parse failure count; if continuously increasing, check sender data format and version
feedback_hz	Actual joint feedback and control update frequency; if too low, check /JOINTS_DATA and DDS
ik_ms(avg/max)	IK average/maximum execution time; if abnormally high, check unreachable targets, URDF, and host load
control_ms(avg/max)	Total Teleoperation control and publishing time
trigger(L/R)	Left/right trigger raw values, to check if 0.5 threshold is reached
stale	Whether Pico stream loss hold is active
error_code: 0x...	Safety error code in hex, see PrintRobotErrorState output

Safety Warnings

[!WARNING]

• Only one /JOINTS_CMD publisher can run at a time
• Emergency is software posture maintenance, not a hardware emergency stop
• First movements must be small, single-side, step-by-step verification
• The SDK must be stopped before exiting or switching developer modes
• Software safety monitoring, Pico stream loss hold, and Emergency cannot replace hardware emergency stop
• During real robot teleoperation, the physical emergency stop or gamepad red stop button must always be within immediate reach