Chapter4: 刚体系统建模
Modelling Rigid Body Systems
刚体系统是由多个零部件组合而成的。这些零部件包括刚体、关节、以及各种力元件。关节负责系统的运动学约束，所以我们用“关节(joint)”来表示一对刚体之间的任何可能的运动关系。 在本章中，我们介绍描述刚体系统的方法。特别的，我们关注完全由刚体和关节构成的系统。这样的描述被称为系统模型(system model)，它描述的是系统本身，而不是某些方面的表现。
我们可以从以下的视角来理解系统模型的作用。假设我们希望在计算机上计算给定刚体系统的动力学，基本上有两种方法可以完成:
- 用各种计算符号形式化的描述给定系统的运动方程，然后将方程转换成计算程序；
- 构建给定系统的系统模型，并将其作为参数提供给：(a)基于模型的动力学计算过程; (b) 基于模型的方程和代码生成器。
我们更喜欢第二种方法，一方面是因为构建和修改一个系统模型，比手动的建立运动方程更简单，另一方面是因为基于模型的计算过程调试方便，可以深度优化，以期被广泛复用。 本书中的动力学算法都是基于模型的，本章将提供建立系统模型的一些套路。方法2(b)将在10.4 节中详细介绍。
 |
| 图 4.1. 两个刚体系统的连接图。 |
4.1 连接关系 Connectivity
一个系统模型必须包含如下的数据：构成系统的部件清单，以及部件之间的连接关系。后者被称为系统的拓扑逻辑(topology)，或者连接关系(connectivity)。 我们可以用连接图的形式描述系统，它具有如下的特性:
- 节点代表刚体。
- 连边代表关节。
- 只有一个节点表示一个固定的刚体，它被称为基座。其它的所有节点都表示移动的刚体。
- 这是一个无向图。
- 图是联通的，即，任何两个节点之间都可以找到一条路径。
图 4.1中描述了一些连接图的例子。第一种是简单的平面机构，称为曲柄-滑块连杆(crank-slider linkage)。 它由三个移动刚体(B1-B3)，一个固定基座，三个旋转关节(J1-J3)和一个滑动关节(J4)构成。刚体B1(曲柄)的旋转导致B3相对于基座前后往复运动。 在这个连接图中，每个节点表示一个刚体，每个连边表示一个关节。通常我们会标注一些节点和弧，以便清晰地反应图与原始机制之间的对应关系。 此外为了看起来方便，我们还会用专门的图标把基座同其它刚体区分开。基座通常用一个空心的点表示。在机械领域中刚体常被称为连杆(links)。
第二个例子是一个在轨道上的卫星。卫星本身有两个刚体和一个关节构成。但是我们不能直接构建它的连接图，因为它不满足规则3，缺少一个表示基座的节点。 因此在这个例子中，我们增加了第三个刚体，把地球当作基座。根据规则5，我们还得再来一个关节，把地球和卫星连起来。因为卫星和地球之间没有物理连接， 所以我们特别定义了一个6自由度的关节来表示它。这个关节不提供任何运动约束，因此对整个系统也没有作用。它的功能就是定义一个关节变量，以便根据其位置和速度写出整个系统的位置和速度变量。 在这个例子中，卫星一共有\(6 + n_s\)个自由度，其中\(n_s\)是卫星上关节的自由度。因此，我们一共需要\(6 + n_s\)个速度变量，这正是两个关节的自由度。
如果一个刚体通过一个6自由度的关节连接到基座上，那么我们称之为浮动基座floating base。在这个例子中，卫星的本体被选为浮动基座。当然，我们也可以选择天线为浮动基座。
 |
| 图 4.2. 生成树、chords、环。 |
4.1.1 运动树和环
如果图中的任意两个节点之间只存在一条路径，则图就是一棵拓扑树(topological tree)。如果刚体系统的连通图是拓扑树，那么我们称之为运动树。 图 4.1中的卫星是运动树，但曲柄-滑块连杆不是。运动树具有一些特殊属性，可以高效地计算其动力学。
记 \(G\) 为一个链接图。我们将其生成树spanning tree记为\(G_t\)，它是一个包含 \(G\) 中所有节点的子图，其连边是 \(G\) 中边的一个子集，使得 \(G_t\) 是一棵拓扑树。 每个连接图都至少有一个生成树。如果\(G\)本身就是一颗树，那么\(G_t = G\)，否则 \(G_t\) 就是 \(G\) 的子图并且不唯一，图 4.2 给出了一些生成树的例子。
一棵由 \(n\) 个节点构成的拓扑树有 \(n - 1\) 条边。因此，如果 \(G\) 包含了 \(n_n\) 个节点和 \(n_a\) 条边，那么 \(G_t\) 将包含\(n_n\) 个节点和 \(n_n - 1\)条边。 这意味着 \(G\) 中有 \(n_a - n_n + 1\) 条边不在 \(G_t\) 中，这些不在树里的边被称为 chords。尽管 \(G\) 中 chords 的数量是固定的，但是不同的 \(G_t\) 其 chords 集合都是不一样的。 这个差异如图 4.2所示，我们用虚线将 chrods 和生成树中的边区分开。
环路是图中的闭合路径。树中是没有环路的，因此图中的每个环路必须至少经历一个 chord。我们对那些恰好经历一个 chord 的环路特别感兴趣。 对于图中的每个 chord 都有一个这样的环路，但是它依赖生成树的选择。例如，图 4.2 中的图形有两个 chords 和三个环路。该图的任意一个生成树都有两个环路只经过了一个 chord， 第三个环路(未画出)则经过了两个 chord。单chord循环one-chord cycle的路径由它所经过的一个 chord 以及由其连接的两个节点之间的生成树中的唯一路径组成。
连接图中的每个环路都定义了一个运动学环路kinematic loop。但是我们对它们并不感兴趣，而只对独立运动环路independent kinematic loops感兴趣。 这是在制定运动方程时必须考虑的环路的最小集。单chord循环的重要性在于它们确定了一组独立运动环路。 如果我们将刚体系统描述为具有 \(n\) 个运动学环路，我们实际是想说该系统有 \(n\) 个独立运动环路。
我们现在考虑一个由基座、\(N_B\) 个移动刚体和 \(N_J\) 个关节构成的刚体系统，在该系统中可能包含了一些 6 自由度的关节，以确保其可以写成一个连接图。 令 \(G\) 为该系统的连接图，\(G_t\) 是 \(G\) 的一棵生成树。这两个图都有 \(N_B + 1\) 个节点，但是 \(G\) 包含了 \(N_J\) 条边，\(G_t\) 只有 \(N_B\) 条边， 所以该系统的 chord 的数量是 \(N_J - N_B\)。记 \(N_L\) 为独立运行环路的数量，它等于 \(G\) 的 chord 的数量，因此有公式: $$ \begin{equation}\label{equa_4_1} N_L = N_J - N_B \end{equation} $$ 将该式应用于图 4.1中的曲柄-滑块连杆机构，可知它有一个运动学环路。
如果一个系统有运动学环路，那么我们称之为闭环系统closed-loop system，否则称为运动学树。与 chords 对应的关节称为闭环关节loop joints, or loop-closing joints， 那些与生成树对应的关节则称为树关节tree joints。因此一个刚体系统有 \(N_B\) 个树关节和 \(N_L\) 个闭环关节。
 |
