1、关节类型

【19行】定义关节类型

enum class JointType { Prismatic, Revolute, FloatingBase, Nothing };
  • Prismatic：移动关节
  • Revolute：旋转关节
  • FloatingBase：浮动基座

2、spatialRotation

空间旋转矩阵，计算从A到B的变换，其中B绕轴旋转theta，公式如下：

其中，R为旋转矩阵，【25-34】源码：
SXform<T> spatialRotation(CoordinateAxis axis, T theta) {
  static_assert(std::is_floating_point<T>::value,
                "must use floating point value");
  RotMat<T> R = coordinateRotation(axis, theta);
  SXform<T> X = SXform<T>::Zero();
  X.template topLeftCorner<3, 3>() = R;
  X.template bottomRightCorner<3, 3>() = R;
  return X;
}

3、motionCrossMatrix

计算空间运动的叉乘矩阵，实际上是构建速度向量，包括线速度和角速度，各自的的反对称矩阵形式，形式如下：

【40-51】源码：

auto motionCrossMatrix(const Eigen::MatrixBase<T>& v) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  Mat6<typename T::Scalar> m;
  m << 0, -v(2), v(1), 0, 0, 0, v(2), 0, -v(0), 0, 0, 0, -v(1), v(0), 0, 0, 0,
      0,

      0, -v(5), v(4), 0, -v(2), v(1), v(5), 0, -v(3), v(2), 0, -v(0), -v(4),
      v(3), 0, -v(1), v(0), 0;
  return m;
}

4、forceCrossMatrix

计算力的叉乘矩阵，实际上是函数（3）motionCrossMatrix得到的矩阵取反再转置，形式如下

【57-64】源码：

auto forceCrossMatrix(const Eigen::MatrixBase<T>& v) {
  Mat6<typename T::Scalar> f;
  f << 0, -v(2), v(1), 0, -v(5), v(4), v(2), 0, -v(0), v(5), 0, -v(3), -v(1),
      v(0), 0, -v(4), v(3), 0, 0, 0, 0, 0, -v(2), v(1), 0, 0, 0, v(2), 0, -v(0),
      0, 0, 0, -v(1), v(0), 0;
  return f;
}

5、motionCrossProduct

两个运动向量叉乘，原理如下：

其中â为函数（3）中得到的叉乘矩阵，该方法直接给出两向量叉乘的代数结果，省去了矩阵相乘的一步，即:

【70-82】源码：

auto motionCrossProduct(const Eigen::MatrixBase<T>& a,
                        const Eigen::MatrixBase<T>& b) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  SVec<typename T::Scalar> mv;
  mv << a(1) * b(2) - a(2) * b(1), a(2) * b(0) - a(0) * b(2),
      a(0) * b(1) - a(1) * b(0),
      a(1) * b(5) - a(2) * b(4) + a(4) * b(2) - a(5) * b(1),
      a(2) * b(3) - a(0) * b(5) - a(3) * b(2) + a(5) * b(0),
      a(0) * b(4) - a(1) * b(3) + a(3) * b(1) - a(4) * b(0);
  return mv;
}

6、forceCrossProduct

两个力向量叉乘，原理与函数（5）类似

【89-102】源码：

auto forceCrossProduct(const Eigen::MatrixBase<T>& a,
                       const Eigen::MatrixBase<T>& b) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  SVec<typename T::Scalar> mv;
  mv << b(2) * a(1) - b(1) * a(2) - b(4) * a(5) + b(5) * a(4),
      b(0) * a(2) - b(2) * a(0) + b(3) * a(5) - b(5) * a(3),
      b(1) * a(0) - b(0) * a(1) - b(3) * a(4) + b(4) * a(3),
      b(5) * a(1) - b(4) * a(2),
      b(3) * a(2) - b(5) * a(0),
      b(4) * a(0) - b(3) * a(1);
  return mv;
}

7、sxformToHomogeneous

空间变换矩阵转齐次坐标矩阵，形式如下

空间变换矩阵：

齐次变换矩阵：

其中R
∈R3×3为旋转矩阵，r∈R3×1为线性位移向量，【108-118】源码：

auto sxformToHomogeneous(const Eigen::MatrixBase<T>& X) {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 matrix");
  Mat4<typename T::Scalar> H = Mat4<typename T::Scalar>::Zero();
  RotMat<typename T::Scalar> R = X.template topLeftCorner<3, 3>();
  Mat3<typename T::Scalar> skewR = X.template bottomLeftCorner<3, 3>();
  H.template topLeftCorner<3, 3>() = R;
  H.template topRightCorner<3, 1>() = matToSkewVec(skewR * R.transpose());
  H(3, 3) = 1;
  return H;
}

8、homogeneousToSXform

齐次变换矩阵转空间变换矩阵，作用与函数（7）相反，形式参考（7），【124-134】源码：

auto homogeneousToSXform(const Eigen::MatrixBase<T>& H) {
  static_assert(T::ColsAtCompileTime == 4 && T::RowsAtCompileTime == 4,
                "Must have 4x4 matrix");
  Mat3<typename T::Scalar> R = H.template topLeftCorner<3, 3>();
  Vec3<typename T::Scalar> translate = H.template topRightCorner<3, 1>();
  Mat6<typename T::Scalar> X = Mat6<typename T::Scalar>::Zero();
  X.template topLeftCorner<3, 3>() = R;
  X.template bottomLeftCorner<3, 3>() = vectorToSkewMat(translate) * R;
  X.template bottomRightCorner<3, 3>() = R;
  return X;
}

9、createSXform

根据旋转矩阵R，位移向量r，创建空间变换矩阵，形式如下：

其中R
∈R3×3为旋转矩阵，r∈R3×1为线性位移向量，【140-151】源码：

auto createSXform(const Eigen::MatrixBase<T>& R,
                  const Eigen::MatrixBase<T2>& r) {
  static_assert(T::ColsAtCompileTime == 3 && T::RowsAtCompileTime == 3,
                "Must have 3x3 matrix");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 matrix");
  Mat6<typename T::Scalar> X = Mat6<typename T::Scalar>::Zero();
  X.template topLeftCorner<3, 3>() = R;
  X.template bottomRightCorner<3, 3>() = R;
  X.template bottomLeftCorner<3, 3>() = -R * vectorToSkewMat(r);
  return X;
}

10、rotationFromSXform

从空间变换矩阵提取旋转矩阵，左上角的矩阵块，【157-162】源码：

auto rotationFromSXform(const Eigen::MatrixBase<T>& X) {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 matrix");
  RotMat<typename T::Scalar> R = X.template topLeftCorner<3, 3>();
  return R;
}