ocs2_pinocchio_interface文件夹

pinocchio_forward_declaration.h

  1. JointCollectionDefaultTpl：这是一个模板结构体，具有两个模板参数：Scalar 和 Options。它可能用于存储一组关节对象。
  1. ModelTpl：这是一个模板结构体，具有三个模板参数：Scalar、Options 和 JointCollectionTpl。它可能用于描述一个机器人模型，其中包括了机器人的连杆、关节等信息。JointCollectionTpl 模板参数表示用于存储关节对象的数据结构。
  1. DataTpl：这是一个模板结构体，具有三个模板参数：Scalar、Options 和 JointCollectionTpl。它可能用于存储与机器人模型相关的数据，例如关节位置、速度等。JointCollectionTpl 模板参数表示用于存储关节对象的数据结构。
  1. JointModelTpl：这是一个模板结构体，具有三个模板参数：Scalar、Options 和 JointCollectionTpl。它可能用于描述单个关节的模型，包括关节的类型、参数等信息。JointCollectionTpl 模板参数表示用于存储关节对象的数据结构。

PinocchioInterface.h & PinocchioInterface.cpp

类模板定义：

  • Model：Pinocchio 模型的类型，是一个模板类型，依赖于标量类型 SCALAR。
  • Data：Pinocchio 数据的类型，同样依赖于 SCALAR。
  • JointModel：关节模型的类型，这里特别指定了 scalar_t 作为标量类型。

类成员函数：

  • 构造函数：从给定的 Pinocchio 模型构造 PinocchioInterfaceTpl 对象。它接受一个 Pinocchio 模型和一个可选的 URDF 模型作为参数。
  • 析构函数：清理资源。
  • 拷贝构造函数和移动构造函数：允许复制和移动 PinocchioInterfaceTpl 对象。
  • 拷贝赋值运算符和移动赋值运算符：允许对象的赋值操作。
  • getModel：获取 Pinocchio 模型。
  • getData：获取可变的和不可变的 Pinocchio 数据。
  • getUrdfModelPtr：获取 URDF（统一机器人描述格式）模型的共享指针。
  • toCppAd：将 Pinocchio 接口转换为 CppAD（自动微分库）标量类型。将 PinocchioInterfaceTpl 对象转换为使用 CppAD 标量类型的对象。该方法仅当 SCALAR 为 scalar_t 类型时才可用。该方法利用 getModel() 方法获取机器人模型，并将其转换为 CppAD 标量类型后返回一个新的 PinocchioInterfaceTpl 对象。
  • operator<<：重载输出流运算符，用于打印 Pinocchio 接口信息。

类私有成员：

  • robotModelPtr_：一个指向 Pinocchio 模型的共享指针。
  • robotDataPtr_：一个指向 Pinocchio 数据的独占指针。
  • urdfModelPtr_：一个指向 URDF 模型的共享指针。

类模板实例化：

  • PinocchioInterface：使用 scalar_t 作为模板参数的 PinocchioInterfaceTpl 类的别名。
  • PinocchioInterfaceCppAd：使用 ad_scalar_t（CppAD 的标量类型）作为模板参数的 PinocchioInterfaceTpl 类的别名。

特殊成员函数：

  • toCppAd：一个重载的成员函数，它将当前的 Pinocchio 模型转换为 CppAD 标量类型的模型。这里使用了 SFINAE（Substitution Failure Is Not An Error）技巧来实现条件编译。

模板实例化声明：

  • extern template class PinocchioInterfaceTpl<scalar_t>; 和 extern template class PinocchioInterfaceTpl<ad_scalar_t>; 是对外的模板实例化声明，它们告诉编译器存在这些模板的实例化，但定义在其他地方。

显式模板实例化：

  • template class PinocchioInterfaceTpl<scalar_t>; 是一个显式模板实例化，它告诉编译器编译这个模板的特定实例。

输出流运算符重载：

  • 重载了 << 运算符，以便于将 PinocchioInterface 对象的信息输出到流中，例如标准输出或文件。

urdf.h & urdf.cpp

