训练我的人形机器人：用底层控制让 Unitree R1 的肘关节动起来

TLDR

使用带有明确轨迹的低层通信，我们让 Unitree R1 的肘关节上下运动。在此过程中，我们会绕道经过构型空间与拓扑学、微分几何、刚体运动、PD 控制，以及 DDS 管道，每个部分末尾都会有一些 Python 代码。文中还散布着一些很酷的交互式图表。

但在这一切之前，先来一段机器人旋转肘击墙壁的必备视频。

那堵墙毫无胜算。

多亏了我的妻子 Sabrina，我拿到了 Unitree 的新款 R1 EDU。它附带了我还没安装的灵巧手，以及一个计算背包，我安装时搞砸了，因为以太网端口被胶带盖住了，哈哈。

这篇文章主要讲我如何使用低层控制让机器人做一个简单的动作。打鼓的最简化版本可能只是移动肘关节，但实际上这主要是为了让我熟悉这个机器人及其工作原理，并从中产出一些内容。

当然，如果你读了我刚写的东西然后说“呃，不对”，那你是对的。但我必须确保肘部运动在某种程度上与真实动作相符。打鼓实际上会涉及手腕和肘部，控制反弹，而在更高速度下，或者双击和三击时，就会用到手指。我上过大约一年非常基础的打鼓课，这刚好够我知道这些。无论如何，所有这些都要花很长时间，所以我想至少分享一些基础的东西。

你以为我用这个动作来干什么……

在此过程中，我们会做一个非常基础的机器人学入门，只不过是用真实的机器人。我们只看一个关节，即我实际控制的肘关节，它如何连接到连杆，以及一些基本术语，这样你就不必去查阅参考资料。你显然可以在机器人学书籍中查到所有这些，但为了这篇文章你不应该需要那样做。

肘部上下移动。这被称为旋转关节，意思是它绕一个固定的轴旋转。

我没有严格定义关节，但你可以把它看作两个刚体之间的连接，具有某个正数的自由度。我也没有定义那些。不幸的是，在这一点上我对微分几何的了解比机器人学更多，哈哈，所以我们可能应该定义一些东西。

定义刚体、关节、自由度

刚体是一种无论经过何种变换，其所有点之间的距离都保持不变的身体。刚体运动是直接等距变换，因此它既保持距离又保持定向。（请不要问定向，否则我会说它是最高非零上同调群生成元的选择。）

关节是两个刚体之间的连接，具有某个正数的自由度，而自由度的数量就是构型流形的维数。刚体本身就是连杆，所以在手臂上，上臂和前臂是连杆，它们之间的肘部是关节。

如果不受约束，上下移动我的肘部只会让它在一个圆上运动。所以从拓扑上讲，该关节可以处于的位置空间是 S1。注意 R1 的关节限位发布在开发者文档中。

定义构型空间

机构的结构空间 C 是它能处于的所有位置的集合，每个位姿对应 C 中的一个点。对于单个无约束的旋转关节，C≅S1：一个角度，且 θ 与 θ+2π 是同一个构型。最后这一点正是它是个圆而不是一条直线的原因。

当然，它不能只是一个集合。它需要拓扑，否则 S1×S1 和 S2 就会是同一个东西。

这个关节显然连接在地面上，但在这里我们把机器人的其余部分当作地面，只看悬挂在它上面的这一个关节。

一旦我们施加约束，就会看到它不再绕圈。从上面的链接可以看出，这个机器人上的关节是有约束的，肘部在弧度上有某个最小值和最大值。所以看看仅仅通过添加限位，拓扑就发生了怎样的变化，因为区间是可收缩的，而圆不是。

两个关节

现在取两个无约束的旋转关节。每个关节各自是一个圆，且它们互不影响，所以结构空间是乘积

T2  :=  S1×S1,

也就是一个环面。我没有定义环面，但我假设你脑中有一张沿对边粘合的纸的画面（另见下面的交互演示）。

现在把它与球面 S2 比较。两者都是二维流形，所以局部看起来一样，因为其中任意一点都有一个邻域同胚于 R2 的开子集。全局来看它们完全不同，而这种差异是拓扑性的。

