使用 NVIDIA Holoscan 5 构建机器人重力补偿控制回路

想象用手引导机械臂，同时其电机提供刚好足够的扭矩来支撑其重量并保持机器人直立。这种简单的交互是机器人通过演示学习、操作物体或与人协同工作的有用起点，它依赖于重力补偿回路。

构建这样的回路只能通过基于机械臂所有连杆的精确模型（包括其尺寸、质量和惯性矩）连续计算系统的逆动力学来实现。这种计算需要快速且低抖动地完成，并且需要知道机器人的当前状态，因此重力补偿演示是机器人模型和执行这些算法的运行时的绝佳“生命证明”。

NVIDIA Holoscan 5 引入了机器人控制以及简化的传感器处理。在本教程中，我们将构建自定义的 Holoscan SDK 操作符并将它们连接成图。我们将在真实机器人上测试代码，并使用 Holoscan SDK 内置的 Nsight Systems 注解来检查延迟和抖动。

本演练已在 Jetson Orin Nano、Jetson Thor 和 x86 Ubuntu 24.04 系统上测试过。

为什么选择 Holoscan？

▶ https://developer.download.nvidia.com/devblogs/rebot-side-by-side-10M-h265.mp4

视频 1. 安装了 Holoscan 用于重力补偿工作负载的响应式机器人与自定义脚本运行时的对比

Holoscan SDK 将低延迟控制和高吞吐量感知带入一个编程模型，其执行路径专为基于 CPU 和 GPU 的工作负载设计。其目标是实现透明的运行时：在使应用程序执行明确且可检查的同时，最小化可避免的框架开销。

让控制路径专注于工作。 Holoscan 使用分区将操作符链分组到单个线程上，避免跨线程切换和不必要的系统调用，从而降低延迟和抖动。时钟和数据到达触发器表达工作应在何时运行，而有界的、丢弃最旧数据的队列防止过时的工作积累。

引入现有算法和代码，而不是为框架重写它们。 Holoscan 5 拥有新的轻量级编程模型，可以轻松集成到现有代码库中，并且将数据传入和传出 Holoscan 图从未如此简单。

添加感知而无需采用不同的编程模型。 无论您处理的是 10 kHz 的以 CPU 为中心的电机控制回路、高吞吐量的 GPU 算法还是 AI 推理，Holoscan 都允许开发者及其代理使用相同的操作符、端口和图 API 清晰地表达执行意图，将工作负载逻辑上拆分为可以共存的独立图，并能够将分区固定到专用 CPU 核心、CUDA 绿色上下文，甚至作为 CUDA 图执行。

检查计划，然后测量结果。 操作符通过时序契约表达其意图，在首次运行之前，编译后的计划会揭示应用程序将如何执行，并在部署前捕获配置错误。这意味着代理和开发者可以快速迭代设计，而无需部署到真实硬件。此外，内置的 NVTX 注解使操作符执行和消息传输在 Nsight Systems 中可见。开发者可以检查主机控制路径延迟和更新抖动，然后随着应用程序的增长重复这些测量。

一个易于使用的扭矩控制开发平台

本教程围绕 Seeed Studio reBot Arm B601-DM 构建，这是一个易于使用、价格实惠的平台，用于入门现代机器人控制。大多数低成本机械臂开发平台使用高齿轮比的业余伺服电机，仅适用于位置控制。reBot Arm 使用准直接驱动（QDD）执行器，为控制和传感提供高扭矩带宽——这对于触觉、协作机器人或其他面向扭矩的项目（如本项目）非常重要。

从重力补偿开始

在添加感知管道、运动规划器和其他高级控制之前，构建一个基本的重力补偿回路有助于提供一个有用（且有趣！）的健全性检查，以验证您的仿真模型是否准确描述了物理机器人。虽然位置控制示例可以掩盖关节轴、偏移、连杆放置、质量和力矩的问题，但所有这些都会以即时可观察的方式影响所需的扭矩；DH 参数或质量属性中的任何错误都可能导致机械臂在姿态变化时下垂或向上推。

即使您的最终应用不需要重力补偿，扭矩控制也让我们能够塑造机器人对力的响应方式——这是人类与协作机器人之间更安全交互的关键因素。我们的机械臂将重力前馈与轻微的刚度和阻尼相结合，因此它既能支撑自身重量，又能顺应人的手部动作。

这种灵活性使得时序至关重要。通过扭矩命令跟踪位置需要数百或数千赫兹的快速、一致的反馈，而运行时开销和线程切换可能会增加相位滞后和抖动，导致机器人变得不稳定。

本教程使用 Pinocchio 作为逆动力学求解器。它加载机器人的 URDF 模型并提供递归牛顿-欧拉算法（RNEA）求解器，该求解器计算指定位置、速度和加速度下的关节扭矩。我们还使用 motorbridge 库来读取关节反馈并向机械臂的电机发送命令。

