手臂控制例程

本例程介绍如何通过 DDS rt/lowcmd 话题控制机器人双臂关节，实现上肢实时控制。通信基于 pnd_sdk_python，消息定义见 DDS 消息定义。

例程 arm_control_example.py 为全机型通用的上肢控制示例：通过 --robot 选择机型，仅驱动手臂关节（肩、肘、腕），腿 / 腰 / 颈等其余关节保持启动时刻的实测角。关节顺序、PD 参数、目标位姿全部集中在同目录的 arm_control_config.json 中，无需改代码即可调参。

与 手臂动作服务接口（gRPC 预设动作文件播放）不同，本例程通过 MotorCmd 逐关节下发指令，侧重实时控制；二者适用场景不同，请勿混用。

---

概述

项目	说明
通信方式	DDS 发布/订阅
控制话题	rt/lowcmd（身体关节指令）
反馈话题	rt/lowstate（关节状态）
控制频率	50 Hz（control_dt = 0.02，见配置文件）
控制范围	双臂（肩 / 肘 / 腕），其余关节保持启动角
支持机型	adam_lite / adam_sp / adam_pro / adam_u
SDK	pnd_sdk_python

完整底层例程解析见 DDS 底层运动参考例程。若使用 ROS 2 中间件，可参考 ROS2 底层运动参考例程。

---

前置条件

1. 确认 强化学习 启动客户端 已完成。
2. 使用网线连接机器人与用户计算机，网络配置见 快速开发（真机）。
3. 机器人进入开发者模式（遥控器 LO + RO），RCU 指示灯为蓝色慢速呼吸。

安全提示

测试时建议将机器人悬挂或置于开阔场地，运行前请确保机器人周围没有障碍物。程序会以“复位 → 张开 → 放下”的顺序移动双臂，每步前均需按回车确认。

---

获取例程

git clone https://github.com/pndbotics/pnd_sdk_python.git
cd pnd_sdk_python && sudo pip3 install -e . --user

例程位于 pnd_sdk_python/example/low_level/：

example/low_level/
├── arm_control_example.py      # 上肢控制例程（本文，全机型通用）
├── arm_control_config.json     # 关节顺序 / PD / 位姿配置
├── adam_lite/adam_lite_low_level_example.py   # 各机型底层全身例程
├── adam_sp/adam_sp_low_level_example.py
├── adam_pro/adam_pro_low_level_example.py
└── adam_u/open_arm.py

🔗 仓库地址：pnd_sdk_python

---

运行例程

以 Adam Pro 为例（需将 enp59s0 替换为实际有线网卡名）：

# 查看网卡名
ip a

cd ~/pnd_sdk_python/example/low_level
# --robot: adam_lite / adam_sp / adam_pro / adam_u（默认 adam_pro）
# --net:   DDS 网卡名（默认 lo）
python3 arm_control_example.py --robot adam_pro --net enp59s0

程序启动后为交互式流程：先提示确认机器人周围无障碍物，按回车后依次执行以下动作，每一步都需按回车确认再继续：

1. 双臂复位：回到 default 位姿；
2. 双臂张开：水平展开成 T 字姿态（spread）；
3. 双臂放下：回到 down 位姿；
4. 退出控制：手臂 kp 在约 1.5 s 内平滑降为 0，手臂进入变软阻尼状态，随后持续发送指令直到按 Ctrl+C 结束。

后台线程会全程持续发送 lowcmd（即便在等待回车或退出后），以保证指令不会过期；仅在 Ctrl+C 时停止。

退出开发者模式：遥控器 LT + B，RCU 指示灯恢复紫色慢速呼吸。

---

关节配置与位姿

所有可调参数集中在 arm_control_config.json，无需修改 Python 代码：

字段	说明
control_dt	控制周期（秒），0.02 对应 50 Hz
max_joint_velocity	关节角速度限幅（rad/s），用于位姿间平滑插值
robots	各机型的关节顺序，决定 motor_cmd 索引
pd	各关节的 kp / kd
poses	default / spread / down 三组手臂目标位姿（单位 rad）