我喜欢用连杆指向的方向来看待它，这个方向位于 S2 上，所以想象一根杆一端固定、另一端可自由指向任意方向。如果你把它指向北极，并使用相同的旋转角，它就退化为完全不动。准确地说：如果我在北极，并指定纬度和经度，那么改变经度实际上根本不会让我移动。这是坐标的失效，而不是球面的失效，而且这正是被称为万向节锁的同一个失效。

注意万向节锁

球关节的构型空间是 SO(3)，它是一个紧致 3-流形，因此不存在单一的全局坐标图。所以任何用三个角度（欧拉角）进行的参数化都必然在某处退化：在奇异构型下，三个轴中的两个对齐，参数化就丢失了一个自由度，尽管关节本身并没有丢失任何东西。参见 万向节死锁。

人们实际规避这个问题的方法是干脆不再使用三个角度。单位四元数用四个数携带一个旋转，并有一个约束把它们限制在一个球面上，而由于它从来就不是一个三坐标图，也就没有图可供退化。不过这丝毫没有改变 SO(3) 的拓扑，只是不再假装 SO(3) 是一盒三个旋钮。

真正把它们区分开的是基本群。球面是单连通的，而环面不是：

π1(S2)≅1,π1(S1×S1)≅Z×Z.

所以球面的 π1 是平凡群，意味着每个环路都收缩到一个点，而环面的 π1 是两个生成元上的自由阿贝尔群，每个圆各对应一个环路。严格来说 π1 需要一个基点，但这些空间都是道路连通的，所以你选哪个点得到的群都一样。

这是我在本文中真正关心的部分。运动是一个拓扑问题，而拓扑关乎形状，不关乎维数。环面和球面维数完全相同，而这一事实对我们毫无帮助：数自由度只能告诉你写下一个位姿需要多少个数字，却完全不能告诉你当你开始转动它们时会发生什么。

顺便我们还可以看看两个圆的楔和，也就是两个圆在单点处粘合，

π1(S1∨S1)≅F2,

两个生成元上的自由群，它是非阿贝尔的，所以现在环路的顺序变得重要了。在环面上 ab 和 ba 是同一个类，而在楔和上它们不是。

注意楔和甚至不是流形

S1∨S1 上除粘合点外的每个点都有一个看起来像区间的邻域。粘合点则没有：它任意足够小的邻域都是四条弧交于一点，而 R1 的任何开子集都不长这样。

看这一点最快的方法是删掉这个点。从区间中取走一个内点，剩下两块。从它的邻域中取走粘合点，剩下四块，而同胚不可能改变你得到多少块。所以楔和恰恰在我们关心的那一点上不满足定义。

移动机械臂究竟意味着什么

现在我们来谈谈实际移动机械手意味着什么，特别是在 R1 上意味着什么。要求一个关节到达某个位置，就意味着计算一个力矩。

先快速说一下力矩，因为下面所有内容都以它为单位。让一个质量沿直线运动需要力，旋转某物也没什么不同，只是以角度的方式发生：摆动一个连杆也需要用力，而力矩就是这种力的旋转版本。它是你在离轴一定距离处推的结果，所以远处轻推和近处重推效果相同。牛顿也一并参与进来，F=ma 变成了

τ=J q¨，

其中转动惯量 J 扮演质量的角色，q¨ 是角加速度。本页每个交互式演示都在积分这个方程，对于肘部大小的物体，J=0.05 kg m2。

所以我们实际发送的力矩公式就是标准的 PD 控制器加上一个前馈力矩：

τ=Kp (qdes−q) + Kd (q˙des−q˙) + τff

这里 q 和 q˙ 是关节的测量位置和速度，qdes 和 q˙des 是我们要求的值，τ 是以 N m 为单位的力矩。这使得 Kp 成为以 N m/rad 为单位的刚度，Kd 成为以 N m s/rad 为单位的阻尼，这一点值得记住，因为这两个增益不在同一量级上，你不能直接比较它们的数值。

