本篇继续介绍动力学库当中的一些函数及其数学运算

11、translationFromSXform

从空间变换矩阵提取位移向量，形式如下：

变换矩阵：

因此有：

【168-175】源码：

auto translationFromSXform(const Eigen::MatrixBase<T>& X) {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 matrix");
  RotMat<typename T::Scalar> R = rotationFromSXform(X);
  Vec3<typename T::Scalar> r =
      -matToSkewVec(R.transpose() * X.template bottomLeftCorner<3, 3>());
  return r;
}

12、invertSXform

空间变换矩阵的逆，首先有性质如下：

其中R为旋转矩阵，因此有：

并且：

TIPS：重写代码时，不必像MIT的代码里面那样绕来绕去，直接按逆的形式构建矩阵更为简洁，【180-189】源码：

auto invertSXform(const Eigen::MatrixBase<T>& X) {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 matrix");
  RotMat<typename T::Scalar> R = rotationFromSXform(X);
  Vec3<typename T::Scalar> r =
      -matToSkewVec(R.transpose() * X.template bottomLeftCorner<3, 3>());
  SXform<typename T::Scalar> Xinv = createSXform(R.transpose(), -R * r);
  return Xinv;
}

13、jointMotionSubspace

关节运动向量的子空间，根据关节类型以及旋转轴（移动轴），创建一个向量。例如关节类型为旋转关节，轴为X轴，则：

【195-213】源码：

SVec<T> jointMotionSubspace(JointType joint, CoordinateAxis axis) {
  Vec3<T> v(0, 0, 0);
  SVec<T> phi = SVec<T>::Zero();
  if (axis == CoordinateAxis::X)
    v(0) = 1;
  else if (axis == CoordinateAxis::Y)
    v(1) = 1;
  else
    v(2) = 1;

  if (joint == JointType::Prismatic)
    phi.template bottomLeftCorner<3, 1>() = v;
  else if (joint == JointType::Revolute)
    phi.template topLeftCorner<3, 1>() = v;
  else
    throw std::runtime_error("Unknown motion subspace");

  return phi;
}

14、jointXform

关节空间变换矩阵,输入关节类型，轴，以及角度（位移），返回变换矩阵。当关节类型为旋转关节时，调用函数（2）得到：

当关节为移动关节时，调用函数（9）创建变换矩阵，得到：

【219-237】源码：

Mat6<T> jointXform(JointType joint, CoordinateAxis axis, T q) {
  Mat6<T> X = Mat6<T>::Zero();
  if (joint == JointType::Revolute) {
    X = spatialRotation(axis, q);
  } else if (joint == JointType::Prismatic) {
    Vec3<T> v(0, 0, 0);
    if (axis == CoordinateAxis::X)
      v(0) = q;
    else if (axis == CoordinateAxis::Y)
      v(1) = q;
    else if (axis == CoordinateAxis::Z)
      v(2) = q;

    X = createSXform(RotMat<T>::Identity(), v);
  } else {
    throw std::runtime_error("Unknown joint xform\n");
  }
  return X;
}

16、spatialToLinearVelocity

空间向量转线速度,给点空间速度
以及给定点
的位置，求得该点下的线速度。速度向量：

可求得线速度：

【261-271】源码：

auto spatialToLinearVelocity(const Eigen::MatrixBase<T>& v,
                             const Eigen::MatrixBase<T2>& x) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 vector");
  Vec3<typename T::Scalar> vsAng = v.template topLeftCorner<3, 1>();
  Vec3<typename T::Scalar> vsLin = v.template bottomLeftCorner<3, 1>();
  Vec3<typename T::Scalar> vLinear = vsLin + vsAng.cross(x);
  return vLinear;
}

17、spatialToAngularVelocity

空间速度获取角速度，后三个变量即为角速度

【277-282】源码：

auto spatialToAngularVelocity(const Eigen::MatrixBase<T>& v) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  Vec3<typename T::Scalar> vsAng = v.template topLeftCorner<3, 1>();
  return vsAng;
}

18、spatialToLinearAcceleration

给定空间加速度向量，和速度，求线加速度

【289-300】源码：

auto spatialToLinearAcceleration(const Eigen::MatrixBase<T>& a,
                                 const Eigen::MatrixBase<T2>& v) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 6,
                "Must have 6x1 vector");

  Vec3<typename T::Scalar> acc;
  // classical accleration = spatial linear acc + omega x v
  acc = a.template tail<3>() + v.template head<3>().cross(v.template tail<3>());
  return acc;
}

【307-323】源码：

auto spatialToLinearAcceleration(const Eigen::MatrixBase<T>& a,
                                 const Eigen::MatrixBase<T2>& v,
                                 const Eigen::MatrixBase<T3>& x) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 6,
                "Must have 6x1 vector");
  static_assert(T3::ColsAtCompileTime == 1 && T3::RowsAtCompileTime == 3,
                "Must have 3x1 vector");

  Vec3<typename T::Scalar> alin_x = spatialToLinearVelocity(a, x);
  Vec3<typename T::Scalar> vlin_x = spatialToLinearVelocity(v, x);

  // classical accleration = spatial linear acc + omega x v
  Vec3<typename T::Scalar> acc = alin_x + v.template head<3>().cross(vlin_x);
  return acc;
}

19、sXFormPoint

对给定点作坐标变换，利用函数rotationFromSXform以及translationFromSXform从空间变换矩阵中提取旋转矩阵以及位移向量，作以下运算：

【329-340】源码：

auto sXFormPoint(const Eigen::MatrixBase<T>& X,
                 const Eigen::MatrixBase<T2>& p) {
  static_assert(T::ColsAtCompileTime == 6 && T::RowsAtCompileTime == 6,
                "Must have 6x6 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 vector");

  Mat3<typename T::Scalar> R = rotationFromSXform(X);
  Vec3<typename T::Scalar> r = translationFromSXform(X);
  Vec3<typename T::Scalar> Xp = R * (p - r);
  return Xp;
}

20、forceToSpatialForce

将某一点的力向量转为空间向量，力的空间向量表述为：

其中n为力矩，f为力，对于给定点P，其受力
作用，有：

【248-358】源码：

auto forceToSpatialForce(const Eigen::MatrixBase<T>& f,
                         const Eigen::MatrixBase<T2>& p) {
  static_assert(T::ColsAtCompileTime == 1 && T::RowsAtCompileTime == 3,
                "Must have 3x1 vector");
  static_assert(T2::ColsAtCompileTime == 1 && T2::RowsAtCompileTime == 3,
                "Must have 3x1 vector");
  SVec<typename T::Scalar> fs;
  fs.template topLeftCorner<3, 1>() = p.cross(f);
  fs.template bottomLeftCorner<3, 1>() = f;
  return fs;
}