用于从不同的URDF（Unified Robot Description Format）数据源创建 PinocchioInterface 对象。PinocchioInterface 是一个处理机器人模型和数据的接口类，通常与 Pinocchio 库一起使用，该库用于机器人动力学计算。
  1. getPinocchioInterfaceFromUrdfFile：从 URDF 文件创建，这些函数接受一个文件路径作为输入，该文件包含 URDF 格式的机器人模型描述。
      • 一个版本只接受 URDF 文件路径。
      • 另一个版本接受 URDF 文件路径和一个额外的根关节模型，根关节定义了机器人附着的基础，例如一个具有6个自由度的基座。
      • 使用 ::urdf::parseURDFFile 函数解析 URDF 文件。如果解析成功（即返回的 urdfTree 不是 nullptr），则调用 getPinocchioInterfaceFromUrdfModel 函数来创建 PinocchioInterface 对象。如果解析失败，则抛出一个 std::invalid_argument 异常。
  1. getPinocchioInterfaceFromUrdfString：从 URDF 字符串创建,这些函数接受一个包含 URDF 数据的 std::string 对象。
      • 一个版本只接受 URDF 字符串。
      • 另一个版本接受 URDF 字符串和一个根关节模型。
      • 使用 ::urdf::parseURDF 函数解析 URDF 字符串。其余实现与从文件创建相同。
  1. getPinocchioInterfaceFromUrdfModel：从 URDF 模型树创建，这些函数接受一个指向 urdf::ModelInterface 对象的共享指针。
      • 一个版本只接受 URDF 模型树指针。
      • 另一个版本接受 URDF 模型树指针和一个根关节模型。
      • 这个函数直接调用 pinocchio::urdf::buildModel 来从 URDF 模型树构建 Pinocchio 模型。然后使用构建的模型和 URDF 模型树指针创建 PinocchioInterface 对象并返回。
  • 这些函数在创建 PinocchioInterface 对象时，会将根关节模型传递给 pinocchio::urdf::buildModel 函数。这允许在构建 Pinocchio 模型时，将机器人模型附加到一个特定的根关节上。

PinocchioEndEffectorKinematics.h & PinocchioEndEffectorKinematics.cpp

定义了一个名为 PinocchioEndEffectorKinematics 的类，它实现了使用 Pinocchio 库的末端执行器运动学。这个类是 EndEffectorKinematics 接口的一个具体实现，专门用于处理机器人末端执行器的位置和速度等动力学特性。

构造函数：

  • 构造函数接受一个 PinocchioInterface 对象、一个状态映射（PinocchioStateInputMapping<scalar_t>）和一个末端执行器名称数组作为输入。构造函数初始化了 pinocchioInterfacePtr_ 为 nullptr，克隆了状态映射对象，并移动了末端执行器 ID 数组。然后，它遍历末端执行器 ID，为每个 ID 获取对应的 Pinocchio 模型中帧的 ID。。

成员函数：

  • clone：返回 PinocchioEndEffectorKinematics 对象的动态分配副本。
  • setPinocchioInterface：设置用于缓存的 Pinocchio 接口。在调用获取器（getters）之前，必须设置 Pinocchio 接口。
  • getIds：获取末端执行器的 ID（名称）。
  • getPosition：获取末端执行器的位置向量。需要先更新 Pinocchio 接口中的 pinocchio::Data。通过 Pinocchio 接口访问 pinocchio::Data 来计算所需的动力学特性。
  • getVelocity：获取末端执行器的速度向量。需要先更新 Pinocchio 接口中的 pinocchio::Data，包括前向运动学和速度。通过 Pinocchio 接口访问 pinocchio::Data 来计算所需的动力学特性。
  • getOrientationError：获取末端执行器的定向误差。通过 Pinocchio 接口访问 pinocchio::Data 来计算所需的动力学特性。
  • getPositionLinearApproximation：获取末端执行器位置的线性近似。需要先计算关节雅可比矩阵。
  • getVelocityLinearApproximation：获取末端执行器速度的线性近似。需要先计算速度导数和关节雅可比矩阵。
  • getOrientationErrorLinearApproximation：获取末端执行器定向误差的线性近似。需要先计算关节雅可比矩阵。