所以就是位置偏差乘以 Kp，速度偏差乘以 Kd，然后再加上你想直接添加的任何力矩。它真正告诉我们的是如何根据误差定期调整力。你查看当前位置与你被要求到达的位置，查看你缩小这个差距的速度，然后相应地施加力。Kp 项是把你拉向目标的弹簧，Kd 项是防止你冲过目标的阻尼器。

有趣的是，你已经知道不少这类东西。定速巡航就是一个控制器，其中误差是你的设定速度与实际速度之差，输出是油门。自动扩缩容也是一个控制器，其中误差是目标利用率与实际利用率之差，输出是你添加的实例数量。这两者通常都是 PID 而不是 PD，所以它们还会对误差随时间积分，以消除稳态偏差。

以下是我实际使用的增益值。这些是直接来自 R1 教程的默认值，我还没有坐下来好好调过，所以把它们当作起点，而不是经过深思熟虑的结果。

# stiffness, the proportional part of the PD controller, in N·m/rad
# think of the spring equation, but angular: the further the joint is from
# where we asked it to be, the harder this pulls it back
Kp = [
    200, 200, 200, 200, 200, 200,     # legs
    200, 200, 200, 200, 200, 200,     # legs
    300, 300,                         # waist
    100, 100, 100, 100, 50,           # arms
    100, 100, 100, 100, 50,           # arms
    50, 10                            # head
]

# damping, the derivative part, in N·m·s/rad
# this one reads velocity rather than position, so it pushes against however
# fast the joint is already moving, which is what keeps the spring above from
# overshooting and ringing
Kd = [
    3, 3, 3, 3, 3, 3,                 # legs
    3, 3, 3, 3, 3, 3,                 # legs
    5, 5,                             # waist
    2, 2, 2, 2, 2,                    # arms
    2, 2, 2, 2, 2,                    # arms
    2, 0.1                            # head
]

每个关节一个条目，共 26 个。腿部和腰部最硬，这很合理，因为它们是支撑机器人对抗重力的部分，手臂较软，头部更软，最后一个关节的 Kp=10，在那里你宁愿它让步而不是跟你较劲。

两个列表都按关节索引，这个索引值得放在你面前，还有到线格式的映射以及每个关节被钳制的限值。

参考关节索引、IDL 映射和限值

# continuous indices
class R1JointIndex:
    LeftHipPitch = 0
    LeftHipRoll = 1
    LeftHipYaw = 2
    LeftKnee = 3
    LeftAnklePitch = 4
    LeftAnkleB = 4
    LeftAnkleRoll = 5
    LeftAnkleA = 5
    RightHipPitch = 6
    RightHipRoll = 7
    RightHipYaw = 8
    RightKnee = 9
    RightAnklePitch = 10
    RightAnkleB = 10
    RightAnkleRoll = 11
    RightAnkleA = 11
    WaistRoll = 12
    WaistYaw = 13
    LeftShoulderPitch = 14
    LeftShoulderRoll = 15
    LeftShoulderYaw = 16
    LeftElbow = 17
    LeftWristRoll = 18
    RightShoulderPitch = 19
    RightShoulderRoll = 20
    RightShoulderYaw = 21
    RightElbow = 22
    RightWristRoll = 23
    HeadPitch = 24
    HeadYaw = 25
    
# map from the contigous index to the actual IDL index
# of the data structure that will be sent across
# joint index
joint_idx_in_idl = [
    0, 1, 2, 3, 4, 5,
    6, 7, 8, 9, 10, 11,
    12, 13,
    15, 16, 17, 18, 19,
    22, 23, 24, 25, 26,
    29, 30
]
    
# (min, max) joint limits in rad
R1_JOINT_LIMITS = [
    (-2.9322, 2.5482),
    (-1.0472, 1.7453),
    (-2.7402, 2.7402),
    (-0.1745, 2.4260),
    (-0.8727, 0.5760),
    (-0.2618, 0.2618),
    (-2.9322, 2.5482),
    (-1.7453, 1.0472),
    (-2.7402, 2.7402),
    (-0.1745, 2.4260),
    (-0.8727, 0.5760),
    (-0.2618, 0.2618),
    (-0.5236, 0.5236),
    (-2.6180, 2.6180),
    (-3.1416, 2.0944),
    (-0.2269, 2.4784),
    (-1.9199, 1.9199),
    (-0.9757, 2.1850),
    (-1.9199, 1.9199),
    (-3.1416, 2.0944),
    (-2.4784, 0.2269),
    (-1.9199, 1.9199),
    (-0.9757, 2.1850),
    (-1.9199, 1.9199),
    (-0.6283, 0.6283),
    (-2.0071, 2.0071),
]