| 图 4.3. 无分支运动树的唯一编码 |
4.1.2 编码规则(Numbering)
我们有很多种方法来区分不同的刚体和关节，但是最方便的还是给他们编上号。我们应当选择一种编码策略，能够方便的在动力学算法中使用。 根据这个原则，刚体从 0 编码到 \(N_B\)，关节则从 1 到\(N_J\)编码，编码的过程还要遵循如下的规则:
 |
| 图 4.4. 图 4.1中的曲柄-滑块连杆机构的所有可能编码 |
- 选择一个生成树, \(G_t\)
- 给基座赋予编码 0，并将之设置为 \(G_t\) 的根节点
- 对剩余的节点从1到\(N_B\)排序，保证子节点的编码一定大于其父节点
- 对\(G_t\)中的边从1到\(N_B\)排序，保证第 \(i\) 条边连接了节点 \(i\) 和它的父节点
- 剩下的边可以从\(N_B + 1\)到\(N_J\)随意编号
- 每个刚体的编码与其对应的节点一致，关节与边一致。
这种编码方法被称为正则编码regular numbering。如果这些规则应用于一个无分支运动树unbranched kinematic tree，该树上没有一个节点有超过一个子节点， 那么刚体和关节的编码就从基座开始递增到叶子节点上，如图 4.3所示。对于其他的情况，编码就不唯一。比如图 4.4中， 画出了图 4.1中的曲柄-滑块连杆机构的所有可能编码。有两棵树是无分支的，因此它们只有一种编码。这两棵树分别在图中的左上角和右下角。 另外两棵树在根节点上就分叉了，所以它们各有三种编码方式。
4.1.3 关节极性(Joint Polarity)
每个关节都连接着两个刚体，我们把其中一个称为关节的前驱predecessor，另一个称为关节的后继successor，关节本身则被称为从前驱到后继的连接。 这种表述有两层含义，其一是定义了关节与刚体变量之间的关系，其二声明了系统中关节连接作用的传导关系(which way round the joint)。比如说，一个关节的速度就被定义为后继相对于前驱的相对速度， 而关节传递的力则被定义为从前驱作用传导到后继上的力。
我们把关节连接作用在两个刚体之间的传导关系称为它的极性。我们可以在连接图中，给边添加一个从前驱指向后继的箭头，来表示这种极性。 如下图 4.5和图 4.6所示，我们对树关节的极性约定，树关节的极性是从父节连接到子节from parent to child。 在没有明确说明的情况下，我们认为所有树关节都遵循这个约定。
 |
| 图 4.5. 关节前驱和后继阵列。 |
 |
| 图 4.6. 前驱、后继、父节点阵列。 |
一些关节在物理上是对称的，这意味着后继相对于前驱的运动，定性的qualitatively，与前驱相对于后继的运动相同。 不具备这一特征的关节，均被称为非对称的。交换一个对称关节的极性并不会给刚体系统带来本质上的差异，非对称关节则不然。 比如，两个刚体通过球窝关节ball-and-socket连接，这是一种对称关节。那么谁用球(ball)，谁用窝(socket)并不重要， 如果我们将交换它们的身份，其表述的运动仍然相同。不同的是，如果两个刚体用齿轮齿条rack-and-pinion连接，它不对称， 那么谁是齿轮，谁是齿条就很重要。因此非对称关节的极性有物理系统确定，而对称关节的极性则不然，我们可以在系统模型中自行选择。
4.1.4 形式化表述(Representation)
在计算机中有很多种方式描述连接图。最简单的一种方式就是，用两个数组\(p\)和\(s\)分别记录各个关节中前驱和后继刚体的编号。对于关节 \(i\)， \(p(i)\) 和 \(s(i)\) 分别表示它的前驱和后继。如图 4.5所示，选取了图 4.4中的两个连接图，并给每个关节添加了表示极性的箭头。
数组 \(p\) 和 \(s\) 一起提供了一个连接图的完整描述，我们可以根据它们计算出其他一些有用的量。比如说，父列表parent array, \(\lambda\)， 描述了在生成树中每个刚体的父节点。对于刚体 \(i\) 我们有 \(\lambda(i)\) 表示其父节点。根据编码规则 3 和 4，树关节 \(i\) 在刚体 \(i\) 和 \(\lambda(i)\) 中建立连接， 并且有 \(\lambda(i) < i\)，所以有: $$ \begin{equation}\label{equa_4_2} \lambda(i) = \text{min}\left(p(i), s(i)\right), \qquad (1 ≤ i ≤ N_B) \end{equation} $$ 图 4.5中的两个图的父列表分别是 \(\lambda = [0, 1, 2]\) 和 \(\lambda = [0, 0, 1]\)，图 4.6中给出了一个更复杂的例子。
如果一个树关节满足\(s(i) = i\)，那么它从父节点连接到了子节点，称它在树中是正向连接的。否则，从子节点连接到父节点，则称之为逆向连接。 图 4.5中的所有树关节都是正向连接的，箭头都是从根节点出发向外发散的。图 4.6中除了关节 4 之外所有的树关节也都是正向的。 在图4.6中关节的编号和正向箭头都被省略了。之所以省略树关节的编号，是因为它可以根据编码规则推导出来，而正向(forward)是树关节方向的标准方向。
根据运动树的连接关系，我们还可以挖掘出三种有用的信息。对于除了基座之外的任何刚体\(i\), \(k(i)\)表示刚体\(i\)和基座之间路径上的所有树关节的集合， \(\mu(i)\)是刚体\(i\)的子节点的集合，\(v(i)\)是以刚体\(i\)为根的子树中所有刚体的集合。它们分别被称为支撑(support), 子节点(child)和子树(subtree)集合。 如果一个树关节在刚体和基座之间的路径上，我们就说它支撑着这个刚体。\(v(i)\)是关节\(i\)支撑的所有刚体的集合。对于图 4.6中的生成树，其支撑、子节点、子树集合为: $$ \begin{array}{l|l|l} k(1) = {1} & \mu(0) = {1, 4} & v(0) = {0, 1, 2, \cdots, 8} \\ k(2) = {1,2} & \mu(1) = {2, 3} & v(1) = {1, 2, 3, 5} \\ k(3) = {1,3} & \mu(2) = {} & v(2) = {2} \\ \vdots & \vdots & \vdots \\ k(8) = {4,8} & \mu(8) = {} & v(8) = {8} \end{array} $$ \(k, \lambda, \mu, v\)有一些很显然的简单的特性。比如，\(\mu(i) \subseteq v(i), j \in k(i) \Rightarrow i \in v(j)\) 等。有一个不那么显然，但是很有用的特性: $$ \begin{equation}\label{equa_4_3} \sum_{i=1}^{N_B}\sum_{j\in k(i)} (\cdots) = \sum_{j=1}^{N_B}\sum_{i \in v(j)}(\cdots) \end{equation} $$ 上式中左侧遍历支撑刚体\(i\)的所有关节\(j\)的和，右侧遍历关节\(j\)支撑的所有刚体\(i\)的和。它们是相等的。The left-hand side is a summation over all \(i, j\) pairs in which joint \(j\) supports body \(i\), but the right-hand side is also a summation over all \(i, j\) pairs in which joint \(j\) supports body \(i\), and so the two are the same.
 |
