肢体链（GeoChain）

• GeoChain是链接两个实体GeoEntity的链条，一个GeoChain内部具有多个首尾相连的GeoLink.
  

• GeoChain里边有几个关键的成员：

  base 基座实体，在机器人中默认所有的GeoChain全部指向base
  terminal 末端实体，通常为Foot或Hand类型
  basedockerPos这个GeoLink连接到基座的点位
  boneLength类型为数组，代表每段骨骼的长度
  thresholds每一段骨骼末端旋转的阈值
  joints关节数值（与骨骼数量相同）
  chain 最关键的成员，GeoChain中的所有骨骼，每一段骨骼都是一个Bone。骨骼的末端rotation就是joints的数值。
  mass 总质量，massCenter一个1x3 ndarray，代表质心

• 除此之外，GeoChain还有以下函数：

  ResolveChain 姿态逆解（从末端位置姿态到各个关节角度）
  BuildChain 姿态正解（从各个关节角度到末端位置姿态）

正解姿态

• 实现骨骼链条的正解和逆解前需要具备一些基本的数学知识。

旋转矩阵

• 现在在空间上有一个点，坐标为$(x_1,y_1,z_1)$，从空间原点到这个点连接了杆子，然后以原点为中心，让这条杆子按顺序绕x,y,z旋转指定度数
  $\theta1,\theta2,\theta3$,那么这个点的坐标会变成$(x_2,y_2,z_2)$。

• 那么问题是:如何根据 $\theta1,\theta2,\theta3$ 以及$(x_1,y_1,z_1)$，计算$(x_2,y_2,z_2)$

• 首先应当注意的一点是： 旋转的顺序在这里非常关键。我们把三个轴向的旋转拆分成三个旋转矩阵。代码如下：

  def EulerMatrix(roll, pitch, yaw,order='xyz'):
    # 将欧拉角转换为旋转矩阵
    matrixBundle = {}
    #   yaw
    matrixBundle['z'] = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])
    #   pitch
    matrixBundle['y'] = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])
    #   roll
    matrixBundle['x'] = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])
    mat = np.eye(3)
    for axis in order:
        mat = matrixBundle[axis] @ mat
    return mat

逆解姿态

重心计算