其中有两件事值得单独拿出来说。每个手臂块依次是肩部俯仰、肩部滚转、肩部偏航、肘部、腕部滚转，所以整篇文章讨论的这个关节是 LeftElbow = 17，它以 Kp=100 N m/rad 和 Kd=2 N m s/rad 运行。它的限值是 [−0.9757, 2.1850] 弧度，所以大约从 −56° 到 125°：范围是 181°，略多于半圈，远不及无约束关节本可以拥有的整圈。这个区间就是第一节中那个可收缩的东西，写下来就是这样。

joint_idx_in_idl 列表是另一个需要注意的。上面整齐连续的编号并不是上线格式，所以每条命令都必须通过那个映射来写，这就是为什么 joint_idx_in_idl[i] 会出现在下面代码的每一行。

与机器人通信

现在这将是纯粹的工程问题，即我们实际上如何与机器人本身通信。它是 DDS，具体来说是 Cyclone DDS。出于这些目的，我还没有真正深入研究规范，但如果你想了解更多，可以在那里查看。

大大简化来说，DDS 有点像发布订阅。它显然不仅仅是发布订阅，但我喜欢这样想：你从机器人读取状态，然后向机器人发送命令。我们发送的命令基本上就是所有电机的状态，而每个电机的状态就是我们接收到的状态。

这就是它在 Python 中的实际样子。这里面有相当多的冗余代码，但在这一切背后只有两件事在发生：我们订阅机器人的状态更新，并定期发送命令回去。其他一切都是设置。（这些是我控制器类上的方法，还有一个 C++ SDK 以相同方式工作。）

def Init(self):
    # first we typically shut down the motion switcher client
    # coming from the remote controller
    self.msc = MotionSwitcherClient()
    self.msc.SetTimeout(5.0)
    self.msc.Init()

    # keep releasing until no mode is holding the robot
    status, result = self.msc.CheckMode()
    while result is None or result["name"]:
        self.msc.ReleaseMode()
        status, result = self.msc.CheckMode()
        time.sleep(1)
    self.msc.ReleaseMode()

    # sending commands to the robot
    # create publisher #
    self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
    self.lowcmd_publisher_.Init()

    # receiving robot state
    # create subscriber #
    self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
    self.lowstate_subscriber.Init(self.LowStateHandler, 10)

def Start(self):
    # LowCmdWrite gets called every control_dt_ seconds, forever
    self.lowCmdWriteThreadPtr = RecurrentThread(
        interval=self.control_dt_, target=self.LowCmdWrite, name="control"
    )

    # don't start writing until the first state message has told us
    # which mode machine we're talking to
    while not self.update_mode_machine_:
        time.sleep(1)

    self.lowCmdWriteThreadPtr.Start()

# the read side: fires on every state message from the robot
def LowStateHandler(self, msg: LowState_):
    self.low_state = msg

    # seed the shared state before releasing Start(), otherwise the control
    # thread can begin a tick while desired is still empty
    self.shared_state.set_low_state(msg)

    if not self.update_mode_machine_:
        self.mode_machine_ = self.low_state.mode_machine
        self.update_mode_machine_ = True

所以 Init 释放遥控器让机器人所处的任何模式，然后恰好打开两个通道，rt/lowcmd 用于写入，rt/lowstate 用于读取。LowStateHandler 是读取端，每当机器人有话说时就会触发。写入端是 LowCmdWrite，它改为按定时器触发，每 control_dt_ 秒一次，那才是轨迹真正变成消息的地方。等我们有了值得写入的轨迹，就会讲到它。

底线是我们以某个频率持续获取状态，比如每秒 500 次，并以相同速率发送命令回去。那个读写循环就是整个接口。这部分实际上没有任何机器学习，只是运动和轨迹。