Example 4.1: 我们在这个简单的例子中介绍 \(\lambda\) 在动力学算法中的作用。给定树关节的速度，我们要计算刚体系统中每个刚体的速度。 令 \(\boldsymbol{v}_i\) 表示刚体 \(i\) 的速度。\(\boldsymbol{v}_{Ji}\) 表示关节 \(i\) 传导的速度，即子节点相对于父节点的速度。 现在，我们可以以父节点的速度和关节的速度之和，写出刚体\(i\)的速度的递推表达式: $$ \boldsymbol{v}_i = \boldsymbol{v}_{\lambda(i)} + \boldsymbol{v}_{Ji} \qquad (\boldsymbol{v}_0 = \boldsymbol{0}) $$ 因此计算各个刚体速度的伪代码如右侧所示。由于 \(\lambda(i) < i\)，所以在计算 \(\boldsymbol{v}_i\) 之前总是已经计算了 \(\boldsymbol{v}_{\lambda(i)}\)。 所以刚体速度的表达式可以改写为: $$ \begin{equation}\label{equa_4_4} \boldsymbol{v}_i = \sum_{j \in k(i)} \boldsymbol{v}_{Jj} \end{equation} $$
Example 4.2:Example 2.3引入了刚体雅可比矩阵， 它将各个刚体的空间速度写成了关节速度变量的函数。如果刚体\(i\)的雅可比矩阵为 \(\boldsymbol{J}_i\)，那么有 $$ \boldsymbol{v}_i = \boldsymbol{J}_i \dot{\boldsymbol{q}} $$ 式(2.19)已经给出了计算不平衡运动链的 \(\boldsymbol{J}_i\) 的公式。 现在我们来推导一般形式。如果关节\(j\)的速度为 \(\boldsymbol{S}_j\dot{\boldsymbol{q}}_j\)，定义 \(\epsilon_{ij}\)如下: $$ \begin{equation}\label{equa_4_5} \epsilon_{ij} = \begin{cases} 1 & \text{if } j \in k(i) \\ 0 & \text{otherwise} \end{cases} \end{equation} $$ 所以，如果关节\(j\)支撑刚体\(i\)，那么\(\epsilon_{ij}\)就是一个非零值。刚体\(i\)的速度就可以写成如下的形式: $$ \boldsymbol{v}_i = \sum_{j \in k(i)} \boldsymbol{S}_j \dot{\boldsymbol{q}}_j = \sum_{j=1}^{N_B} \epsilon_{ij} \boldsymbol{S}_j \dot{\boldsymbol{q}}_j = \begin{bmatrix} \epsilon_{i1} \boldsymbol{S}_1 & \cdots & \epsilon_{iN_B}\boldsymbol{S}_{N_B} \end{bmatrix} \begin{bmatrix} \dot{\boldsymbol{q}}_1 \\ \vdots \\ \dot{\boldsymbol{q}}_{N_B} \end{bmatrix} $$ 因此刚体雅可比的一般表达式为: $$ \begin{equation}\label{equa_4_6} \boldsymbol{J}_i = \begin{bmatrix} \epsilon_{i1} \boldsymbol{S}_1 & \epsilon_{i2}\boldsymbol{S}_2 & \cdots & \epsilon_{iN_B}\boldsymbol{S}_{N_B} \end{bmatrix} \end{equation} $$
4.2 几何 Geometry
 |
| 图 4.7. 一个刚体系统的几何模型。 |
如果两个物体通过关节连接，那么它们的相对运动就会受到限制。要完整的描述约束需要了解两件事：关节允许的运动以及关节相对于各个刚体的位置。 前者由关节模型描述，后者由系统的几何形状描述。
一个刚体系统的几何模型是用来描述关节相对于各个刚体的位置的。 如图 4.7中所示的一个刚体系统对的几何模型，这个刚体系统有一个固定的基座\(B_0\)，和三个移动的刚体记为\(B_1, B_2, B_3\)，三个树关节\(J_1, J_2, J_3\)， 和一个闭环关节\(J_4\)。图中的虚线只表示各个关节的连接关系，并不是实际的物理位置。
每个移动的刚体都有一个与之一起运动的局部坐标系，分别被标记为\(F_1, F_2, F_3\)，我们称这个坐标系为刚体坐标系body coordinates, or link coordinates。 还有一个与基座固连在一起的坐标系 \(F_0\)，为整个刚体系统确定了一个绝对的参考系。刚体 \(B_i\) 的位置由\(F_i\)相对于\(F_0\)的位置定义，可以表示成坐标变换矩阵\({}^i\boldsymbol{X}_0\)。
图中还画了几个坐标系命名为 \(F_{i,j}\)，其中\(i\)表示刚体\(j\)表示关节。这些坐标系定义了关节在刚体中的位置。每个树关节\(i\)都是从坐标系\(F_{\lambda(i), i}\) 连接到坐标系 \(F_i\)， 如果关节是逆向的则反过来，而每个闭环关节\(i\)都是从\(F_{p(i),i}\)到\(F_{s(i), i}\)的连接。因此我们一共需要确定\(N_B + 2N_L\)个坐标系的位置， 每个树关节都有一个，闭环关节有两个。坐标系\(F_{i,j}\)相对于\(F_i\)的位置由变换矩阵\({}^{i,j}\boldsymbol{X}_i\)给出，这个矩阵是一个常量。 这些变换，或者说是通过它们推导出的数据，定义了一个刚体系统的几何模型。
 |
