跳转到内容

遥操作开发示例

PICO 上半身遥操视频 或 Bilibili演示 DR02 Pro 的仿真到实机流程。更多入口见DR02 视频教程。

项目概述

Section titled “项目概述”

dr02_tele 是 DR02 Pro 的上半身 Pico 遥操软件包，通过 Pico 头显和手柄实现机器人的上半身实时遥控。程序通过 /JOINTS_CMD 发布位置 PD 指令，控制腰部和双臂，腿部运动由机器人内部策略负责。

代码资源：DeepRoboticsLab/deep-robotics-teleoperate

系统架构

Section titled “系统架构”
Pico 头显/手柄
    ↓ (PXREA 协议)
Robotics Service (本机)
    ↓ (JSON 数据解析)
PicoDataReceiver
    ↓
PicoTeleoperationController (IK + 滤波 + 保护)
    ↓
TeleoperationState::Run()
    ↓
DR2ProInterface::SetJointCommand()
    ↓
/JOINTS_CMD (31 自由度 PD 指令)
    ↓
机器人固件执行

启动流程

Section titled “启动流程”
main.cpp
int main() {
  rclcpp::init(0, 0);


  // 创建状态机 (DR02 Pro)
  auto fsm = std::make_shared<HumanoidStateMachine>(RobotName::DR2PRO);


  // 初始化接口、创建状态控制器、启动安全监控
  fsm->Start();


  // 进入 2ms 控制循环
  fsm->Run();


  // 停止所有线程
  fsm->Stop();


  rclcpp::shutdown();
  return 0;
}

Start() 依次执行：

  1. 创建 ROS 2 节点 sdk_deploy
  2. 初始化 UserCommandInterface (订阅 /GAMEPAD_KEY)
  3. 初始化 DR2ProInterface (31 自由度)
  4. 创建 ControlParameters 和 SafeController
  5. 构建 4 个状态控制器 (Emergency, ZeroPos, Teleoperation, Down)
  6. 等待 3 秒安全初始化
  7. 启动各接口和安全监控线程
  8. 进入初始状态 Emergency

[!NOTE]

dr02_tele 的状态机基于相同的 HumanoidStateMachine 框架，但用 Teleoperation 状态替换了默认的 RLControl 和 Idle 状态。

通用状态机信息请参考：

Pico遥操控制器

Section titled “Pico遥操控制器”

PicoTeleoperationController

Section titled “PicoTeleoperationController”

职责： 封装 Pico SDK，处理手柄和头显数据，计算 IK 目标，应用滤波和保护。

接口定义：

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


    struct Command {
        std::array<float, kDof> position{};
        std::array<float, kDof> velocity{};
        bool ready = false;  // false 表示 Pico 断流，保持上次目标
    };


    int Start();                              // 启动 PXREA
    void Stop();                              // 关闭 PXREA
    void SeedFromCurrent(const std::array<float, kDof>& current);  // 从当前位置初始化
    Command Update(const std::array<float, kDof>& current);         // 执行一次遥操更新
    void ResetTargetsToCurrent(const std::array<float, kDof>& current);  // 重置目标到当前
    bool IsConnected() const;                 // 检查 PXREA 连接状态
};

PicoDataReceiver

Section titled “PicoDataReceiver”

职责： 连接 Robotics Service，接收并解析 Pico JSON 数据。

数据结构：

struct PicoControllerState {
    bool valid{false};
    bool primary{false};          // 主键按下
    bool secondary{false};        // 副键按下
    bool axis_click{false};       // 摇杆按下
    float grip{0.0f};             // 握力
    float trigger{0.0f};          // trigger 值 (0.5 触发锚点锁定)
    std::array<float, 2> axis{0.0f, 0.0f};  // 摇杆方向
    std::array<double, 3> pos{0.0, 0.0, 0.0};     // 位置
    std::array<double, 4> quat{0.0, 0.0, 0.0, 1.0}; // 四元数 (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};      // 本机接收时间 (单调时钟)
    uint64_t valid_packet_count{0};       // 有效数据包计数
    uint64_t parse_error_count{0};        // 解析错误计数
    uint64_t state_sequence{0};           // 状态序列号
};

手柄控制双臂

Section titled “手柄控制双臂”

Trigger 锚点机制：

  1. 当某侧 trigger 达到阈值 0.5 时，记录该手柄姿态和该侧机器人末端姿态作为锚点
  2. trigger 按下后的第一帧只建立锚点，不求解位移
  3. 后续帧使用相对锚点的平移和旋转生成 6D 末端目标
  4. 松开 trigger 会清除该侧锚点，下次按下时从当前机器人指令姿态重新建锚