私有成员：

  • pinocchioInterfacePtr_：指向 PinocchioInterface 对象的指针，用于访问 Pinocchio 模型和数据。
  • mappingPtr_：指向状态映射对象的智能指针，它负责将 OCS2 状态映射到 Pinocchio 状态。
  • endEffectorIds_：存储末端执行器名称的数组。
  • endEffectorFrameIds_：存储与末端执行器名称对应的 Pinocchio 模型中帧的 ID。

PinocchioStateInputMapping.h

定义了一个模板类 PinocchioStateInputMapping，它用于在 OCS2（Optimal Control Software 2）状态和输入向量与 Pinocchio 状态和输入之间进行映射。

类模板定义：

  • PinocchioStateInputMapping 是一个模板类，它接受一个模板参数 SCALAR，通常用于指定标量类型（如 double 或 float）。

类成员类型定义：

  • vector_t：使用 Eigen 库定义的动态大小列向量类型。
  • matrix_t：使用 Eigen 库定义的动态大小矩阵类型。

构造函数和赋值运算符：

  • 默认构造函数：PinocchioStateInputMapping() = default; 创建一个默认的映射对象。
  • 虚析构函数：virtual ~PinocchioStateInputMapping() = default; 确保派生类可以正确地执行析构。
  • 删除的赋值运算符：PinocchioStateInputMapping<SCALAR>& operator=(const PinocchioStateInputMapping<SCALAR>& rhs) = delete; 防止对象的复制赋值。

纯虚函数：

  • virtual PinocchioStateInputMapping<SCALAR>* clone() const = 0;：创建当前对象的副本，需要派生类实现。
  • virtual vector_t getPinocchioJointPosition(const vector_t& state) const = 0;：从 OCS2 状态向量中获取 Pinocchio 关节点位置。
  • virtual vector_t getPinocchioJointVelocity(const vector_t& state, const vector_t& input) const = 0;：从 OCS2 状态和输入向量中获取 Pinocchio 关节点速度。
  • virtual std::pair<matrix_t, matrix_t> getOcs2Jacobian(const vector_t& state, const matrix_t& Jq, const matrix_t& Jv) const = 0;：将 Pinocchio 的雅可比矩阵 dfdq 和 dfdv 映射到 OCS2 的雅可比矩阵 dfdx 和 dfdu。

虚函数：

  • virtual void setPinocchioInterface(const PinocchioInterfaceTpl<SCALAR>& pinocchioInterface) {}：如果映射需要 PinocchioInterface，则使用此方法并设置更新后的 PinocchioInterface。默认实现为空，因为并非所有映射都需要 Pinocchio 接口。

保护构造函数：

  • PinocchioStateInputMapping(const PinocchioStateInputMapping<SCALAR>& rhs) = default;：受保护的拷贝构造函数，允许派生类复制构造。

ocs2_centroidal_model 文件夹

CentroidalModelInfo.h & CentroidalModelInfo.cpp

定义了一个模板结构体 CentroidalModelInfoTpl，以及相关的类型定义、枚举类、函数和操作符重载。这个结构体用于存储与质心模型相关的信息，通常在机器人学和动力学分析中使用。

类型别名和枚举类定义：

  • CentroidalModelInfo：使用 scalar_t 类型实例化的 CentroidalModelInfoTpl 的别名。
  • CentroidalModelInfoCppAd：使用 ad_scalar_t 类型实例化的 CentroidalModelInfoTpl 的别名，ad_scalar_t 可能是用于自动微分的标量类型。
  • CentroidalModelType：一个枚举类，定义了两种质心模型类型：完整的质心动力学（FullCentroidalDynamics）和单一刚体动力学（SingleRigidBodyDynamics）。
  • toString：一个函数，将 CentroidalModelType 枚举值转换为字符串。
  • operator<<：一个输出流运算符重载，允许将 CentroidalModelType 枚举值输出到 std::ostream。

模板结构体定义：

  • CentroidalModelInfoTpl：一个模板结构体，使用模板参数 SCALAR 来允许不同的数值类型（如 double 或 float）。