为了方便，我们定义了一个数组来表示一棵树的变换 \(\boldsymbol{X}_T\)，其中 \(\boldsymbol{X}_T(i) = {}^{\lambda(i), i}\boldsymbol{X}_{\lambda(i)}\)。它们是树关节的位置变换， 完整描述了一棵生成树的几何模型。此外，对于从1到\(N_L\)的闭环关节，我们还定义数组 \(\boldsymbol{X}_P\) 和 \(\boldsymbol{X}_S\)，满足关系， \(\boldsymbol{X}_P(i - N_B) = {}^{p(i), i}\boldsymbol{X}_{p(i)}\) 和 \(\boldsymbol{X}_S(i - N_B) = {}^{s(i), i}\boldsymbol{X}_{s(i)}\)。它们描述的是闭环关节的位置变换。
最后，每个关节都定义了一个关节变换\(\boldsymbol{X}_{Ji}\)，它是关节位置变量的函数。我们将在4.4 节中讨论这些变换。 如图中所示，如果 \(i\) 是一个树关节，\(\boldsymbol{X}_{Ji}\) 就是 \({}^i\boldsymbol{X}_{\lambda(i),i}\)，如果是闭环关节，则是\({}^{s(i),i}\boldsymbol{X}_{p(i),i}\)。
Example 4.3: 我们要计算一个运动树上每个刚体的位置，即，计算每个刚体的 \({}^i\boldsymbol{X}_0\)。其算法如右图所示。
在这个代码片段中，\(\boldsymbol{X}_J\)是一个局部变量，jtype(i) 是一个描述关节 \(i\) 的数据结构，
函数jcalc用于根据关节描述和关节位置向量\(\boldsymbol{q}_i\)计算关节变换。jtype(i) 和 jcalc 将在4.4 节中详细介绍。
 |
| 图 4.8. Denavit-Hartenberg 坐标系和参数。 |
4.3 D-H 坐标 (参见历史文章)
一般情况下，我们需要六个参数才能描述一个坐标系相对另一个的位姿。但是对于一些特定的刚体系统，我们有可能用较少的参数来定位刚体坐标系。 Danavit 和 Hartenberg 就提供了一种方法，每个关节只需要四个参数。它们利用了一个特性，一些常见的关节类型可以用空间中的一条线来表征，定位一条线只需要四个参数。
如右侧的图 4.8所示，将 Denavit-Hartenberg(DH) 方法应用到了一个无分支的运动树表示的机械臂或者类似的设备上。 这个系统有一些重要的几何特征:
- 一个表示基座的坐标系
- \(n\) 个关节轴
- 一个末端(end-effector)坐标系，与最后的连杆固连在一起。
- 轴 \(z_0\) 与 \(z_{n+1}\) 分别表示基座和末端的坐标系的 \(z\) 轴。
- 对于 \(n\) 个关节，我们用轴 \(z_1\) 到 \(z_n\) 表示。其中 \(z_i\) 是第\(i\)个关节的轴线。如果关节 \(i\) 是平动的(prismatic)， 那么\(z_i\) 可以是任何方向的轴，但与 \(z_{i-1}\) 相交的轴会是比较明智的选择。
- 轴 \(x_i\) 通常与 \(z_i\) 和 \(z_{i+1}\) 垂直，并从\(z_i\)指向\(z_{i+1}\)。如果 \(z_i\) 与 \(z_{i+1}\) 相交，那么 \(x_i\) 的方向就存在一个 \(180°\) 的歧义。 如果 \(z_i\) 和 \(z_{i+1}\) 平行，那么这两个轴的公垂线的位置就有无限种可能，通常我们会选择与 \(x_{i-1}\)相交的那个。
- 刚体坐标系\(F_i\)由轴 \(x_i, y_i, z_i\) 定义，其中 \(y_i\) 由 \(x_i\) 和 \(z_i\) 根据右手坐标系规则确定。
- \(d_i\)：沿着 \(z_i\) 轴，\(x_{i-1}\) 与 \(x_i\) 之间的距离。
- \(\theta_i\)：\(x_{i-1}\) 轴到 \(x_i\) 轴需要绕 \(z_i\) 轴转过的角度。
- \(a_i\)：沿着 \(x_{i-1}\) 轴，\(z_{i-1}\) 轴到 \(z_i\) 轴之间的距离。\(x_{i-1}\) 的定义意味着 \(a_i ≥ 0\)。
- \(\alpha_i\)：\(z_{i-1}\) 轴到 \(z_i\) 轴需要绕 \(x_{i-1}\) 轴转过的角度。
就其基本形式而言，Denavit-Hartenberg 方法适用的刚体系统，其每个关节都可以通过关节轴的形式表示，并且其拓扑结构都是无分支运动树或单个闭合运动回路。 然而，该方法可以适用于更广泛的系统类别，如Khalil和Dombre(2002)所述。DH参数在各种教科书中都有描述，如Angeles(2003)和Craig(2005)。 符号细节往往因作者而异，尤其是 \(x_i, a_i, \alpha_i\) 的编号。这里采用 Springer Handbook of Robotics(Siciliano and Khatib，2008) 的符号系统。
表 4.1 关节模型公式.
Joint Model Formulae
 |
4.4 关节模型
关节可以看做是两个笛卡尔坐标之间的运动约束，这两个笛卡尔坐标一个与关节的后继刚体绑定，一个绑定到前驱上，分别记为 \(F_s\) 和 \(F_p\)。 关节模型提供了一些必要的信息，使得我们可以将 \(F_s\) 相对于 \(F_p\) 的运动描述成关于关节变量的函数。
根据类型的不同，一个关节允许的运动方式也不一样。比如说，旋转关节允许绕着一个轴做纯转动，而球关节则匀速绕着一个点任意转动。因此我们需要为每种关节建立一个关节模型。 这个模型实际上就是一对数据和公式，用于计算特定的量，比如，关节变换矩阵\({}^s\boldsymbol{X}_p\)、关节速度\(\boldsymbol{v}_J\)、关节的运动子空间\(\boldsymbol{S}\)等等。 表 4.1 中列举了一些常见关节的公式，图 4.9中示例了一些关节的几何解释。
 |
