Robotics Library项目代码分析（5）math库下的spatial

最新推荐文章于 2024-10-06 14:42:09 发布

no_to_last

最新推荐文章于 2024-10-06 14:42:09 发布

阅读量868

点赞数 18

文章标签： c++ 开发语言

本文链接：https://blog.csdn.net/no_to_last/article/details/141368806

版权

math库下的spatial

前言

我们看了一下math库下的子文件夹metrics，里面主要包含了l2，l2 squared函数，现在我们继续分析spatial文件夹下的内容。spatial有空间的意思，现在让我们开始分析。

ArticulatedBodyInertia.h

一些声明

首先还是老样子，一个在rl以及math下面的包裹的spatial空间，这里面先定义了三个前置声明的class，分别是ForceVector，MotionVector和RigidBodyInertia。以及一个ArticulatedBodyInertia的类。

namespace rl {
namespace math {
namespace spatial {
template <typename Scalar>
class ForceVector;
template <typename Scalar>
class MotionVector;
template <typename Scalar>
class RigidBodyInertia;
class ArticulatedBodyInertia{...};
}  // namespace spatial
}  // namespace math
}  // namespace rl

ArticulatedBodyInertia的构造函数

分析一个类应该先从其构造函数，定义开始。让我们先从构造函数入手，上面那个是一个默认的构造函数，没有参数，下面那个是传入一个other的构造函数，这个other是一个Eigen::DenseBase<OtherDerived>，获取other的topRightCorner<3, 3>和topLeftCorner<3, 3>，bottomRightCorner<3, 3>，将其传递给centerOfGravityData，inertiaData和massData。其中为啥要特意加上template呢，因为种类的topRightCorner，topLeftCorner和bottomRightCorner去是作为一个模板函数，传递的是一个较为通用的函数,然后通过这个函数调用返回一个矩阵并赋值返回。这里传入的other是由DenseBase派生的矩阵，因为DenseBase是类似于Eigen定义的一个基础的矩阵，被各种矩阵作为基类继承，它实现了一些基本的矩阵运算。

  ArticulatedBodyInertia() {}

  template <typename OtherDerived>
  ArticulatedBodyInertia(const ::Eigen::DenseBase<OtherDerived>& other)
      : centerOfGravityData(other.template topRightCorner<3, 3>()),
        inertiaData(other.template topLeftCorner<3, 3>()),
        massData(other.template bottomRightCorner<3, 3>()) {}

ArticulatedBodyInertia的析构函数
这里非常简单，就是一个默认的析构函数，通过cpp的RAII机制会自动释放上面的资源。

virtual ~ArticulatedBodyInertia() {}

ArticulatedBodyInertia类里面的一些定义
其中Scalar是在ArticulatedBodyInertia类上面的那个template<typename Scalar>定义的，这里主要就是定义了四种矩阵，分别是centerOfGravityData，inertiaData，massData和inertiaInverseData。矩阵的大小也已经确定了，类型都是模板Scalar。剩下三个就是给那三个Matrix加上一个const常量。

  typedef Scalar ScalarType;

  typedef ::Eigen::Matrix<Scalar, 6, 6> MatrixType;

  typedef ::Eigen::Matrix<Scalar, 3, 3> CenterOfGravityType;

  typedef const CenterOfGravityType ConstCenterOfGravityType;

  typedef ::Eigen::Matrix<Scalar, 3, 3> InertiaType;

  typedef const InertiaType ConstInertiaType;

  typedef ::Eigen::Matrix<Scalar, 3, 3> MassType;

  typedef const MassType ConstMassType;

当然，我们别漏下这一行，这是一个宏，Eigen 库中的一个宏，用于确保在使用 Eigen 库时，动态分配的对象在内存中是对齐的。这个宏会重载 new 和 delete 操作符，使得分配的内存满足 Eigen 的对齐要求。C++17在编译期会对具有固定大小的结构进行内存对齐，所以如果是使用比较新的编译器，使用c++17标准的编译器，这个宏是可以省略的。

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

私有成员
这里就是定义了三个Matrix，类型都是Eigen::Matrix<Scalar, 3, 3>。

 private:
  CenterOfGravityType centerOfGravityData;

  InertiaType inertiaData;

  MassType massData;

成员函数
这里其实就是定义了几个成员函数，分别是cog()和inertia()，mass()，matrix()。其中cog()和inertia()返回的是CenterOfGraintType和InertiaType，mass()返回的是MassType。matrix()返回的是一个MatrixType，这个MatrixType中topLeftCorner<3, 3>和topRightCorner<3, 3>分别对应inertiaData和centerOfGravityData，bottomLeftCorner<3,3>()对应cog的转置，而bottomRightCorner<3, 3>()对应massData，和传入参数的构造函数是对应的。

  CenterOfGravityType& cog() { return centerOfGravityData; }

  ConstCenterOfGravityType& cog() const { return centerOfGravityData; }

  InertiaType& inertia() { return inertiaData; }

  ConstInertiaType& inertia() const { return inertiaData; }

  MassType& mass() { return massData; }

  ConstMassType& mass() const { return massData; }

  MatrixType matrix() const {
    MatrixType res;
    res.template topLeftCorner<3, 3>() = inertia();
    res.template topRightCorner<3, 3>() = cog();
    res.template bottomLeftCorner<3, 3>() = cog().transpose();
    res.template bottomRightCorner<3, 3>() = mass();
    return res;
  }

重载
这个也是一样，就是根据其定义，然后赋值，其他的-，*结构都差不多。

  template <typename OtherDerived>
  ArticulatedBodyInertia& operator=(
      const ::Eigen::MatrixBase<OtherDerived>& other) {
    cog() = other.template topRightCorner<3, 3>();
    inertia() = other.template topLeftCorner<3, 3>();
    mass() = other.template bottomRightCorner<3, 3>();
    return *this;
  }

另外还有一些重载重载只有声明，留在了ArticulatedBodyInertia.hxx中定义了

  template <typename OtherScalar>
  ArticulatedBodyInertia& operator=(const RigidBodyInertia<OtherScalar>& other);

其他两个函数
其中setIdentity和setZero分别将centerOfGravityData，inertiaData和massData都设置为单位矩阵或者0，zero是0，identity是单位矩阵。

  void setIdentity() {
    cog().setZero();
    inertia().setIdentity();
    mass().setIdentity();
  }

  void setZero() {
    cog().setZero();
    inertia().setZero();
    mass().setZero();
  }

ArticulatedBodyInertia.hxx

下面这里使用了两个模板参数 Scalar 和 OtherScalar 是为了允许不同类型的标量（即数值类型）之间的赋值操作。ArticulatedBodyInertia.h中几个只有声明的重载函数放到这里面定义了。这两个定义放到这里有点多余，可以全部放到.h文件里面的，我们将这里面的声明放到.h文件里面，然后编译，目前是可以编译成功没有问题的。

template <typename Scalar>
template <typename OtherScalar>
inline ArticulatedBodyInertia<Scalar>&
ArticulatedBodyInertia<Scalar>::operator=(
    const RigidBodyInertia<OtherScalar>& other) {
  cog() = other.cog().cross33();
  inertia() = other.inertia();
  mass() = ::Eigen::Matrix<Scalar, 3, 3>::Identity() * other.mass();
  return *this;
}