例程会自动完成两件事：

• 识别手臂关节：按关节名包含 shoulder / elbow / wrist 的关键字挑选出手臂关节索引，其余关节（腿、腰、颈）保持启动时刻的实测角度；
• 匹配机型自由度：关节数量由配置中的 robots 顺序决定（例如 Adam Lite 与含腕俯仰/横滚的机型自由度不同），无需手动填写索引。

新增一个自定义位姿只需在 poses 中增加一组关节角，并在代码中调用 move_to(current, arm_target("你的位姿名")) 即可。

---

代码解析

例程核心流程如下：

1. 初始化通信

from pndbotics_sdk_py.core.channel import (
    ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize,
)
from pndbotics_sdk_py.idl.default import pnd_adam_msg_dds__LowCmd_
from pndbotics_sdk_py.idl.pnd_adam.msg.dds_ import LowCmd_, LowState_

ChannelFactoryInitialize(1, args.net)
pub = ChannelPublisher("rt/lowcmd", LowCmd_); pub.Init()
sub = ChannelSubscriber("rt/lowstate", LowState_); sub.Init(low_state_handler, 1)

2. 保持非手臂关节：从首帧 low_state 读取全部关节实测角，作为非手臂关节的保持目标。

hold_q = [low_state.motor_state[i].q for i in range(n)]

3. 构造指令：所有关节写入 mode=1 与保持角，再用目标位姿覆盖手臂关节的 q；退出时对手臂 kp 乘以缩放系数实现“变软”。

def set_cmd(arm_q, arm_kp_scale=1.0):
    for i in range(n):
        cmd.motor_cmd[i].mode = 1
        cmd.motor_cmd[i].q  = float(hold_q[i])
        cmd.motor_cmd[i].kp = float(kp[i])
        cmd.motor_cmd[i].kd = float(kd[i])
    for k, j in enumerate(arm_ids):   # 仅覆盖手臂关节
        cmd.motor_cmd[j].q  = float(arm_q[k])
        cmd.motor_cmd[j].kp = float(kp[j] * arm_kp_scale)

4. 后台持续发送：独立线程以 control_dt 周期调用 pub.Write(cmd)，避免指令过期。

def publisher_loop():
    while True:
        pub.Write(cmd)
        time.sleep(dt)
threading.Thread(target=publisher_loop, daemon=True).start()

5. 平滑到位：move_to 按 max_joint_velocity * dt 对每关节增量限幅，逐步逼近目标位姿。

def move_to(current, target):
    while np.max(np.abs(target - current)) > 1e-3:
        current += np.clip(target - current, -max_delta, max_delta)
        set_cmd(current)
        time.sleep(dt)
    return target.copy()

---

典型控制流程

安装 pnd_sdk_python → 进入开发者模式 → 连接网络 → 运行 arm_control_example.py
  → 依次回车执行 复位/张开/放下 → 退出控制（变软）→ Ctrl+C 结束

---

与其他接口的关系

能力	接口	文档
实时上肢关节控制（推荐）	DDS rt/lowcmd	本文
预设上半身动作播放	gRPC SetMotion	手臂动作服务接口
全身轨迹跟踪	gRPC SetTrackingMotion	手臂动作服务接口
底层全身例程	DDS rt/lowcmd 完整示例	DDS 底层运动参考例程
灵巧手控制	DDS rt/handcmd	灵巧手控制说明
ROS 2 替代方案	lowcmd / handcmd	ROS2 底层运动参考例程

---

常见问题

现象	可能原因	处理建议
卡在“等待 low_state 数据”	未进开发者模式或网络未连通	确认 LO+RO、网段与 --net 网卡参数
提示“未知机型”	--robot 取值不在配置内	使用 adam_lite / adam_sp / adam_pro / adam_u
手臂无响应	Kp/Kd 为 0 或未进开发者模式	检查 arm_control_config.json 的 pd，确认已进开发者模式
程序报错找不到网卡	网卡名参数错误	用 ip a 查看后传入正确接口名