| 图 4.9. 一些关节位置变量的几何解释。 |
现在我们来详细解释一种模型。表 4.1的第一行描述了旋转关节，它允许 \(F_s\)相对 \(F_p\) 绕着公共的 \(z\) 轴转动，其关节变量的几何含义就是转过的角度。 表中的 \(\boldsymbol{E}\) 和 \(\boldsymbol{r}\) 列分别描述了关节变换的转动和平移的分量，而关节的变换矩阵为 \({}^s\boldsymbol{X}_p = \text{rot}(\boldsymbol{E})\text{xlt}(\boldsymbol{r})\)。 关于\(\text{rot}, \text{xlt}, \text{rz}\)的定义参见表2.2。如果关节变量\(q_1\)碰巧是零， 那么\(F_s\)与\(F_p\)一致。我们将尽可能的将这种约定应用于所有的关节模型。还有一个应用于所有关节模型的约定是 \(\boldsymbol{S}, \boldsymbol{T}\)及其相关量， 它们都以\(F_s\)坐标系表示。对于旋转关节来书这一点无关紧要，因为\({}^s\boldsymbol{S} = {}^p\boldsymbol{S}\)，但是对于其他类型的关节则不然。
一个设计良好的动力学模拟器，应该有个类似于关节模型库joint model library的模块，其中包含了软件所支持的所有类型的关节的模型。
我们应该用表达式 jtype(i) 来获取关节 \(i\) 的关节类型描述符。实现关节类型描述符的最简单方法，就是给库中的每个模型一个类型码。
比如说，类型码'R' 用于表示旋转关节，'P' 表示平动关节。但是有些关节是由类型和参数集合一起描述的。比如螺旋关节，几何特征就是螺旋的，同时还需要指定螺距的参数。
刚体中的每一个螺旋关节都可以由不同的螺距。所以这样的参数是各个关节的属性，它并不是关节模型的属性。为了记录关节参数，我们应该将关节描述符定义成一个包含类型代码和关节参数集合的数据结构。
关节模型计算套路(Joint Model Calculation Routine)
本书后续的章节中介绍的几种动力学算法都是建立在关节模型数据上的。为了简化算法描述，我们假设存在一个函数，jcalc，可以在一次调用之后返回所有请求的关节相关的数据。
下面是调用该函数的一些例子:
$$
\begin{array}{ll}
\left[\boldsymbol{X}_J, \boldsymbol{S}, \boldsymbol{v}_J, \boldsymbol{c}_J\right] = \text{jcalc}(type, \boldsymbol{q}, \boldsymbol{\alpha}) & \text{(explicit joint constraint)} \\
\left[\boldsymbol{\delta}, \boldsymbol{T}\right] = \text{jcalc}(type, {}^s\boldsymbol{X}_p) & \text{(implicit joint constraint)} \\
\left[\dot{\boldsymbol{q}}\right] = \text{jcalc}(type, \boldsymbol{q}, \boldsymbol{\alpha}) & \text{(for qdfn (Eq. 3.7))}
\end{array}
$$
表达式的左侧的列表是调用函数的返回值，请求参数 type 表示关节类型描述符，jtype(i)。
我们将在第8章中介绍，使用 jcalc 计算隐式关节约束，并在本章的后续内容中介绍 \(\dot{\boldsymbol{q}}\) 的计算方法。
在算法列表中，我们通常假设 \(\boldsymbol{\alpha} = \dot{\boldsymbol{q}}\)，并在参数列表中使用 \(\dot{\boldsymbol{q}}\) 代替 \(\boldsymbol{\alpha}\)。
若关节显式的与时间关联，则必须将时间添加到列表中的最后一个参数。请注意，\(\boldsymbol{v}_J, \boldsymbol{c}_J, \boldsymbol{S}, \boldsymbol{T}\) 都是坐标系\(F_s\)下的变量。
\(\boldsymbol{v}_J\) 和 \(\boldsymbol{c}_J\) 的表达式已经在第 3.5 节中给出了。
 |