结构体成员：

  • EIGEN_MAKE_ALIGNED_OPERATOR_NEW：一个宏，确保 Eigen 库中的矩阵和向量在使用 new 操作符时正确对齐。
  • scalar_t：结构体中使用的标量类型，与模板参数 SCALAR 相同。
  • vector_t、vector3_t、matrix3_t：Eigen 库中的向量和矩阵类型，分别用于动态大小的向量、3元素向量和3x3矩阵。
  • EnableIfScalar_t：一个类型特征模板，使用 SFINAE（Substitution Failure Is Not An Error）原则，在类型 T 与 scalar_t 相同时启用。
  • 质心模型信息成员变量：
    • centroidalModelType：标识使用的质心模型类型。
    • numThreeDofContacts：3自由度接触点的数量（仅受力）。
    • numSixDofContacts：6自由度接触点的数量（受力和力矩）。
    • endEffectorFrameIndices：末端执行器帧索引的向量，区分3自由度和6自由度接触点。
    • generalizedCoordinatesNum：广义坐标的数量，用于 Pinocchio 模型中。
    • actuatedDofNum：驱动自由度的数量。
    • stateDim：定义系统流图所需的状态数量。
    • inputDim：定义系统流图所需的输入数量。
    • robotMass：机器人的总质量。
    • qPinocchioNominal：SRBD 模型中使用的标量类型机器人标称配置。
    • centroidalInertiaNominal：SRBD 模型中使用的标称机器人质心惯性（在标称基座帧中表示）。
    • comToBasePositionNominal：SRBD 模型中使用的标称质心到基座位置（在标称基座帧中表示）。

模板函数：

  • toCppAd：一个模板函数，允许将 CentroidalModelInfo 对象转换为 CentroidalModelInfoCppAd 对象，即从标量类型转换为自动微分类型。

外部模板实例化：

  • extern template struct CentroidalModelInfoTpl<scalar_t>; 和 extern template struct CentroidalModelInfoTpl<ad_scalar_t>;：这些声明用于在编译时实例化模板结构体，确保为 scalar_t 和 ad_scalar_t 类型提供具体实现。

AccessHelperFunctions.h

包含了一系列模板函数，它们提供对机器人系统中不同动力学属性的读写访问。这些属性包括接触力、接触力矩、关节速度、质心动量、基座位姿、关节角度和广义坐标。这些函数利用了 Eigen 库和 CentroidalModelInfoTpl 结构体来处理相关数据。
  • getContactForces：这两个函数提供对接触力的访问。第一个函数模板允许读写访问，而第二个函数模板提供只读访问。接触力是指机器人与环境接触点上的力，通常用于模拟机器人的步行或抓取等行为。
  • getContactTorques：这两个函数提供对接触力矩的访问，同样一个是可读写的，另一个是只读的。接触力矩是指在接触点上作用的力矩，它影响机器人的平衡和姿态。
  • getJointVelocities：这两个函数提供对机器人关节速度的访问。关节速度是描述机器人关节移动快慢的量，对于控制机器人的运动至关重要。
  • getNormalizedMomentum：这两个函数提供对质心动量的访问。质心动量是机器人动力学中的一个重要概念，它描述了机器人整体的动量，通常与机器人的稳定性和平衡有关。
  • getBasePose：这两个函数提供对基座位姿的访问。基座位姿描述了机器人在空间中的位置和方向，对于机器人的导航和路径规划非常重要。
  • getJointAngles：这两个函数提供对关节角度的访问。关节角度是描述机器人关节位置的参数，对于机器人的运动学和动力学分析至关重要。
  • getGeneralizedCoordinates：这两个函数提供对广义坐标的访问。广义坐标是描述机器人系统状态的一组变量，可以包括关节角度、速度等，是机器人动力学分析的基础。
 

ModelHelperFunctions.h

包含了一系列模板函数，主要用于计算与机器人的质心动力学相关的各种雅可比矩阵和导数。

1. computeFloatingBaseCentroidalMomentumMatrixInverse

功能: 计算对应于浮动基变量的质心动量矩阵的子块的逆矩阵。
输入参数:Ab: 质心动量矩阵。
输出参数:返回6x6的矩阵Ab_inv。
实现逻辑:
  • 首先获取矩阵Ab的质量mass，即Ab(0, 0)。
  • 计算Ab的右下角3x3子矩阵的逆矩阵Ab_22_inv。
  • 初始化一个6x6的零矩阵Ab_inv。
  • 按照公式填充Ab_inv：