设置 Python 示例

本演练使用 Holoscan 的 Python API。教程仓库还包含完整的 C++ 实现，并附有构建说明。两者实现相同的三操作符控制回路。

在 Linux 上使用 Python 3.12 或更高版本。从仓库根目录：

python3.12 -m venv build/python-venv
build/python-venv/bin/python -m pip install --only-binary=:all: -r requirements-python.txt
build/python-venv/bin/python -m pip check

检查机械臂配置文件中的串行设备和电机 ID。教程仓库的技术指南涵盖了配置、数学和硬件启动。

将控制回路表达为三个操作符

Holoscan 5 有两个编程概念：操作符（Operators）和图（Graphs）。要读取机器人状态、使用逆动力学求解器计算命令并发送给机械臂，我们可以将该问题分解为三个操作符并连接成图：

操作符是 Operator 的子类，有三个重要的方法需要实现：

表 1. 自定义 Holoscan 操作符需要实现的核心方法

以下是三个操作符的概要：

1. 按时钟读取关节状态

JointStateSourceOp 在 setup() 中声明一个输出。其 OnClock 契约请求每 2.5 毫秒一个 tick（我们以 400 Hz 运行此演示）。每个 compute() 读取所有七个电机并发出它们的状态：

import holoscan as hsdk

class JointStateSourceOp(hsdk.Operator):
    state = hsdk.Output("state", hsdk.pyobject)

    def setup(self, spec):
        spec.output(self.state)

    def contract(self):
        return hsdk.Contract().trigger(hsdk.OnClock(period=hsdk.time.us(2500)))

    def start(self):
        self.sequence = 0

    def compute(self, ctx):
        self.sequence += 1
        self.state.emit(robot.read_state(self.sequence))

2. 状态到达时计算重力补偿

PinocchioGravityOp 声明一个状态输入和命令输出。OnEach 在状态到达时运行它。hsdk.Param 声明将增益、力矩缩放和关节方向暴露为操作符参数：

class PinocchioGravityOp(hsdk.Operator):
    state = hsdk.Input("state", hsdk.pyobject)
    command = hsdk.Output("command", hsdk.pyobject)
    kp = hsdk.Param(1.5, range=(0.0, 500.0))
    kd = hsdk.Param(1.0, range=(0.0, 5.0))
    tau_scale = hsdk.Param([1.0] * 6)
    joint_direction = hsdk.Param([1.0] * 6)

    def setup(self, spec):
        spec.input(self.state, queue_depth=1)
        spec.output(self.command)

    def contract(self):
        return hsdk.Contract().trigger(hsdk.OnEach(self.state))

    def compute(self, ctx):
        state = self.state.receive_data()
        command = controller.compute(
            state, kp=self.kp, kd=self.kd,
            tau_scale=self.tau_scale, joint_direction=self.joint_direction)
        self.command.emit(command)

compute() 将观察到的状态和当前参数传递给一个辅助函数，该函数将六个机械臂角度映射到 URDF 模型，检查限制，并通过 Pinocchio 获取重力力矩：

gravity = pin.computeGeneralizedGravity(self.model, self.data, self.q)[self.v_indices]

其力矩成为电机命令中的前馈，与位置、速度、刚度和阻尼一起。机械臂从第一条命令开始使用重力前馈和配置的柔顺增益。

3. 将每条命令发送到电机

MotorCommandSinkOp 声明一个输入，并在命令到达时运行。compute() 将命令交给一个小型机器人辅助程序，该程序在发送批次之前拒绝过期、乱序或超出范围的命令：

class MotorCommandSinkOp(hsdk.Operator):
    command = hsdk.Input("command", hsdk.pyobject)

    def setup(self, spec):
        spec.input(self.command, queue_depth=1)

    def contract(self):
        return hsdk.Contract().trigger(hsdk.OnEach(self.command))

    def compute(self, ctx):
        robot.send_command(self.command.receive_data())

    def stop(self):
        robot.disable()

发送所有七个电机命令后，辅助程序记录完成时间。如果 100 毫秒（默认超时）内没有批次完成，应用程序的看门狗会尝试禁用电机并停止图。操作符的 stop() 回调也会禁用电机。

连接、编译、准备和运行

应用程序通过端口连接操作符，并在创建操作符时提供重力参数：

graph = hsdk.Graph("rebot-gravity", default_clock=hsdk.RealtimeClock)
source = graph.op(JointStateSourceOp, "joint_feedback")
gravity = graph.op(PinocchioGravityOp, "gravity", **controller.parameters.operator_kwargs())
sink = graph.op(MotorCommandSinkOp, "motor_commands")
graph.add_flow(source.state, gravity.state, queue_depth=1, queue_policy=hsdk.QueuePolicy.DROP_OLDEST)
graph.add_flow(gravity.command, sink.command, queue_depth=1, queue_policy=hsdk.QueuePolicy.DROP_OLDEST)
graph.partition("control").add(source).add(gravity).add(sink)