Example 4.4:例 4.3 显示了如何计算运动树中每个物体的坐标变换\({}^i\boldsymbol{X}_0\)。 现在我们扩展它同时计算\({}^i\boldsymbol{X}_0\) 和 \({}^0\boldsymbol{v}_i\)。\({}^0\boldsymbol{v}_i\)是基坐标系下表示的刚体\(i\) 的速度。 该算法的伪代码如右图所示。
该算法中有一个新特性。它先计算 \({}^i\boldsymbol{X}_0\)，然后使用 \({}^i\boldsymbol{X}_0\) 来对 \(\boldsymbol{v}_J\) 进行坐标变换。 详细的解释可以在附录 A 中找到，其中显示了如何直接通过 \(\boldsymbol{X}\) 和 \(\boldsymbol{v}\) 计算 \(\boldsymbol{X}^{-1}\boldsymbol{v}\)， 不需要先计算\(\boldsymbol{X}^{-1}\)。我们可以直接通过 \(\boldsymbol{X}\) 和 \(\boldsymbol{f}\) 计算 \(\boldsymbol{X}^*\boldsymbol{f}\) 和 \((\boldsymbol{X}^*)^{-1}\boldsymbol{f}\)，而不必先算\(\boldsymbol{X}^*\) 或 \((\boldsymbol{X}^*)^{-1}\)。 我们在几个动态算法中应用到了这一点。
Example 4.5:表 4.1 中列举的平面关节和6自由度关节有一个特殊的地方，就是他的平移变量是在\(F_s\)坐标系下的变换， 而不是\(F_p\)坐标系下的。这样做是为了，让其运动子空间变成一个简单的常数，使得 \(\mathring{\boldsymbol{S}} = \boldsymbol{0}, \boldsymbol{c}_J = \boldsymbol{0}\)， 并且\(\boldsymbol{S}\)的这种简单形式使得很多优化方法可以应用到动力学算法中。但是，这也存在一个很大的缺点。如果后继刚体距离前驱刚体很远，并且开始旋转， 那么旋转将产生很大的位置变量变化率。(想象一下翻滚的卫星或飞机。)想要兼顾两者，可以在\(F_p\)坐标系下描述位置，在\(F_s\)坐标系下描述速度。
令\({}^pq_1, {}^pq_2, {}^pq_3\)为平面关节的一组位置变量，其中 \({}^pq_2, {}^pq_3\) 是\(F_p\)坐标系下关于\(x,y\)方向的变换。
\({}^sq_1, {}^sq_2, {}^sq_3\)是该关节的另一组位置变量，其中 \({}^sq_2, {}^sq_3\) 是\(F_s\)坐标系下关于\(x,y\)方向的变换。
如图 4.9所示，这两组变量应用表 4.1中的模型，有变换关系:
$$
\begin{bmatrix}
{}^pq_1 \\ {}^pq_2 \\ {}^pq_3
\end{bmatrix} = \begin{bmatrix}
1 & 0 & 0 \\
0 & c_1 & -s_1 \\
0 & s_1 & c_1
\end{bmatrix} \begin{bmatrix}
{}^sq_1 \\ {}^sq_2 \\ {}^sq_3
\end{bmatrix}
$$
我们想用\({}^pq_i\)表示位置变量，\({}^s\dot{q}_i\)表示速度变量。我们需要一个公式来根据位置和速度变量计算位置变量的导数。这样的公式是通过对上面的方程求导得到的:
$$
\begin{bmatrix}
\dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3
\end{bmatrix} = \begin{bmatrix}
1 & 0 & 0 \\
-q_3 & c_1 & -s_1 \\
q_2 & s_1 & c_1
\end{bmatrix} \begin{bmatrix}
\alpha_1 \\ \alpha_2 \\ \alpha_3
\end{bmatrix}
$$
上式中，我们去掉了\({}^pq_i, {}^p\dot{q}_i\)的上角标\(p\)，并用\(\alpha_i\)替代了\({}^s\dot{q}_i\)。
对于该平面关节的模型，如果需要从 \(\boldsymbol{q}\) 和 \(\alpha\) 计算 \(\dot{\boldsymbol{q}}\)，那么这些都是 jcalc 所必须完成的计算任务。
 |
| 图 4.10. 齿轮齿条关节。 |
Example 4.6:让我们构建一个如图 4.10 所示的齿条齿轮关节rack-and-pinion joint的关节模型。该关节允许作为后继刚体中的小齿轮沿着作为前身刚体中的齿条滚动。 其中，齿条平行于\(F_p\)坐标系的\(x\)轴，坐标系\(F_s\) 的 \(z\) 轴穿过齿轮的中心。齿轮的半径为 \(r\)，它是关节模型的一个参数。该关节仅允许一个运动自由度， 因此只有一个关节变量，记为\(q_1\)，表示齿轮的旋转角度。
假设 \(q_1 = 0\) 时，坐标系 \(F_p\) 与 \(F_s\) 重合，那么从 \(F_p\) 到 \(F_s\) 的变换关系有，沿着 \(x\) 轴的方向平移 \(rq_1\)，以及绕着\(z\)轴转动了\(q_1\)两个方面。 因此关节的变换关系为: $$ \boldsymbol{X}_J = {}^s\boldsymbol{X}_p = \begin{bmatrix} \boldsymbol{E} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{E} \end{bmatrix} \begin{bmatrix} \boldsymbol{1} & \boldsymbol{0} \\ -\boldsymbol{p}\times & \boldsymbol{1} \end{bmatrix} $$ 其中 $$ \boldsymbol{E} = \text{rz}(q_1) = \begin{bmatrix} c_1 & s_1 & 0 \\ -s_1 & c_1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \qquad \text{and} \qquad \boldsymbol{p} = \begin{bmatrix} rq_1 \\ 0 \\ 0 \end{bmatrix} $$ 接下来计算\(\boldsymbol{v}_J\)和\(\boldsymbol{S}\)。我们把关节的速度写成关节的位置和速度变量的函数We begin by expressing the joint velocity as a function of the joint’s position and velocity variables. 相对于\(F_p\)，齿轮以角速度\(\boldsymbol{\omega} = \begin{bmatrix} 0 & 0 & \dot{q}_1 \end{bmatrix}^T\)， 绕着一个通过 \(\boldsymbol{p}\) 的轴转动，并且以 \(\dot{\boldsymbol{p}} = \begin{bmatrix} r\dot{q}_1 & 0 & 0 \end{bmatrix}^T\) 的线速度平动。 这相当于在\(F_p\)坐标系下表示的空间速度: $$ {}^p\boldsymbol{v}_J = \begin{bmatrix} \boldsymbol{\omega} \\ \boldsymbol{p} \times \boldsymbol{\omega} \end{bmatrix} + \begin{bmatrix} \boldsymbol{0} \\ \dot{\boldsymbol{p}} \end{bmatrix} = \begin{bmatrix} \begin{bmatrix} 0 \\ 0 \\ \dot{q}_1 \end{bmatrix} \\ \begin{bmatrix} rq_1 \\ 0 \\ 0 \end{bmatrix} \times \begin{bmatrix} 0 \\ 0 \\ \dot{q}_1 \end{bmatrix} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 0 \\ r\dot{q}_1 \\ 0 \\ 0 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ \dot{q}_1 \\ r\dot{q}_1 \\ -rq_1\dot{q}_1 \\ 0 \end{bmatrix} $$ 因为只有一个关节变量，其运动子空间矩阵可以由公式 \(\boldsymbol{S} = \boldsymbol{v}_J / \dot{q}_1\) 给出。因此有: $$ \boldsymbol{v}_J = {}^s\boldsymbol{X}_p {}^p\boldsymbol{v} = \begin{bmatrix} 0 \\ 0 \\ \dot{q}_1 \\ r c_1 \dot{q}_1 \\ - r s_1 \dot{q}_1 \\ 0 \end{bmatrix} \qquad \text{and} \qquad \boldsymbol{S} = \frac{\boldsymbol{v}_J}{\dot{q}_1} = \begin{bmatrix} 0 \\ 0 \\ 1 \\ r c_1 \\ -r s_1 \\ 0 \end{bmatrix} $$ 最后，因为\(\boldsymbol{S}\)不是一个常数，我们还需要计算 \(\boldsymbol{c}_J\) 的公式: $$ \boldsymbol{c}_J = \mathring{\boldsymbol{S}}\dot{q}_1 = \frac{\partial \boldsymbol{S}}{\partial q_1} \dot{q}_1^2 = \begin{bmatrix} 0 \\ 0 \\ 0 \\ -r s_1 \dot{q}_1^2 \\ -r c_1 \dot{q}_1^2 \\ 0 \end{bmatrix} $$ 关于\(\mathring{\boldsymbol{S}}\)参见式 3.42。 如果 \(r = 0\)，那么齿轮齿条关键就退化成一个旋转关节了。
表 4.2 jcalc模拟反极性关节
How jcalc simulates an opposite-polarity joint
 |