2. updateCentroidalDynamics

功能: 更新data.Ag中的质心动量矩阵和data.com[0]中的质心位置，适用于FullCentroidalDynamics和SingleRigidBodyDynamics模型。
输入参数:
  • interface: 包含模型和数据的Pinocchio接口。
  • info: 质心模型信息。
  • q: 关节位置（广义坐标）。
实现逻辑:
  • 调用pinocchio::forwardKinematics计算前向运动学。
  • 根据模型调用pinocchio::computeJointJacobians计算关节雅可比矩阵。
  • 调用pinocchio::updateFramePlacements更新框架位置。

3. updateCentroidalDynamicsDerivatives

功能: 更新data.dHdq中的质心动量导数，适用于FullCentroidalDynamics和SingleRigidBodyDynamics模型。
输入参数:
  • interface: 包含模型和数据的Pinocchio接口。
  • info: 质心模型信息。
  • q: 关节位置（广义坐标）。
  • v: 关节速度（广义坐标的导数）。
实现逻辑:
  • 调用pinocchio::forwardKinematics计算前向运动学。
  • 调用pinocchio::computeJointJacobians计算关节雅可比矩阵。
  • 调用pinocchio::updateFramePlacements更新框架位置。

4. getMappingZyxGradient

功能: 计算ZYX欧拉角导数与全局角速度映射的导数。
输入参数:
  • eulerAngles: 从qPinocchio中提取的ZYX欧拉角。
输出参数:
  • 返回一个张量，表示映射相对于ZYX欧拉角的导数。

5. getRotationMatrixZyxGradient

功能: 计算从基坐标系到世界坐标系的旋转矩阵相对于ZYX欧拉角的导数。
输入参数:
  • eulerAngles: 从qPinocchio中提取的ZYX欧拉角。
输出参数:
  • 返回一个张量，表示旋转矩阵相对于ZYX欧拉角的导数。

6. getCentroidalMomentumZyxGradient

功能: 计算质心动量相对于ZYX欧拉角的导数。
输入参数:
  • interface: 包含模型和数据的Pinocchio接口。
  • info: 质心模型信息。
  • q: 关节位置（广义坐标）。
  • v: 关节速度（广义坐标的导数）。
输出参数:
  • 返回一个6x3的矩阵，表示质心动量相对于ZYX欧拉角的导数。

7. getCentroidalMomentumMatrix

功能: 从pinocchioInterface数据中获取质心动量矩阵。
输入参数:
  • interface: 包含模型和数据的Pinocchio接口。
输出参数:
  • 返回data.Ag中的质心动量矩阵。

8. getPositionComToContactPointInWorldFrame

功能: 计算世界坐标系下质心到接触点的位置。
输入参数:
  • interface: 包含模型和数据的Pinocchio接口。
  • info: 质心模型信息。
  • contactIndex: 接触点的索引。
输出参数:
  • 返回一个向量，表示世界坐标系下质心到接触点的位置。

9. getTranslationalJacobianComToContactPointInWorldFrame

功能: 计算世界坐标系下质心到接触点的平移雅可比矩阵。
输入参数:
  • interface: 包含模型和数据的Pinocchio接口。
  • info: 质心模型信息。
  • contactIndex: 接触点的索引。
输出参数:
  • 返回一个矩阵，表示质心到接触点的平移雅可比矩阵。

10. getNormalizedCentroidalMomentumRate

功能: 计算归一化质心动量的时间导数（线性+角动量），并表示在质心坐标系中。
输入参数:
  • interface: 包含模型和数据的Pinocchio接口。
  • info: 质心模型信息。
  • input: 系统输入向量。
输出参数:
  • 返回一个向量，表示归一化质心动量的时间导数。
 

PinocchioCentroidalDynamics.h & PinocchioCentroidalDynamics.cpp

定义了一个名为 PinocchioCentroidalDynamics 的类，它实现了基于 Pinocchio 库的机器人质心动力学模型。