不过，它是强化学习的基础。我这么说，意思是： 想象一下，不是我们试图硬编码一条轨迹，而是机器人学习一种策略， 在每个时刻读取机器人的状态，并告诉执行器期望的 位置、速度、加速度等等。显然，状态可能是一个大得多的 东西，比如房间或摄像头输入，但最简单地说，它就是我们已有的同一个读和 写的循环。

轨迹

理想情况下，我们想为肘部创建一条轨迹，而我显然选择了一条振荡的 轨迹。它必须是平滑的，最重要的是它必须是连续的。

那么，什么是轨迹？它基本上是位置流形上的一条曲线， 可选地还有速度和其他。

以下是间断点处发生的情况。假设我们将机器人重置到零状态，因此 q=0，然后我们立即告诉机器人我们希望它处于 −10 弧度。因为 我们每 Δt=1/500 秒请求一次，所以在那一个 tick 内你需要的平均速度是

ΔqΔt  =  −10 rad0.002 s  =  −5000 rad/s,

而 PD 律会欣然要求一个与 10 弧度误差成比例的力矩。别管 10 弧度早已超出肘部的极限，重点是算术会做什么。 这里肯定会出问题。如果我要求你在两 毫秒内移动那么远，对你来说也会相当具有挑战性。

在真实机器人上，这表现为一个缠绕的猛冲。控制器看到巨大的误差， 命令它被允许的任何力矩，关节猛地冲向目标，然后 阻尼项必须在另一侧接住它。

这正是本帖顶部的视频。那不是失去 平衡的机器人，而是一个被要求去它在给定时间内无法到达的地方的机器人， 在尽力而为，并带着墙壁和地板一起。同样的算术，只是涉及了更多的手臂部分。

所以这意味着我们需要给机器人一个连续且平滑的位置。那么 这里平滑是什么意思？我们要求存在且连续的每一阶导数，都为我们 多买来一样不能跳变的东西：

q∈C0:q 从不 跳变q∈C1:q˙ 存在 且 连续q∈C2:q¨ 存在 且 连续

这里要小心，因为连续性本身并不能保证速度方面的事情。 一个连续函数完全可能处处不可微。它真正保证的是 指令位置永远不会跳变，而这正是上面导致力矩爆炸的原因。 如果你想要速度不跳变，那就是 C1，而加速度不跳变 则是 C2。加加速度是三阶导数 q...，所以实际上要约束它需要 C3，或者 至少是 Lipschitz 的 q¨。在实践中 C2 已经足够了。

另外，“光滑”这个词在这里承担了两个作用。对于轨迹，我只是指 我们正在做的事情所需的足够多阶导数。对于下面的流形内容，它指的是 C∞。

所以现在你明白我在暗示什么了。我们已经讨论过构型空间和 位置是流形，因为坐标系上施加的约束， 以及它们如何在局部移动、在局部能沿多少个维度移动，还有 我们看到的一些全局约束，比如万向节锁。现在我们还想要引入 光滑性，原因是我们希望能够求速度、 加速度等等，换句话说就是无穷多阶导数。在现实世界中 这很可能只是有限多阶，因为到了某个点你的高阶导数 越来越不重要，但大致就是这个意思。

流形、光滑结构、李群

定义拓扑流形

一个第二可数的 Hausdorff 拓扑空间，其中每个点都有一个邻域 同胚于 Rn 的一个开子集。那个 n 被称为 流形的维数，而且它实际上是良定义的，因此同一个空间不可能既是 n 维又是 m 维，这就是区域不变性。

虽然拓扑流形的定义乍看起来有些突兀，但 要求第二可数的原因在于某些局部到全局的约束和单位分解， 以便能够在流形上适当地粘合东西。具体处理参见 参考文献中的 Lee，任何微分几何书都会涵盖它。

定义光滑结构

一个极大图册，其转移映射是 C∞。极大性很重要，否则 两个完全兼容的图册会被算作不同的结构。参见参考文献中的 Lee， 因为这里其实不是关于这个定义的。

