宇树 R1 迎宾机器人（三）：挥手与张望——关节控制与安全的动作设计

前两篇搭好了识别和输入输出，这一篇是最有"机器人味"的部分：让 R1 挥手打招呼、空闲时自然地左右张望。人形机器人在人群里做动作，安全是第一位的，所以这篇重点也在"怎么让动作又自然又不伤人"。

先认识 R1 的关节编号

R1 全身关节都有编号，做上半身动作用到的是手臂和头：

左臂: 15=肩俯仰 16=肩横滚 17=肩偏航 18=肘 19=腕
右臂: 22=肩俯仰 23=肩横滚 24=肩偏航 25=肘 26=腕
头部: 29=俯仰(点头) 30=偏航(摇头)

控制一个关节，就是给它一个目标角度（弧度）。一套动作，本质是"让某几个关节，在若干秒内，从当前角度平滑转到目标角度"。

安全第一：先把角度关进笼子

这是给人形机器人写动作和写普通程序最不一样的地方——动作幅度必须先受限，再谈好不好看。手臂大幅甩出去可能碰到人或桌子。我定了几条硬规矩：

手臂只在身体前方小幅活动，不向侧面大甩；
肩横滚最多 ±0.8 弧度（约 45°），手不会伸太远；
每套动作的每个关节角度，都先过一遍安全限位裁剪，超了就夹回来；
安全限位比机器人的实际物理限位再缩 30%，留余量。

MAX_SHOULDER_ROLL = 0.8
SAFE_LIMITS = {                 # (下限, 上限)，比物理限位缩小 30%
    22: (-2.0, 1.4),            # 右肩俯仰
    23: (-MAX_SHOULDER_ROLL, 0.15),   # 右肩横滚：只准向内一点，不准向外大甩
    25: (-0.6, 1.5),           # 右肘
    # ... 其余关节同理
}

def clamp_pose(pose):
    """把每个关节角度夹到安全范围内"""
    out = {}
    for joint, angle in pose.items():
        if joint in SAFE_LIMITS:
            lo, hi = SAFE_LIMITS[joint]
            out[joint] = max(lo, min(hi, angle))   # 超上限压到上限，超下限抬到下限
        else:
            out[joint] = angle
    return out

每一帧发给机器人之前都先 clamp_pose 一遍，这样哪怕动作定义写错了、角度填大了，也不会真的甩出危险幅度。这是兜底，很重要。

一套动作 = 几个关键帧

我把每套打招呼动作写成一串关键帧：每帧是"持续多少秒 + 哪些关节转到什么角度"。比如"右手小幅招手"：

GESTURE = {
    "name": "右手小幅招手",
    "frames": [
        # (持续秒, {关节: 目标角度})
        (0.8, {22: -1.0, 23: -0.2, 25: 1.2}),                 # 抬起右手
        (0.5, {22: -1.0, 23: -0.2, 25: 0.8, 26: 0.3}),       # 手腕摆一下
        (0.5, {22: -1.0, 23: -0.2, 25: 1.2, 26: -0.3}),      # 往回摆
        (0.5, {22: -1.0, 23: -0.2, 25: 0.8, 26: 0.3}),       # 再摆
        (1.0, DEFAULT_POSE),                                  # 慢慢放回默认姿态
    ],
}

DEFAULT_POSE 是"自然垂手站立"的姿态，每套动作最后都用 1 秒慢慢回到它，避免动作结束时手僵在半空。我准备了六套（左手招手、右手招手、双手欢迎、抱拳、轻抬致意……），识别到人时随机挑一套、且不和最近用过的重复，看起来不呆板。

让动作柔和：余弦缓入缓出

如果直接把关节从角度 A 跳到角度 B，机器人会"啪"地一下弹过去，又快又硬还危险。要在 A 和 B 之间插很多中间帧、平滑过渡。用匀速插值（线性）也生硬——真实的自然动作是"起步慢、中间快、收尾慢"。我用余弦曲线做这个缓入缓出：

