使用 NVIDIA Holoscan 5 让机械臂感觉失重

想象一下手动引导机械臂的过程，同时其电机提供的扭矩足以支撑其重量并保持机器人垂直行走。这种简单的交互是通过演示进行学习、操纵物体或与人一起工作的机器人的有用起点，依赖于重力补偿回路。

只有根据机械臂所有环节、其尺寸、质量和惯性矩的精确模型，不断计算系统的反向动力学，才能构建这样的循环。这种计算需要在低抖动的情况下快速完成，需要了解机器人的当前状态，因此重力计算演示对于机器人模型和执行这些算法的运行时来说都是一个很好的生命证明。

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

此演示已在 Jetson Orin Nano、Jetson Thor 和 x86 Ubuntu 24.04 系统上进行了测试。

为何选择 Holoscan？

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

让控制路径专注于工作。Holoscan 使用分区在单个线程上对运算符链进行分组，从而避免跨线程传递和不必要的系统调用，从而减少延迟和抖动。时钟和数据到达触发器表示工作应运行的时间，而具有界限、拖放时间最长的队列可防止过时的工作累积。

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

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

检查计划，然后衡量结果。操作员通过定时合同表达他们的意图，在首次运行之前，编译后的计划会显示应用程序将如何执行，并在部署之前捕获配置错误。这意味着智能体和开发者无需部署到真实硬件，即可快速迭代设计。此外，内置的 NVTX 标注可在 Nsight Systems 中显示操作员执行和消息传输。开发者可以检查主机控制路径延迟并更新抖动，然后随着应用的发展重复这些测量。

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

本教程围绕 Seeed Studio reBot Arm B601-DM 构建，这是一个易于访问且经济实惠的平台，用于开始使用现代机器人控制。大多数低成本的机械臂开发平台都使用高度调整的业余爱好机，而这些机仅适用于位置控制。reBot Arm 使用准直接驱动 (QDD) 执行器，为控制和传感提供高扭矩带宽，这对于触觉反馈、协作机器人或类似的其他扭矩导向型项目非常重要。

从 Gravity – Comp 开始

在添加感知管线、运动规划器和其他更高级别的控制之前，构建基本的重力补偿循环有助于提供实用 (和娱乐性！)检查仿真模型是否准确描述了实体机器人。虽然位置控制示例可以隐藏关节轴、偏移、链路放置、质量和力矩的问题，但所有这些问题都会以可即时观察的方式影响所需的扭矩；DH 参数或质量属性中的任何错误都会导致手臂下垂或随着姿势的改变而向上推。

即使您的最终应用不需要重力补偿，扭矩控制也能让我们改变机器人对力的反应方式，这是人类与协作机器人更安全互动的关键要素。我们的手臂将重力前馈与光刚度和阻尼相结合，因此它可以支撑其重量，同时向人的手妥协。

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

本教程使用匹诺作为反动力学求解器。它加载机器人的 URDF 模型，并提供递归 Newton–Euler 算法 (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

检查 ARM 配置文件中的串行设备和电机 ID。教程资源库的技术指南涵盖配置、数学运算和硬件启动。

将控制回路表示为三个运算符

Holoscan 5 有两个编程概念：运算符和图形。要读取机器人的状态，使用反动力学求解器计算命令，并将其发送到机械臂，我们可以将该问题分解为连接到图形中的三个运算符：

运算符属于 Operator 子类别，可实现三种重要方法：

表 1. 适用于自定义 Holoscan 运算符的核心方法

以下是要构建的三个运算符的概要：

1. 时钟上读取关节状态

JointStateSourceOp 在 setup() 中声明一个输出。其 OnClock 合约每 2.5 毫秒请求一次刻度 (我们在 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()

发送全部 7 个电机命令后，助手程序会记录完成时间。如果 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 (禁用电机) 之前，请手动将其返回主页。

使用 Nsight Systems 测量延迟和抖动

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

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 延迟在 C++ 和 Python 中分别降低约 5% 和 9.6%。这些连续的分析运行排除了前十秒，并保留了延迟异常值。

两个 Holoscan 版本都调用 Pinocchio 基于 C++ 的求解器，但 Python 在 Pinocchio 和 Holoscan 运行时周围添加了解释器执行、对象分配和 GIL 协调。这些成本对于具有微秒延迟预算的 CPU 控制回路非常重要。C++ 使我们能够更好地控制关键路径中的分配和系统调用 (尽管此示例仍会分配内存，而非零系统调用工作流) 。

对于需要数毫秒或数十毫秒 (数量级更长) 的基于 GPU 的感知工作负载，在 CPU 上优化少量分配和偶然的系统调用通常无关紧要。Python 仍然是构建这些工作流的实用方法 (前提是工作流元素在 CUDA 中实现) 。

从重力补偿到完整的控制栈

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

重力补偿是基础，而不是最终控制器。教程库包括第二个 Python 示例，该示例将运动学和阻抗控制添加为可重复使用的 Holoscan 运算符。它允许您根据习得的策略注入目标位置和方向，同时手臂保持与触觉相符。

之后，您可以进行手动指导式教学、远程操作和操作。要添加感知，请使用 Holoscan 的视觉模块以及 NVIDIA Isaac ROS 的组件，后者提供可重复使用的 Holoscan 运算符和参考应用，例如 GR00T 推理。

后续步骤

下载 Holoscan 5 SDK，并在此处学习完整教程。