手柄映射参数：

参数数值说明
Trigger 阈值0.5触发锚点锁定
平移死区0.015 m小于此值视为无移动
手柄位置扩张1.2放大手柄移动范围
平移比例1.2 × 1.2 = 1.44最终平移增益
相对锚点最大平移0.5 m限制最大平移距离
旋转死区0.035 rad小于此值视为无旋转
相对锚点最大旋转1.45 rad限制最大旋转角度

头显控制腰部

Section titled “头显控制腰部”

锚点机制：

进入 Teleoperation 后，第一份有效头显姿态会建立头显和腰部锚点。之后使用相对姿态控制：

头显变化腰部目标比例
pitch关节 0 (WaistZ)+0.8
yaw关节 1 (WaistX)-0.9
roll关节 2 (WaistY)-0.6

头显角度死区： 0.006 rad

一阶滤波系数： alpha = 0.88

最终限制： 腰部还会经过软件限位和每反馈帧限速 (0.006 rad)

Pico 断流保护

Section titled “Pico 断流保护”

检测机制：

使用 Pico 数据在本机被接收时的单调时钟判断数据新鲜度，不把它当作网络延迟测量。

保护行为：

  • 尚未收到有效 Pico 数据，或最近数据超过 200 ms 未更新时，进入 stale hold
  • stale hold 会冻结最近一次真正发送给机器人的目标，不会继续靠近尚未下发的旧 IK 目标
  • 断流时清除手柄和头显锚点，并要求所有 trigger 先松开
  • 数据恢复且检测到所有 trigger 已松开后，才允许重新按下 trigger 建立新锚点

若恢复时手柄仍扣住，终端不会立即恢复手臂控制，这是预期保护行为。

IK 与指令保护链路

Section titled “IK 与指令保护链路”

IK 配置：

在进入 Teleoperation 时加载默认 URDF，或加载 DR02_PRO_TELEOP_URDF 指定的文件。在完整 31 自由度模型中锁定腰部、颈部和双腿的 17 个关节，只保留双臂 14 自由度进行联合求解。

保护和整形顺序：

  1. 手柄位移限制： 将手柄相对平移和旋转限制在锚点附近的允许范围
  2. 联合 IK 求解： 同时优化双臂位置与姿态，并在肘部进入躯干半径 0.25 m 的圆柱区域时加入权重 5.0 的向外软代价
  3. URDF 限位裁剪： 每次 IK 迭代使用 URDF 关节位置上下限裁剪结果
  4. 四帧加权平均： 14 维手臂目标经过权重 {0.1, 0.2, 0.3, 0.4} 的平均化
  5. 二阶平滑器： 临界阻尼二阶平滑，omega = 30
  6. 软件限位： 腰部和双臂目标经过软件限位表 (见下)
  7. 输出限速： 最终输出相对上一条已发送指令的变化量限制为每反馈帧 0.006 rad

软件限位表：

索引关节软件范围 (rad)
0waist z[-0.6, 0.6]
1waist x[-0.4, 0.4]
2waist y[-0.4, 0.4]
3 / 10left / right shoulder y[-1.2, 1.4]
4left shoulder x[-0.7, 1.8]
11right shoulder x[-1.8, 0.7]
5 / 12left / right shoulder z[-1.6, 1.6]
6 / 13left / right elbow[-0.2, 1.8]
7 / 14left / right wrist z[-2.6, 2.6]
8 / 15left / right wrist y[-1.45, 1.45]
9 / 16left / right wrist x[-1.45, 1.45]

[!WARNING]

肘部圆柱代价是软约束，不是网格碰撞检测。它不能检测双臂互撞，还不能保护机器人免于环境或人员碰撞。

控制频率

Section titled “控制频率”

状态机的主循环每 2 ms 唤醒一次，但只有 /JOINTS_DATA 的本地接收时间戳发生变化时才执行一次状态控制并发布新指令。因此实际控制频率由关节反馈频率决定，不是固定的 500 Hz。

手柄控制

Section titled “手柄控制”

手柄按键通过 /GAMEPAD_KEY 话题传递：

按键有效状态结果
L1Emergency、Down请求进入 ZeroPos
L2ZeroPos请求进入 Teleoperation
R1任意状态请求进入 Emergency
R2Emergency、Teleoperation请求进入 Down