分区将回调在一个运行时拥有的线程上串行执行的操作符分组。将三者都保留在 control 中避免了读取状态、计算力矩和发送命令之间的线程切换。源设定节奏；其他操作符跟随数据。深度为一、丢弃最旧的队列防止旧样本堆积。

Holoscan 在启用电机之前将此描述编译为可检查的执行计划：

plan = hsdk.compile(build_graph(robot, controller))
if not plan:
    raise RuntimeError(plan.summary())

prepare_gravity_compensation(robot, controller)
hsdk.run_async(plan)

模型和电机初始化位于 prepare_gravity_compensation() 中，在图编译之后、执行之前立即进行。应用程序的信号处理程序在 Ctrl+C 时设置 stop_requested；主循环监督命令进度，会话上下文管理器停止并加入图。控制策略 涵盖启动、看门狗和关闭。

观察机械臂响应

从机械臂处于断电安全原位开始：

build/python-venv/bin/python python/gravity_compensation.py

当 Gravity compensation active 出现时，手动引导机械臂。在按下 Ctrl+C（会禁用电机）之前手动将其归位。

▶ https://developer.download.nvidia.com/devblogs/Holoscan5GravComp.mp4

视频 2. 控制器在姿态变化时更新关节力矩

使用 Nsight Systems 测量延迟和抖动

Holoscan 与 NVIDIA Nsight Systems 无缝集成，为您提供强大的工具来分析 GPU 和 CPU 工作负载的真实系统行为。NVIDIA Nsight Systems。无需跟踪装饰器或自定义计时代码，只需分析未修改的应用程序：

mkdir -p build/profiles
nsys profile -t nvtx --sample=none --backtrace=none --cpuctxsw=none \
  -o build/profiles/gravity-python \
  build/python-venv/bin/python python/gravity_compensation.py

打开时间线：

您还可以计算标准范围摘要：

nsys stats --report nvtx_sum build/profiles/gravity-python.nsys-rep

固定核心

Holoscan 的部署 API 允许我们将控制分区固定到某个 CPU。对于这些运行，我们将辅助线程和分析器线程放在另一个 CPU 上，然后使用临时 Linux cgroup 从其他应用程序中保留两个物理核心（包括它们的 SMT 兄弟线程）。这是一个简单的设置，无需重新编译内核、更改启动参数或重启，但中断、定时器和一些内核线程仍可能在这些核心上运行。

我们比较了所有四种配置，每次运行保留超过一百万个周期。Holoscan 延迟从进入第一个操作符到进入最后一个操作符，不包括电机命令传输。实线显示固定运行；虚线显示未固定运行。

表 2. C++ 和 Python Holoscan 管道在固定与未固定配置下测得的延迟分布。

固定加隔离将观察到的 p99.99 延迟降低了约 5 倍（C++）和 9.6 倍（Python）。这些顺序的分析运行排除了前 10 秒并保留了延迟异常值。

两个 Holoscan 版本都调用 Pinocchio 基于 C++ 的求解器，但 Python 在 Pinocchio 和 Holoscan 运行时周围增加了解释器执行、对象分配和 GIL 协调。这些成本对于具有微秒延迟预算的 CPU 控制回路很重要。C++ 让我们对关键路径中的分配和系统调用有更多控制（尽管此示例仍分配内存，并非零系统调用管道）。

对于基于 GPU 的感知工作负载，耗时毫秒或数十毫秒——长几个数量级——优化 CPU 上的小分配和附带系统调用通常不那么重要。Python 仍然是组合这些管道的实用方式（前提是管道元素在 CUDA 中实现）。

从重力补偿到完整控制栈

你现在已经有一个可工作的重力补偿回路，它由三个 Holoscan 算子构建：一个按时钟读取关节状态，一个用 Pinocchio 计算重力力矩，一个向电机发送命令。Holoscan 在启用电机之前编译并检查了图，Nsight Systems 展示了它的运行方式。

重力补偿是基础，而不是完成的控制器。教程仓库包含第二个 Python 示例，它将运动学和阻抗控制添加为可复用的 Holoscan 算子。它允许你注入目标位置和方向，例如来自学习策略的目标，同时机械臂保持对触摸的柔顺性。

▶ https://developer.download.nvidia.com/devblogs/hsdk5-kcartesian-hold-10M-h265.mp4

视频 3. 实时笛卡尔位置保持，在保持触觉柔顺的同时维持目标方向

在此基础上，你可以构建手把手示教、遥操作和操作。要添加感知，请使用 Holoscan 的视觉模块，以及 NVIDIA Isaac ROS 的组件，后者提供可复用的 Holoscan 算子和参考应用，例如 GR00T 推理。

后续步骤

下载 Holoscan 5 SDK 并按照此处的完整教程操作。