严格来说，图卡只是定义光滑性的一种方式，还有其他的。例如， 我们可以抛开在某个特定点发生什么，转而使用函数的芽 及其正向极限，也就是预层并无限地放大到该点。 我知道的另一种方式是微分空间，进而通向分层流形。然后还有 微分空间。同样的事情，这里都不涉及，参见参考文献。

现在看看我们如何从一个流形开始，基于一些有用的东西逐步添加越来越多的抽象。当我们谈论旋转刚体和平移它们时，我们开始思考复合和可逆性，显然这些让我们想起群结构，一个联合连续且必须尊重流形上的运算和光滑结构的群结构。这在实践中意味着，如果我做一个旋转再做另一个旋转，复合也是该空间的一个元素，而且它也是一个旋转。

定义李群

一个同时也是光滑流形的群，其乘法和求逆都是光滑映射。

注意我们如何自然地从流形开始，然后添加光滑性，再添加群结构。每一步都保留对象，并对它们之间的映射提出更多要求：连续，然后 C∞，然后还要尊重乘法。我想在某个时候画出它的范畴图，但现在还没有。

对于一个肘部我们不需要这些，但既然我们已经搭好了梯子，不妨看看坐在它顶端的东西。

回到机器人编程

所以我想要一个非常简单的上下振荡的轨迹，因为我听到“振荡”这个词，通常就需要某种周期函数。在这种情况下，我就选择一个正弦：

q(t)=Asin⁡(ωt)

为什么我选择正弦？因为我一开始把机器人放在零位，比如说前三秒我把所有位置重置为零，我不希望在零处有间断。由于 sin⁡0=0，轨迹正好从机器人已经在的位置开始。所以我就让它成为一个普通的老式正弦。

有趣的是，这可以追溯到参数曲线，因为归根结底我们只是通过改变 t 来描述一条曲线。而且不只是 3D 空间中的曲线，而是那个构型空间流形上的曲线：

γ:[0,T]→C,γ(t)=[ Asin⁡(ωt) ].

我们在正弦外面加上大括号，是为了提醒它实际上是一个等价类。Asin⁡(ωt) 是一个数，而 C 是一个圆，所以要落到 C 中，我们把这个数读作角度，识别 S1=R/2πZ。对于我们的肘部来说这几乎无关紧要，因为关节限位使整个轨迹都位于一个坐标卡内，我们可以直接使用角度，但正弦本身并不是流形上的一个点，除非你说清楚如何读取它。

所以一个关于 t 的普通正弦确实描述了其构型空间上的某种曲线。

仅位置

你可以看到，基本版本的上上下下实际上并没有指定任何速度。留下 q˙des=0 并不意味着速度项不起作用，它意味着该项变成了 −Kd q˙，所以它是纯粹的阻尼，对抗你已有的任何运动。你在同时告诉关节要处于一个移动的目标位置，又要保持静止，而你得到的是两者之间的折中。

由此产生的滞后在关节运动最快的地方最严重，对于正弦来说就是行程的中段，而在峰值处最小，因为那里 q˙ 本来就趋于零，两个要求不再冲突。所以运动并不会崩溃，它只是落后了，并且比你要求的幅度更浅。在下面的速度交互中，中段误差约为四分之一弧度，而在折返处只有百分之几弧度，你可以在那里打开和关闭它，观察差距的张开和闭合。

虽然我们把期望轨迹描述为正弦，但控制器从未接收到一个连续函数。在 500Hz 下，我们只是对正弦采样，每 2ms 发送一个新的位置目标。机器人接收到的是离散的设定点序列，而由此产生的采样点非常接近我们心中所想的连续轨迹。

这是可以玩的完整内容。把 Δt 拖大，直到拐角变得明显，然后点击按钮回到机器人实际运行的频率。

这是 Python 的写入端，上面每一个想法最终都在这里变成电机接收到的内容。