HandleKey 实现：

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;
        // ...
    }
}

如果在不允许的当前状态下按下对应按钮，状态机不会执行切换，并会在终端输出忽略该请求的原因。

编译与部署

Section titled “编译与部署”

环境要求

Section titled “环境要求”
Terminal window
source /opt/ros/<ros-distro>/setup.bash
cd ~/your_project/deep-robotics-msg
# 按照该仓库 README 完成编译
source install/setup.bash

环境准备：

Terminal window
source /opt/ros/<ros-distro>/setup.bash
# 参照 [资源下载](../../sdk/appendix/resources.md) 获取依赖消息库

真机编译 (ARM 机载设备)：

Terminal window
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

仿真编译 (x86 主机)：

Terminal window
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]

真机编译必须使用 BUILD_PLATFORM=arm，并保持 BUILD_SIM=OFF。启用 BUILD_SIM=ON 会关闭真机运行时安全监控，禁止用于实机。

BUILD_SIM=ON 只会关闭真机使用的运行时 SafeController 监控，不会启动仿真器，也不会修改 ROS Topic 或 Pico 控制逻辑。

仿真编译结果仅用于仿真，不得用于控制实机。

如果改用新终端编译 dr02_tele，必须先按以下顺序重新加载环境：

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

常见问题

Section titled “常见问题”
现象原因与解决方案
一直显示 wait data update/JOINTS_DATA 没有数据，检查 ROS 2 环境、drdds 和 DDS 配置
PXREA 初始化失败Robotics Service 未运行，或使用 x86 版本依赖而非 ARM 版本
找不到 URDF项目根目录中的 deep_robotics_model/ 为空，或 DR02_PRO_TELEOP_URDF 路径错误

检查关节数据：

Terminal window
ros2 topic echo /JOINTS_DATA

Pico 连接问题

Section titled “Pico 连接问题”
现象原因与解决方案
Pico 无数据Pico 与 Orin 未连接同一 WiFi、接收地址错误、三个发送开关未打开、头显休眠
PXREAInit returned 0PXREA 初始化调用成功
PXREA server connected状态机已连接本机 Robotics Service
device foundRobotics Service 已发现 Pico 设备
first state JSON已收到并解析第一份 Pico 状态

控制问题

Section titled “控制问题”
现象原因与解决方案
有 Pico 数据但手臂不动未进入 Teleoperation 状态、trigger 未达到 0.5 阈值、需要先松开 trigger
trigger latched该侧刚建立锚点，需继续移动手柄才产生相对目标
手臂跳变IK 目标不可达、锚点过时、应先松开 trigger 重新建锚
安全性触发 (safe_control_mode != 0)关节数据异常或电机过温，排除异常后状态机自动从 Emergency 恢复

诊断信息

Section titled “诊断信息”

Teleoperation 每秒输出一次诊断：

[PicoTeleoperation] diag pico_age_ms=... packets=...(total=...)
parse_fail=...(total=...) feedback_hz=... ik_ms(avg/max)=.../...
control_ms(avg/max)=.../... trigger(L/R)=.../... stale=...
字段含义与检查方向
pico_age_ms最近有效 Pico 数据的本机接收时间；超过 200 表示断流保护
packets最近一秒收到的有效状态包；为 0 时检查 Pico Send 和 Robotics Service
parse_failJSON 解析失败数量；持续增加时检查发送端数据格式和版本
feedback_hz实际关节反馈与控制更新频率；过低时检查 /JOINTS_DATA 和 DDS
ik_ms(avg/max)IK 平均/最大耗时；异常增大时检查不可达目标、URDF 和主机负载
control_ms(avg/max)整个 Teleoperation 控制与发布耗时
trigger(L/R)左右 trigger 原始值，用于检查是否达到 0.5
stale是否正在执行 Pico 断流保持
error_code: 0x...安全错误码十六进制值，查看 PrintRobotErrorState 输出

安全警告

Section titled “安全警告”

[!WARNING]

  • 同一时刻只能运行一个 /JOINTS_CMD 发布者
  • Emergency 是软件姿态保持，不是硬件急停
  • 首次动作必须小幅、单侧、分步骤验证
  • 停止 SDK 后才能退出或切换开发者模式
  • 软件安全监控、Pico 断流保持和 Emergency 都不能替代硬件急停
  • 真机遥操期间必须始终确保实体急停或手柄红色停止按钮可立即触及