极性翻转(Polarity Reversal)
正如4.1.4 节中介绍的那样，如果运动树中的一个关节，其前驱是后继的父节点，那么这个关节就是正向的，否则就是逆向的。 如果关节是对称的，则可以在定义系统模型时选择极性，使其处于正向。但如果它是不对称的，那么其极性由所建模的刚体系统决定。因此，不可能保证每个树节点都是前向的。
处理逆向关键的最简单方法是将其转换成前向的关节。我们可以按照如下的步骤实现。首先，在描述关节的数据结构中添加一个布尔变量，如果关机是逆向的就赋值 true，
否则为 false。根据这一额外信息，只要该布尔变量为 true，我们就可以修改jcalc来模拟相反极性的关节。
实际上，jcalc 只是交换了前驱和后继刚体的角色。它仅需要在返回某些计算值之前对其修改即可。比如说，它不返回给定关节类型的公式计算出的矩阵\({}^s\boldsymbol{X}_p\)，
而是返回这个矩阵的逆；计算\(\boldsymbol{v}_J\)的时候实际返回的是\(-{}^p\boldsymbol{X}_s\boldsymbol{v}_J\)。
表 4.2中列举了需要修改返回值的所有变量。这种方法的优点是，它允许动力学算法假设所有树节点都处于正向的。这可以简化算法，让其更容易实现。
4.5 球面运动
我们有很多方法来表示一般的 3D 旋转(例如，Rooney，1977)，但主要还是选择欧拉角Euler angles和欧拉参数Euler parameters。 欧拉参数也称为单位四元数union quaternion。欧拉角的优点是位置变量是一组三个独立的角度，并且速度变量是它们的导数。这个实现起来相对比较容易。 其缺点是存在奇点，越接近奇点，越病态(ill-conditioned)，最终完全崩溃。单位四元数的主要优点是它不存在奇点。但其缺点是它的四个位置变量不是相互独立的， 这意味着速度变量不能是位置变量的导数。使用单位四元数的软件，必须允许位置变量的数量与速度变量的数量不同。
欧拉角(Euler Angles)
术语“欧拉角”有广义和狭义两个层面。从广义上讲它是指，通过绕指定坐标轴的三个旋转序列来表示一般旋转的任何方法。狭义上是指，第一次和第三次绕z轴旋转， 第二次绕x轴或y轴旋转的特殊情况。
每个版本的欧拉角都存在奇点。只要第一轴和第三轴重合，奇点就会出现。如果第一次和第三次旋转使用相同的坐标轴，那么当第二个角度为 0° 或 180° 时会出现奇点。 如果三次的坐标轴都不相同， 那么当第二个角度为 ±90° 时，就会出现奇点。在奇点附近，所表示的旋转的微小变化都可以引起第一角度和第三角度的大变化。 基于欧拉角的关节模型在奇点处无效。
现在我们用航向角(yaw), 俯仰角(pitch)和滚转角(roll)构建一个球关节模型。它们是先后绕着\(z,y,x\)轴的转动序列。因此，\(F_s\)相对于\(F_p\)的正确位姿是通过如下的转动得到的， 首先，放置\(F_s\)使其与\(F_p\)重合，然后绕着它的\(z\)轴转动角度 \(q_1\) (yaw)，接着绕其\(y\)轴转动角度 \(q_2\) (pitch),最后绕其\(x\)轴转动\(q_3\) (roll)。 最后矩阵 \(\boldsymbol{E}\) 可以写作: $$ \begin{equation}\label{equa_4_7} \boldsymbol{E} = \text{rx}(q_3) \text{ry}(q_2) \text{rz}(q_1) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & c_3 & s_3 \\ 0 & -s_3 & c_3 \end{bmatrix} \begin{bmatrix} c_2 & 0 & -s_2 \\ 0 & 1 & 0 \\ s_2 & 0 & c_2 \end{bmatrix} \begin{bmatrix} c_1 & s_1 & 0 \\ -s_1 & c_1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation} = \begin{bmatrix} c_1 c_2 & s_1 c_2 & -s_2 \\ c_1 s_2 s_3 - s_1 c_3 & s_1 s_2 s_3 + c_1 c_3 & c_2 s_3 \\ c_1 s_2 c_3 + s_1 s_3 & s_1 s_2 c_3 - c_1 s_3 & c_2 c_3 \end{bmatrix} $$ 子空间 \(\boldsymbol{S}\) 的公式可以通过把关节速度写成 \(\dot{\boldsymbol{q}}\) 的函数的形式获得，即 \(\boldsymbol{S} = \partial \boldsymbol{v}_J / \partial \dot{\boldsymbol{q}}\)。 略去推导过程，我们直接给出最终结果: $$ \begin{equation}\label{equa_4_8} \boldsymbol{S} = \begin{bmatrix} -s_2 & 0 & 1 \\ c_2 s_3 & c_3 & 0 \\ c_2 c_3 & -s_3 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \end{equation} $$ \(\boldsymbol{S}\) 的列包含了转轴在\(F_s\)中的坐标，第一列是\(F_p\)的\(z\)轴，第二列是第一次转动之后的\(y\)轴，第三列则是两次转动之后的\(x\)轴。因为\(\boldsymbol{S}\)不是一个常量， 我们还要写出\(\boldsymbol{c}_J\)的表达式: $$ \begin{equation}\label{equa_4_9} \begin{array}{rcl} \boldsymbol{c_J} = \mathring{\boldsymbol{S}} \dot{\boldsymbol{q}} & = & \begin{bmatrix} -c_2 \dot{q}_2 & 0 & 0 \\ -s_2 s_3 \dot{q}_2 + c_2 c_3 \dot{q}_3 & -s_3 \dot{q}_3 & 0 \\ -s_2 c_3 \dot{q}_2 - c_2 s_3 \dot{q}_3 & -c_3 \dot{q}_3 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}\begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{bmatrix} \\ & = & \begin{bmatrix} -c_2 \dot{q}_1 \dot{q}_2 \\ -s_2 s_3 \dot{q}_1 \dot{q}_2 + c_2 c_3 \dot{q}_1 \dot{q}_3 - s_3 \dot{q}_2 \dot{q}_3 \\ -s_2 c_3 \dot{q}_1 \dot{q}_2 - c_2 s_3 \dot{q}_1 \dot{q}_3 - c_3 \dot{q}_2 \dot{q}_3 \\ 0 \\ 0 \\ 0 \end{bmatrix} \end{array} \end{equation} $$
公式(\(\ref{equa_4_7}\))和(\(\ref{equa_4_8}\)) 对应的就是表 4.1中的\(\boldsymbol{E}\)和\(\boldsymbol{S}\)，但其中的\(\boldsymbol{T}\)保持不变。 因此在这个关节模型的例子中，\(\boldsymbol{S}\)变化了而\(\boldsymbol{T}\)保持不变。这种情况是可能的，因为\(\text{range}(\boldsymbol{S})\) 是常数， 并且\(\boldsymbol{T}\)只需满足\(\text{range}(\boldsymbol{T}) = \text{range}(\boldsymbol{S})^{\perp}\)。什么乱七八糟的这是，没懂
欧拉参数(Euler Parameter)(参见定点转动与四元数)
一般旋转也可以通过一个单位矢量和一个角度来描述。单位向量描述了旋转轴，角度描述了转动的大小。单位四元数与这种表示密切相关。如果\(\boldsymbol{u}\) 和 \(\theta\) 分别是单位向量和角度， 那么描述相同旋转的单位四元数为: $$ \begin{equation}\label{equa_4_10}\begin{array}{rcl} p_0 & = & \cos (\theta / 2) \\ p_1 & = & \sin (\theta / 2)u_x \\ p_2 & = & \sin (\theta / 2)u_y \\ p_3 & = & \sin (\theta / 2)u_z \end{array}\end{equation} $$ 这四个值并不是独立的，它们必须满足方程: $$ \begin{equation}\label{equa_4_11} p_0^2 + p_1^2 + p_2^2 + p_3^2 = 1 \end{equation} $$ 球形的关节必须由三个独立的速度变量。因此，如果我们希望用四个非独立的单位四元数作为位置变量，那么速度变量就不能写成位置变量的微分。 速度变量的最佳选择就是在笛卡尔坐标系 \(F_s\) 下的关节角速度矢量 \(\boldsymbol{\omega}\)。这将使得\(\boldsymbol{S}\)变得很简单，就是表 4.1中列举的常值。 因此，我们只需要给出 \(\boldsymbol{E}\) 的方程以及位置变量的求导公式。 $$ \begin{equation}\label{equa_4_12} \boldsymbol{E} = 2 \begin{bmatrix} p_0^2 + p_1^2 - 1/2 & p_1 p_2 + p_0 p_3 & p_1 p_3 - p_0 p_2 \\ p_1 p_2 - p_0 p_3 & p_0^2 + p_2^2 - 1/2 & p_2 p_3 + p_0 p_1 \\ p_1 p_3 + p_0 p_2 & p_2 p_3 - p_0 p_1 & p_0^2 + p_3^2 - 1/2 \end{bmatrix} \end{equation} $$ $$ \begin{equation}\label{equa_4_13} \begin{bmatrix} \dot{p}_0 \\ \dot{p}_1 \\ \dot{p}_2 \\ \dot{p}_3 \end{bmatrix} = \frac{1}{2} \begin{bmatrix} -p_1 & -p_2 & -p_3 \\ p_0 & -p_3 & p_2 \\ p_3 & p_0 & -p_1 \\ -p_2 & p_1 & p_0 \end{bmatrix} \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \end{equation} $$ 基于这些方程的球形关节模型在数值上优于基于式\(\ref{equa_4_7}, \ref{equa_4_8}, \ref{equa_4_9}\) 的关节模型。 但是，它确实给软件带来了负担， 因为它必须处理位置变量数量与速度变量数量不相等的问题。此外还有三个复杂的问题：① 位置变量的值必须通过积分方程\(\ref{equa_4_13}\)来获得而不是直接对速度变量积分； ② 零旋转由一组非零单位四元数定义；③ 在积分过程中需要经常对单位四元数进行归一化。因为积分过程中的截断误差，会导致单位四元数无法完全满足方程\(\ref{equa_4_11}\)， 误差会在关节所支持的整个子树中的线性放大，所以应在每此积分后对它们进行重新归一化。
表 4.3 完整的系统模型.
Complete system model
 |