类定义和成员变量

  • PinocchioCentroidalDynamics 类用于表示机器人的质心动力学。
  • 类模板参数 scalar_t 表示用于内部计算的标量类型，通常为 double 或 float。
  • 类成员变量包括：
    • pinocchioInterfacePtr_：指向 PinocchioInterface 的指针，用于访问 Pinocchio 模型和数据。
    • mapping_：CentroidalModelPinocchioMapping 类的实例，用于映射 OCS2 状态和输入到 Pinocchio 状态和输入。

构造函数和复制构造函数

  • 构造函数接受一个 CentroidalModelInfo 类型的参数，用于初始化质心模型信息。
  • 复制构造函数用于创建 PinocchioCentroidalDynamics 对象的副本。

成员函数

  • setPinocchioInterface：设置用于缓存的 Pinocchio 接口。在调用 getter 函数之前必须设置此接口。
  • getValue：计算系统流图 x_dot = f(x, u)，即给定状态和输入下的系统状态导数。
  • getLinearApproximation：计算系统流图的一阶线性近似，用于动态规划和控制算法中的梯度和雅可比矩阵计算。
    • 该函数计算系统流图的线性近似，包括状态导数 f、状态雅可比矩阵 dfdx 和输入雅可比矩阵 dfdu。
    • 调用 computeNormalizedCentroidalMomentumRateGradients 来计算归一化质心动量率梯度。
    • 使用 mapping_.getOcs2Jacobian 来获取 OCS2 状态和输入的雅可比矩阵。

私有成员函数

  • computeNormalizedCentroidalMomentumRateGradients：计算质心坐标系中归一化质心动量率（线性和角动量）的梯度。
    • 该函数计算接触力和力矩对质心加速度和归一化角动量率的偏导数。
    • 使用 skewSymmetricMatrix 函数来计算力和力矩的向量化表示。
    • 循环遍历所有接触点，计算线性和角动量率相对于广义坐标和输入的偏导数。

类成员类型定义

  • Vector3、Matrix3x、Matrix6x、Matrix3、Matrix6：使用 Eigen 库定义的不同类型的矩阵和向量。

FactoryFunctions.h & FactoryFunctions.cpp

这段程序是用于创建和处理机器人动态模型，它使用了Pinocchio库。

1. createPinocchioInterface (无关节名称参数版本)****

这个函数用于从URDF文件创建一个PinocchioInterface对象。
  • 参数：urdfFilePath - 机器人URDF文件的绝对路径。
  • 过程：
    • 使用pinocchio::JointModelComposite创建一个复合关节模型，包含平移和球面ZYX旋转关节，以模拟浮动基座的6个自由度（3个平移自由度和3个旋转自由度）。
    • 调用getPinocchioInterfaceFromUrdfFile函数，传入URDF文件路径和复合关节模型，以创建PinocchioInterface对象。
  • 返回值：创建的PinocchioInterface对象。

2. createPinocchioInterface (有关节名称参数版本)

这个函数的作用与无参数版本类似，但允许指定一个关节名称列表，不在列表中的关节将被视为多余关节并从URDF模型中移除。
  • 参数：
    • urdfFilePath - 机器人URDF文件的绝对路径。
    • jointNames - 需要保留的关节名称列表。
  • 过程：
    • 解析URDF文件，创建一个URDF模型树。
    • 遍历模型树中的所有关节，如果关节名称不在jointNames列表中，则将其类型设置为FIXED，即固定关节。
    • 同无参数版本一样，为浮动基座添加6个自由度的复合关节模型。
    • 调用getPinocchioInterfaceFromUrdfModel函数，传入修改后的URDF模型和复合关节模型，以创建PinocchioInterface对象。
  • 返回值：创建的PinocchioInterface对象。

3. createCentroidalModelInfo

