写在前面
在机器人技术领域中，包含运动环路的设计正变得越来越普遍。尽管已有动力学算法来处理这类环路的影响，但许多现代模拟器仍依赖于要求将机器人表示为运动树的动力学库。这一要求在描述机器人的既定标准格式：统一机器人描述格式（URDF）中得到了体现，该格式不支持导致闭链的运动环路。这里介绍了一种增强型URDF，称为URDF+，它解决了URDF的这一关键缺陷，同时保留了机器人技术领域所重视的直观设计理念和低入门门槛。URDF+保留了URDF用于描述开链的元素，并引入了新元素来编码环路关节。我们还提供了一个配套解析器，用于处理来自URDF+的系统模型，以便它们能够与用于将机体分组为局部、解耦环路的闭链系统的递归刚体动力学算法一起使用。此解析过程完全自动化，无需用户手动指定，即可确保约束机体的最佳分组。我们旨在通过开发高效且易于使用的软件工具，推动机器人技术领域向这一优雅解决方案迈进。
内容出自国内首个具身智能全栈学习社区：
具身智能之心知识星球
，
这里包含所有你想要的。
一些介绍
现代刚体动力学（RBD）库所依赖的递归动力学算法最初仅针对开链系统而开发。迄今为止，这些库尚未实现与原始递归算法一样简单或高效的处理运动环路的技术。相反，它们要么（i）近似其动力学效应，要么（ii）使用随着机器人尺寸的增加而扩展性较差的非递归算法。对运动环路缺乏关注可能是通用机器人描述格式（URDF）的原始开发者决定不支持具有运动环路的机器人建模的原因之一。尽管缺乏这种支持，URDF已成为描述机器人模型的既定标准格式。
随着涉及运动环路的设计（图1）作为实现近端驱动的一种手段而变得越来越流行，这一缺陷已不再可接受。诸如平行带传动、差速驱动和四杆机构等设计，可在将致动器的惯性集中在机器人基座结构的同时，实现高速肢体运动。
虽然最初的递归动力学算法是为开链系统开发的，但它们可以适应具有运动环路的系统。这一点首先由Jain认识到，他通过空间核运算符（SKO）来处理这个问题，并证明了具有运动环路的系统可以用SKO模型来表示。因此，这些系统与原始的递归算法是兼容的。在我们最近的工作中，从Featherstone的传播方法视角出发，独立推导出了这些算法。
这些针对闭链的“约束嵌入”算法尚未得到广泛传播。一个可能的原因是缺乏采用这些技术的高效、易用的软件工具。本文以及我们相关的RBD库的目标是推动机器人技术领域接受这一优雅的解决方案，以解决该领域面临的一个关键问题。
然而，我们要强调的是，我们的推动并不涉及从URDF范式转变。已经提出了许多值得称赞的大规模范式转变尝试。例如，仿真描述格式（SDF）提供了URDF用户要求的许多功能，如支持多个机器人、支持多种类型的传感器以及支持运动环路。MuJoCo的MJCF格式采用了与URDF相似的基于运动树的设计哲学。与URDF相比，MJCF支持更新、更详细的元素，如传感器、致动器、约束和接触。另一方面，异步多体框架（AMBF）格式则采用了完全不同的模块化设计哲学，旨在提高人类可读性和约束处理能力。
尽管做出了这些努力，但大多数业内人士认为URDF在未来将会更加普及。因此，我们接受这一观点，并选择增强而非替代URDF。我们增强的格式URDF+的主要特点包括：
-
对原始URDF数据结构的简单扩展，允许建模更多的约束；
-
一个新的解析器，它根据Jain的最小聚合标准自动生成模型，这些模型的主体被最优分组。
当系统的运动环路是“局部的”，即涉及少量的主体时，这种最优分组使得解析后的模型非常适合用于约束嵌入算法。在环路不是局部且非递归方法更高效的情况下，最优解析仍然有助于提供约束雅可比矩阵的稀疏模式。URDF+的数据结构和解析器是作为ROS URDF解析器的分支实现的，并且是开源的。
本文还提供了示例，展示了URDF+如何对与现有URDF格式不兼容的复杂闭链机器人进行建模。我们强调，URDF+保留了URDF的设计哲学，这是社区中许多人所熟悉的。URDF+文件与URDF完全向后兼容。RBD库和模拟器可以（i）更新其算法以使用完全递归技术，或者（ii）保留其现有算法，并使用新的描述格式，通过其现有的非递归算法对闭环进行建模和计算。
背景介绍
System Modeling
机器人系统通常使用图进行建模。图G由一组节点N和无向边E组成。当图G是有向的（也称为有向图）时，其边E从一个节点指向另一个节点。描述机器人系统的图被称为连接图（CG），并具有以下属性：
• 节点代表刚体。• 边代表关节。• 恰好有一个根节点代表固定的基座。• 图是无向且连通的。
当图中任意两个节点之间仅存在一条路径时，该图被称为拓扑树。连接图（CG）的生成树（ST）是一个子图，它包含原CG中的所有节点以及原CG中的一组边，使得该子图是一个拓扑树。这些包含的边构成了树关节集T。剩下的边构成了环关节集L。因此，对于一个有
个非根节点和
条边的CG，将有
个树关节和
个环关节。我们将通过刚体、树关节和环关节来描述连接图，记作
。
利用生成树的性质，我们制定了“正规编号”规则，为节点和边分配标识号：
-
-
-
将剩余的节点从1到NB进行编号，使得每个节点的编号都比其在生成树中的父节点高。
-
对生成树中的边进行编号，使得边i连接节点i和其父节点。
-
我们将使用以下索引约定来区分不同类型的关节。索引i和j将用于索引树关节和刚体（从1到
），l将用于索引环（从1到
），而k用于标识闭合环l的环关节（
）。
树关节变量数量n和环闭合约束数量
分别由以下给出：
Joint Models
机器人的配置可以通过附着在其每个刚体上的坐标系的姿态来描述。我们使用以下约定来描述一个坐标系：
。下标i表示该坐标系刚性地附着在哪个刚体上,下标j表示该坐标系与哪个关节相关联。当i = j时，我们省略j，只写
。这些坐标系及其关系的示意图如图2所示。
从
（其中λ(i)是刚体i的父刚体）变换到
，使用了两种空间变换。树变换
给出了对于任意
，
相对于
, i的姿态。关节变换是关节位置
的函数，并且取决于关节类型。
对于第k个环关节，前驱变换
是一个固定变换，它给出了
,k相对于
的姿态，其中p(k)是前驱刚体。后继变换
对后继刚体执行相同的操作。最后，关节变换
描述了从
到
的变换。
关节模型提供了描述相连刚体之间允许相对运动的信息。这些信息通过三个量来捕获：
。运动子空间矩阵
将关节速度
映射到前驱刚体和后继刚体之间6D空间速度的差异。类似地，约束力子空间矩阵
将约束力
映射到关节上的6D空间力。关节模型确定了如何在给定关节位置
的情况下计算这些量。
Loop Constraints
环关节所施加的运动约束可以以“隐式”形式来表达：
或者，在存在一组独立坐标的情况下，以显式形式表达：
其中，q代表机器人的完整坐标集，y代表独立坐标集。隐式和显式约束的雅可比矩阵和偏差由以下给出：
URDF
统一机器人描述格式（URDF）背后的主要思想是，它编码了一个运动学树。整个碰撞体（Collision Geometry，CG）的描述包含在
元素的内容中。图的节点（机器人的连杆）由元素给出。同样，图的边（机器人的树关节）由
元素描述。元素的内容描述了相应连杆的动力学和外观。例如，
子元素给出了连杆的空间惯性，而
子元素提供了其外观（形状、大小、颜色等）的信息。同样，
元素的内容描述了连杆之间的运动学约束。
和
元素给出了被约束的连杆，
给出了树变换XT，而类型和
描述了关节模型。
闭链系统的递归算法
使闭链系统能够应用递归算法的关键见解是，通过将参与局部环约束的刚体分组，将非ST（单树）连通图转换为ST连通图。对刚体进行分组可以使环约束在局部得到解决，即，仅当在正向或反向遍历过程中遇到该组刚体时才解决。这种局部处理方式避免了大规模矩阵分解的需求。算法最初提出时将这种分组称为“约束嵌入”，并将由此产生的刚体组称为“聚合连杆”。为了服务于我们基于传播方法的视角，之前分别使用了“聚类”和“聚类体”这两个术语。由于在后续开发中对其依赖较少，这里我们默认使用原始术语（约束嵌入和聚合连杆）。在将机器人建模为图时，约束嵌入涉及将多个刚体表示为单个节点。具体来说，构成聚合连杆的刚体用一个节点表示。当所有刚体分别被分组到其对应的聚合节点中时，得到的连通图保证是ST连通图，被称为环聚合连通图（LACG）。LACG的通用架构（General Architecture）由聚合连杆C以及与每组刚体相关的树关节和环关节的集合TC组成。按照此过程，约束被嵌入到聚合连杆中，否则将导致与其他组产生环约束。闭链递归算法的一个关键特性是，随着聚合连杆尺寸的增加，其相对于非递归方法的优势会减弱。因此，虽然聚合连杆的选择可能不是唯一的，但总是存在一组最优分组。在基于图论的多体系统动力学深入分析中，Jain推导出了一个最小聚合准则，当满足该准则时，可保证模型达到最优分组。
描述格式的修改
我们首先应对的是扩展URDF（统一机器人描述格式）描述格式以容纳运动学环的挑战。在应对这一挑战时，我们需要注意保持URDF得以广泛应用的特性。具体来说，当前的URDF具有直观性、入门门槛低以及与多种软件接口兼容的特点。为了保持这些特性，确保URDF+（扩展版URDF）做到以下几点：
1）使用与物理上有意义的属性相对应的元素。2）无需了解约束嵌入的知识。3）对现有的URDF进行最小程度的修改。4）保持与URDF的向后兼容性。
URDF+对URDF仅做了三项修改。其中两项是向
元素添加了新的子元素，另一项是向
元素添加了一个可选的新属性。我们修改的核心思想是保留URDF用于描述运动学树的所有元素，但转而用它们来描述生成树。然后使用新的元素来编码环关节并完成连接图，用户无需手动指定聚合链接。从用户的角度来看，聚合是在“幕后”进行的，这一点非常重要，因为要求用户详细了解空间核操作符、约束嵌入或聚类可能会大大提高入门门槛。
Loops
向URDF中添加以使其成为URDF+的第一个元素是
元素。如前所述，环路指的是存在于连接图（CG）中但不存在于生成树（ST）中的边，大多数连接图允许存在多个生成树。因此，用户有责任确定哪些关节应声明为树关节，哪些应声明为环路关节。虽然某些选择比其他选择更自然（例如，将受控关节和观测关节声明为树关节），但URDF+支持所有组合。指定
所需的信息与指定
的信息类似，但所需信息稍多一些。以下模板显示了
元素的全部内容：
对于第k个环路关节，
元素通过name属性给出前驱节点p(k)，通过
子元素给出前驱变换XP(k)。
元素提供相同的信息，但针对的是后继节点s(k)。与
元素类似，type属性和
子元素指定了关节模型。
上述描述的
元素包含了制定与第l个环路相关的隐式约束所需的所有信息。第一步是找到涉及环路约束的树关节。这通过找到前驱和后继之间的最近公共祖先（NCA）来完成。刚体i的“祖先”是生成树中从根刚体到刚体i路径上的所有刚体。我们使用j ⪯ i来表示刚体j是刚体i的祖先，我们强调这与j ≤ i不同。因此，前驱和后继之间的最近公共祖先由以下公式给出：
与第l个环路相关的树关节是在从最近公共祖先（NCA）到前驱以及从NCA到后继的路径上遇到的那些关节。这些关节集合分别是与路径子链
和
中的刚体相关联的树关节。这些路径子链由以下定义确定：
的标准定义中，其第j个块列:
为了准备后续的约束分组操作，将移除所有
的列，这样
就有
行和
列，而不是n列，其中:
Coupling Constraints
关节对连接体之间的相对运动进行运动学约束编码。虽然关节能够捕捉机器人在运动过程中遇到的许多约束，但它们无法捕捉另一种常见的约束类型：耦合约束。耦合约束是对关节状态q之间的关系进行强制，而不是对刚体运动之间的关系进行强制。换句话说，它们将一个关节施加的约束与另一个不同关节施加的约束进行耦合。因此，传统的关节变换、运动子空间和约束力子空间无法描述它们。我们为此创建了一个新的元素
来描述这类约束。目前，我们将可能的耦合约束类限制为树关节位置之间的线性关系。以下模板显示了
元素的全部内容：
对于表示为第k个环路关节的耦合约束，
和
元素再次给出p(k)和s(k)。耦合约束还取决于前驱节点和后继节点的最近公共祖先（NCA）。具体来说，
元素给出了以下两者之间的比例：（i）前驱树关节相对于NCA的位置和（ii）后继树关节相对于NCA的旋转。
在传统URDF中存在的
元素是
的一个特例，其中前驱和后继的最近公共祖先（NCA）也是这两个刚体的父节点。
注意，（8）要求与νp和νs中的刚体相关联的所有树关节具有相同数量的自由度，并编码相同类型的运动（即旋转或平移）。与通常最自然地表示为隐式约束的
约束不同，耦合约束最自然地表示为显式约束，其中显式约束的雅可比矩阵Gk仅是ηk的函数。齿轮传动是耦合约束的最常见示例，尽管在机器人技术中存在许多变体。
独立坐标
如果用户希望以显式形式表示任何环路约束，他们还必须指定一组独立的广义速度坐标y˙。我们注意到，对于闭链系统的递归算法，只需要显式地表示环路约束的雅可比矩阵Gk和偏置gk。它们不依赖于形如q = γ(y)的显式约束定义。因此，即使在环路约束以隐式形式表示的情况下（例如（6）），也可以系统地推导出等效的显式约束雅可比矩阵和偏置。
独立坐标的选择不是唯一的，因此我们对URDF所做的最后一项修改是一个可选属性，允许用户指定哪些树关节应包含在独立坐标中，哪些不应包含。我们将此属性命名为independent，并在此处展示其使用的模板：
PARSER
URDF+所编码的新数据结构需要一个新的解析器与之配套。原始URDF的解析器处理编码节点和边的元素，并生成运动学树。然而，由于URDF+支持闭链，新的解析器必须执行额外的处理来生成环路聚合连接图。这个解析器，特别是它自动化了Jain的约束嵌入策略。
与强连通分量的关系
Jain的约束嵌入策略要求用户“识别包含[环路约束的前驱体和后继体]的最小子树”。这种识别并不简单，特别是在嵌套或重叠环路的情况下，可能需要约束嵌入的背景知识，而我们希望避免这一点。因此，我们提出了一种能够自动识别最小聚合链路的解析器。通过使用有向图中的强连通分量概念来实现这一点，首先，考虑最小聚合标准的物理解释：当且仅当对于聚合链路C中的每一对刚体