# send the message to the robot
def LowCmdWrite(self):
    # tick the time
    self.time_ += self.control_dt_

    if self.time_ < self.duration_:
        # stage 1
        # set robot to zero posture
        # for unitree R1 it's actually holding the elbows at 90 deg
        # like so
        #
        #   shoulder ─── o
        #                │
        #                │        upper arm, straight down
        #                │
        #      elbow ─── o ──────  forearm, out at 90 deg
        #
        # forgive my terrible ascii art skills
        for i in range(R1_NUM_MOTOR):
            ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
            self.low_cmd.mode_pr = Mode.PR
            self.low_cmd.mode_machine = self.mode_machine_
            self.low_cmd.motor_cmd[
                joint_idx_in_idl[i]
            ].mode = 1  # 1:Enable, 0:Disable
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].tau = 0.0
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].q = (
                1.0 - ratio
            ) * self.low_state.motor_state[joint_idx_in_idl[i]].q
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].dq = 0.0
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].kp = Kp[i]
            self.low_cmd.motor_cmd[joint_idx_in_idl[i]].kd = Kd[i]

    else:
        # stage 2
        # move the left elbow up and down
        i = R1JointIndex.LeftElbow
        idl_i = joint_idx_in_idl[i]
        max_angle = 20 * np.pi / 180
        t = self.time_ - self.duration_

        self.low_cmd.mode_pr = Mode.PR
        self.low_cmd.mode_machine = self.mode_machine_

        self.low_cmd.motor_cmd[idl_i].mode = 1
        self.low_cmd.motor_cmd[idl_i].tau = 0.0
        self.low_cmd.motor_cmd[idl_i].q = max_angle * np.sin(t)
        self.low_cmd.motor_cmd[idl_i].dq = 0.0
        self.low_cmd.motor_cmd[idl_i].kp = Kp[i]
        self.low_cmd.motor_cmd[idl_i].kd = Kd[i]

    self.low_cmd.crc = self.crc.Crc(self.low_cmd)
    self.lowcmd_publisher_.Write(self.low_cmd)

有几点需要注意，因为它们都是之前那些抽象内容在实际代码行中的体现。

self.time_ += self.control_dt_ 是唯一的时钟。任何地方都没有墙钟时间，我们只是数节拍，这就是为什么 tk=k Δt 精确成立。

阶段 1 是复位到 0.0 位置。每个节拍我们从 LowState 取最新的测量关节位置，并在 duration_ 内将其缩放到 0，所以到阶段 2 开始时，我们有 q=0 和 sin⁡0=0 彼此一致。

阶段 2 是整篇文章的六行代码。一个关节，R1JointIndex.LeftElbow，得到 q(t)=Asin⁡(t)，其中 A=20°，即 0.349 rad，舒适地处于那个 [−0.9757, 2.1850] 限制之内。表达式中没有 ω，所以 ω=1 rad/s，周期为 2π≈6.3 秒，这就是为什么视频中的机械臂移动得如此不紧不慢。

然后是实话实说的部分：dq = 0.0 和 tau = 0.0，作用在每个电机上，两个阶段都是如此。所以这正是只有位置的情况，上面描述的 −Kd q˙ 纯阻尼并不是假设，而是这段代码实际做的事。Kp[i] 和 Kd[i] 是前面给出的逐关节表，这就是为什么承载整条手臂的肩部得到 100，而挂在末端的腕部得到 50。

这是在手臂上运行的效果。

加入速度

所以现在我们还想要加入速度，基本上是为了在最顶端把速度平滑地降到零。显然最简单的方法就是用余弦，也就是正弦的导数：

q˙(t)=A ωcos(ωt)。

在这些峰值处，sin(ωt)=±1，我们得到 cos(ωt)=0，这正是我们希望导数为零的地方。现在阻尼项有了可以相减的对象，所以 −Kd q˙ 不再全程对抗运动，而只对我们要求的速度和实际得到的速度之间的差值施加反向作用。

在代码里这只是改一行，因为我们本来就把速度留为零：

self.low_cmd.motor_cmd[idl_i].q  = max_angle * np.sin(t)
self.low_cmd.motor_cmd[idl_i].dq = max_angle * np.cos(t)   # was 0.0

正弦变成余弦，就这样。值得知道的是，这之所以能如此干净地成立，是因为这里 ω=1：一般来说 Asin(ωt) 的导数是 Aωcos(ωt)，所以你需要把 ω 一路带上，忘了它很容易导致要求的速度和你自己的位置指令不一致。