这个函数用于创建一个CentroidalModelInfo对象，它包含了机器人的质心模型信息。
  • 参数：
    • interface - PinocchioInterface对象。
    • type - 质心模型的类型（SRBD或FCD）。
    • nominalJointAngles - SRBD模型中使用的关节名义角度。
    • threeDofContactNames - 具有3个自由度接触点（仅力）的末端执行器名称列表。
    • sixDofContactNames - 具有6个自由度接触点（力和扭矩）的末端执行器名称列表。
  • 过程：
    • 检查名义关节角度的数量是否与模型的广义坐标数量匹配。
    • 初始化CentroidalModelInfoTpl对象，设置质心模型类型、接触点数量、广义坐标数量、驱动自由度数量、状态维度和输入维度。
    • 计算机器人的总质量。
    • 获取3自由度和6自由度接触点的索引，并将其添加到endEffectorFrameIndices列表中。
    • 设置名义基础框架与世界框架对齐，计算名义质心惯性和名义质心到基座位置的偏移。
  • 返回值：创建的CentroidalModelInfo对象。

4. loadCentroidalType

这个函数用于从配置文件中加载质心模型类型。
  • 参数：
    • configFilePath - 配置文件的路径。
    • fieldName - 配置文件中质心模型类型的字段名称，默认为"centroidalModelType"。
  • 过程：
    • 使用Boost库的property_tree读取配置文件并获取质心模型类型的整数值。
    • 将整数值转换为CentroidalModelType枚举类型。

5. loadDefaultJointState

这个函数用于从配置文件中加载默认的关节状态。
  • 参数：
    • numJointState - 关节状态的数量。
    • configFilePath - 配置文件的路径。
    • fieldName - 配置文件中默认关节状态的字段名称，默认为"defaultJointState"。
  • 过程：
    • 创建一个大小为numJointState的vector_t对象defaultJoints。
    • 使用ocs2::loadData::loadEigenMatrix函数从配置文件中加载默认关节状态到defaultJoints。
这些函数共同构成了一个用于创建和管理机器人动态模型的框架，特别是在模拟和控制系统设计中非常有用。

CentroidalModelPinocchioMapping.h & CentroidalModelPinocchioMapping.cpp

定义了一个模板类 CentroidalModelPinocchioMappingTpl，用于质心动力学模型和Pinocchio库之间映射。这个类将质心动力学的状态和输入转换为Pinocchio关节空间的状态和速度，并提供了计算雅可比矩阵的方法。以下是对代码的详细解释：

类模板声明和类型别名

首先，代码声明了一个模板类 CentroidalModelPinocchioMappingTpl，然后定义了两个类型别名 CentroidalModelPinocchioMapping 和 CentroidalModelPinocchioMappingCppAd，分别用于 scalar_t 和 ad_scalar_t 类型的特化。

类定义

构造函数和析构函数

  • explicit CentroidalModelPinocchioMappingTpl(CentroidalModelInfoTpl<SCALAR> centroidalModelInfo)：构造函数，接受一个 CentroidalModelInfoTpl<SCALAR> 类型的参数，用于初始化质心模型信息。初始化 pinocchioInterfacePtr_ 为 nullptr 并移动传入的 centroidalModelInfo。
  • ~CentroidalModelPinocchioMappingTpl() override = default;：默认的析构函数。
  • CentroidalModelPinocchioMappingTpl *clone() const override;：返回当前对象的一个副本。

成员函数

  • void setPinocchioInterface(const PinocchioInterfaceTpl<SCALAR> &pinocchioInterface)：设置用于缓存的Pinocchio接口。设置 pinocchioInterfacePtr_ 为传入的Pinocchio接口的地址。
  • vector_t getPinocchioJointPosition(const vector_t &state) const：根据系统状态向量计算Pinocchio关节位置。
  • vector_t getPinocchioJointVelocity(const vector_t& state, const vector_t& input) const：根据系统状态和输入向量计算Pinocchio关节速度。它使用了质心动量矩阵的逆、关节速度以及一些动力学计算。
  • std::pair<matrix_t, matrix_t> getOcs2Jacobian(const vector_t& state, const matrix_t& Jq, const matrix_t& Jv) const：映射Pinocchio的雅可比矩阵到OCS2的雅可比矩阵。涉及到对Pinocchio模型的动力学和几何属性的计算，以及对不同质心模型类型的处理。

私有成员

  • const PinocchioInterfaceTpl<SCALAR> *pinocchioInterfacePtr_：指向Pinocchio接口的指针。
  • const CentroidalModelInfoTpl<SCALAR> centroidalModelInfo_：存储质心模型信息。