余弦缓入缓出 vs 线性

import math, time

def interpolate(start_pose, end_pose, duration, fps=20):
    steps = max(int(duration * fps), 1)
    for step in range(steps):
        # 余弦插值：t 从 0 到 1，但两头慢中间快
        t = 0.5 * (1 - math.cos(math.pi * (step + 1) / steps))
        pose = {}
        for j in set(start_pose) | set(end_pose):
            s = start_pose.get(j, 0.0); e = end_pose.get(j, 0.0)
            pose[j] = s + (e - s) * t                # 关节从 s 平滑走到 e
        send_joint_command(clamp_pose(pose))         # 发之前再夹一次安全限位
        time.sleep(duration / steps)

一帧一帧地把这些中间姿态发给机器人（20 帧/秒），关节就会柔和地移动。动作在后台线程里跑，不挡住主循环的人脸识别。

说明：真正把角度发到电机的那一句 send_joint_command，要按你手上的 SDK 版本对接底层运控接口——这部分各版本写法不同，可参考宇树 SDK 的底层运动例程。但上面这套"关键帧 + 余弦插值 + 安全裁剪 + 后台播放"的框架是通用的，换任何关节控制后端都不用改。

空闲张望：让它看起来"活着"

没人的时候机器人一动不动像关机了。我加了个头部张望：空闲时让脖子缓慢地左右微摆、上下轻动，看着像在"扫视全场"；一旦看到人脸就回正、面向来宾。

做法是用两条不同周期的正弦分别驱动偏航（左右）和俯仰（上下）——周期错开（5 秒和 7 秒），两个方向不同步，看起来才自然、不机械。再叠一层指数平滑让转动更柔：

头部空闲张望轨迹

IDLE_YAW_AMP = 0.26     # 左右幅度 ±15°
IDLE_PITCH_AMP = 0.10   # 上下幅度 ±6°（小一点，只是微动）
YAW_PERIOD = 5.0        # 左右摆动周期
PITCH_PERIOD = 7.0      # 上下周期，和左右错开
ALPHA = 0.06            # 平滑系数，越小越柔

cur_yaw = cur_pitch = t = 0.0
while running:
    if mode == "idle":
        tgt_yaw = IDLE_YAW_AMP * math.sin(2*math.pi * t / YAW_PERIOD)
        tgt_pitch = IDLE_PITCH_AMP * math.sin(2*math.pi * t / PITCH_PERIOD)
    else:                                   # 看到人 → 回正
        tgt_yaw = tgt_pitch = 0.0
    cur_yaw   += (tgt_yaw   - cur_yaw)   * ALPHA     # 指数平滑，别硬跳到目标
    cur_pitch += (tgt_pitch - cur_pitch) * ALPHA
    send_joint_command({30: cur_yaw, 29: cur_pitch})
    t += 1/20; time.sleep(1/20)

那句 cur += (tgt - cur) * ALPHA 是指数平滑：每一步只朝目标挪一小截（6%），所以即使目标值变化，实际转动也是慢慢跟上去的，不会有任何突兀的跳变。幅度也压得很小（左右 15°、上下 6°），是"微微张望"而不是"疯狂甩头"。

主循环里根据有没有检测到人脸切换状态：看到人 set_alert()（回正），超过 3 秒没人 set_idle()（继续张望）。

串起来

到这里整台机器人就活了：看到人 → 头回正面向他 → 喊名字问候 → 随机挥一套手 → 灯变绿；没人时 → 慢慢左右张望。识别（第一篇）、相机语音遥控（第二篇）、动作（这篇）三块拼起来，就是一台能在年会门口迎宾的 R1。

整个项目里我体会最深的一点：给人形机器人写动作，"安全"和"柔和"不是锦上添花，是必须先做的底线——先把角度关进安全笼子、再用插值和平滑让它柔和，最后才谈动作好不好看。顺序反了就容易出事。