所以基本上就是这样。我要做的就是把它编程实现，并且不犯任何代数错误。我喜欢这样指定它的地方在于，我可以事先画出这些函数，确保没有不连续之类的问题。显然，一旦在机器人上运行，如果你搞砸了，它就会抖动之类的。

可视化摆姿器

做完那个之后，我接下来快速做的是一个可视化摆姿器。Unitree 应用里也埋了类似的东西，但拥有自己的、可以从终端启动的东西很不错，不用安装应用再拖来拖去，而且我可以随意扩展它。

UI 本身相当快速粗糙，只是用 DearPyGui 给每个关节配一个滑块，然后我让 Claude Code 把主题重做成像样一点的样子。

我实现它的方式相当原始。每当有人改变滑块值时，我们显然会做防抖并请求轨迹变更。一旦我们指定了那个轨迹变更，你就把那个关节锁定到一条轨迹上，不再去动它，这就是下面代码里的 self.shared_state.trajectories 字典：滑块写入一个条目，控制线程每个 tick 采样它，当它报告完成时就退出。之后关节就停在那里，用 Kp 项所需的任何力矩来保持位置。

写入侧稍微增长以支持它，因为现在任何关节都可能附带一条轨迹，而大多数关节没有：

# the write side: fires every control_dt_ seconds, forever
def LowCmdWrite(self):
    # tick the time
    self.time_ += self.control_dt_

    # nothing to say until the first state message has arrived
    if not self.shared_state.seeded:
        return

    self.low_cmd.mode_pr = Mode.PR
    self.low_cmd.mode_machine = self.mode_machine_

    if self.time_ >= self.init_duration_:
        self.update_trajectories()

    if self.time_ < self.init_duration_:
        # the reset: scale every joint's measured position down to zero over
        # init_duration_ seconds, so we start from a known pose instead of
        # from wherever the arm happened to be left
        for idx in range(R1_NUM_MOTOR):
            idl_idx = joint_idx_in_idl[idx]
            t = np.clip(self.time_ / self.init_duration_, 0.0, 1.0)
            cmd = self.low_cmd.motor_cmd[idl_idx]
            cmd.mode = 1
            cmd.kp = Kp[idx]
            cmd.kd = Kd[idx]
            cmd.tau = 0.0
            cmd.dq = 0.0
            cmd.q = (1.0 - t) * self.low_state.motor_state[idl_idx].q
    else:
        # the actual run: every joint that has a trajectory gets sampled at
        # this instant, and joints without one are simply not commanded
        for idx in range(R1_NUM_MOTOR):
            if idx not in self.shared_state.trajectories:
                continue
            idl_idx = joint_idx_in_idl[idx]
            traj_fn = self.shared_state.trajectories[idx]
            cmd = self.low_cmd.motor_cmd[idl_idx]
            cmd.mode = 1
            cmd.kp = Kp[idx]
            cmd.kd = Kd[idx]
            cmd.tau = 0.0
            cmd.dq = 0.0
            cmd.q, done = traj_fn()
            # a finished trajectory drops out and the joint just holds
            if done:
                del self.shared_state.trajectories[idx]

    self.record_diagnostics()

    self.low_cmd.crc = self.crc.Crc(self.low_cmd)
    self.lowcmd_publisher_.Write(self.low_cmd)

这还挺不错的，所以你可以尝试不同的手部位置，看看限制在哪里。当然也有规格参数，所以尽量不要把它移动太多。但这挺酷的。

下一步

肯定要买一个动作捕捉设备。我开玩笑的。是吗？

我觉得下一步会做一些有趣的强化学习相关的东西，这样你就能真正训练它，让它做事情。

参考资料

在这里留一些参考资料，以防你想了解更多。

• John M. Lee，Introduction to Smooth Manifolds，以及 Introduction to Riemannian Manifolds。
• Bredon，Topology and Geometry
• Lynch 和 Park，Modern Robotics
• Cyclone DDS 和 DDS 规范。
• Unitree R1 开发者文档。