4.6 一个完整的系统模型
表 4.3 给出了刚体系统的完整系统模型。对于运动树，所需的数据是：\(N_B\)、父节点数组、关节类型描述符数组、树变换矩阵数组、刚体惯性数组。 惯性表示在自身坐标空间中是常数。对于闭环系统，还需额外数据：\(N_L\)、对于每个闭环关节提供其前驱和后继编号、关节类型描述符以及前驱与后继的变换矩阵。
在最简单的情况下，软件包可能只适用于一组有限的关节类型，例如仅支持旋转关节和平动关节。 在这种情况下，jtype(i) 可以只是一个数字类型代码，
并将 \(\boldsymbol{X} J,\boldsymbol{v}_J, \boldsymbol{S}\) 的值直接硬编码到 jcalc 的源码中，甚至直接硬编码到实际使用的动力学算法中。
另一个简化的思路可以考虑关节变量向量，例如\(\boldsymbol{q}\) 和 \(\dot{\boldsymbol{q}}\)。
一般情况下，\(\boldsymbol{q}_i\) 是 \(\boldsymbol{q}\) 的子向量，并且它在\(\boldsymbol{q}\) 中的位置取决于所有前面的子向量的大小。
因此，访问和更新 \(\boldsymbol{q}_i\) 时应该有一个偏移量，对应每个关节一个，指示 \(\boldsymbol{q}\) 中哪个元素是 \(\boldsymbol{q}_i\) 的第一个元素。
如果每个关节都只有一个自由度，比如旋转关节和平动关节的情况，那么 \(\boldsymbol{q}_i\) 就只是 \(\boldsymbol{q}\) 的第\(i\)个元素。
如果软件的定位是通用的开发包，那么创建关节模型库并让 jcalc 访问该库更有意义。
一种特别有用的关节是用户定义的关节。它可以通过扩展接口来提供此功能。或者，在描述关节的数据结构中提供一个字段，简单地要求用户提供其自定义的 jcalc 的函数地址。