Pinocchion 核心算法RNEA 实现详解

rnea.hxx 文件实现了 Recursive Newton-Euler Algorithm (RNEA) 算法家族，这是机器人动力学中的经典算法，主要用于计算：

• 关节力矩：给定机器人的位置、速度和加速度，计算所需的关节驱动力矩
• 非线性效应：计算科氏力和离心力
• 广义重力：计算重力对关节的影响
• 静力矩：计算静态平衡时的关节力矩
• 科氏矩阵：计算机器人动力学方程中的科氏矩阵
  

2. 算法原理与数学公式2.1 基本RNEA算法

RNEA算法采用两步法：

• 前向传播：从基坐标系到末端坐标系计算各关节的速度和加速度
• 后向传播：从末端坐标系到基坐标系计算各关节的力和力矩
  

前向传播公式

vi=vjoint,i+iXparent(i)vparent(i)ai=ajoint,i+iXparent(i)aparent(i)+vi×vjoint,ivi​ai​​=vjoint,i​+iXparent(i)​vparent(i)​=ajoint,i​+iXparent(i)​aparent(i)​+vi​×vjoint,i​​

• v_i：关节i的速度（局部坐标系）
• a_i：关节i的加速度（局部坐标系）
• {}^{i}X_{parent(i)}：从父关节到当前关节的坐标变换
  

后向传播公式

fi=Iiai+vi×(Iivi)−fext,iτi=SiTfi+τparent(i)fi​τi​​=Ii​ai​+vi​×(Ii​vi​)−fext,i​=SiT​fi​+τparent(i)​​

• f_i：关节i处的力（局部坐标系）
• I_i：关节i的惯性张量
• S_i：关节i的运动旋量矩阵
• \tau_i：关节i的力矩
  

2.2 非线性效应计算

非线性效应（科氏力和离心力）的计算公式：

nlei=τi−M(q)ai−G(q)nlei​=τi​−M(q)ai​−G(q)

其中 M(q) 是惯性矩阵，G(q) 是重力向量。

2.3 广义重力计算

广义重力向量 G(q) 的计算公式：

G(q)=τi∣v=0,a=0G(q)=τi​∣v=0,a=0​

即在速度和加速度为零时的关节力矩。

2.4 科氏矩阵计算

科氏矩阵 C(q, v) 满足：

C(q,v)v=nlei−G(q)C(q,v)v=nlei​−G(q)

其元素可通过以下公式计算：

Cij=12(∂Mij∂qkvk+∂Mik∂qjvk−∂Mjk∂qivk)Cij​=21​(∂qk​∂Mij​​vk​+∂qj​∂Mik​​vk​−∂qi​∂Mjk​​vk​)

3. 代码结构与实现细节3.1 核心类结构

类名

功能

关键方法

RneaForwardStepRNEA前向传播计算速度、加速度和力

RneaBackwardStep

RNEA后向传播

计算关节力矩

NLEForwardStep

非线性效应前向传播

计算速度和力

NLEBackwardStep

非线性效应后向传播

计算非线性效应力矩

ComputeGeneralizedGravityForwardStep

广义重力前向传播

计算加速度和力

ComputeGeneralizedGravityBackwardStep

广义重力后向传播

计算重力力矩

CoriolisMatrixForwardStep

科氏矩阵前向传播

计算速度、惯性和雅可比

CoriolisMatrixBackwardStep

科氏矩阵后向传播

计算科氏矩阵

3.2 核心函数实现3.2.1 RNEA主函数template<...>const typename DataTpl<...>::TangentVectorType & rnea(  const ModelTpl<...> & model,  DataTpl<...> & data,  const Eigen::MatrixBase<ConfigVectorType> & q,  const Eigen::MatrixBase<TangentVectorType1> & v,  const Eigen::MatrixBase<TangentVectorType2> & a){  // 初始化  data.tau.setZero();  data.v[0].setZero();  data.a_gf[0] = -model.gravity;  // 前向传播：计算速度和加速度  typedef RneaForwardStep<...> Pass1;  for (JointIndex i = 1; i < model.njoints; ++i) {    Pass1::run(model.joints, data.joints, arg1);  }  // 后向传播：计算力和力矩  typedef RneaBackwardStep<...> Pass2;  for (JointIndex i = model.njoints - 1; i > 0; --i) {    Pass2::run(model.joints, data.joints, arg2);  }  // 添加转子惯性贡献  data.tau.array() += model.armature.array() * a.array();  return data.tau;}3.2.2 前向传播实现template<typename JointModel>static void algo(  const JointModelBase<JointModel> & jmodel,  JointDataBase<typename JointModel::JointDataDerived> & jdata,  const Model & model,  Data & data,  const Eigen::MatrixBase<ConfigVectorType> & q,  const Eigen::MatrixBase<TangentVectorType1> & v,  const Eigen::MatrixBase<TangentVectorType2> & a){  const JointIndex i = jmodel.id();  const JointIndex parent = model.parents;  // 计算关节状态  jmodel.calc(jdata.derived(), q.derived(), v.derived());  // 计算关节位姿变换  data.liMi = model.jointPlacements * jdata.M();  // 计算速度（局部坐标系）  data.v = jdata.v();  if (parent > 0)    data.v += data.liMi.actInv(data.v[parent]);  // 计算加速度（局部坐标系）  data.a_gf = jdata.c() + (data.v ^ jdata.v());  data.a_gf += jdata.S() * jmodel.JointMappedVelocitySelector(a);  data.a_gf += data.liMi.actInv(data.a_gf[parent]);  // 计算力（局部坐标系）  model.inertias.__mult__(data.v, data.h);  model.inertias.__mult__(data.a_gf, data.f);  data.f += data.v.cross(data.h);}3.2.3 后向传播实现template<typename JointModel>static void algo(  const JointModelBase<JointModel> & jmodel,  JointDataBase<typename JointModel::JointDataDerived> & jdata,  const Model & model,  Data & data){  const JointIndex i = jmodel.id();  const JointIndex parent = model.parents;  // 计算关节力矩  jmodel.JointMappedVelocitySelector(data.tau) += jdata.S().transpose() * data.f;  // 传递力到父关节  if (parent > 0)    data.f[parent] += data.liMi.act(data.f);}4. 关键技术点解析4.1 坐标变换处理

RNEA算法在不同坐标系中进行计算，关键的坐标变换包括：

• liMi：关节i的局部位姿变换矩阵
• actInv：将父关节的速度/加速度转换到当前关节的局部坐标系
• act：将当前关节的力转换到父关节的坐标系
  

4.2 运动旋量计算

运动旋量是RNEA算法的核心概念，用于表示关节的运动：

• jdata.v()：关节的局部速度旋量
• jdata.c()：关节的科氏项旋量
• jdata.S()：关节的运动旋量矩阵
  

4.3 惯性张量处理

惯性张量在不同坐标系中的转换：

• model.inertias：关节i的局部惯性张量
• data.oYcrb：关节i在世界坐标系中的惯性张量
  

4.4 雅可比矩阵计算

雅可比矩阵用于科氏矩阵的计算：

• data.J：关节的雅可比矩阵
• data.dJ：雅可比矩阵的时间导数
  

5. 代码优化技术

• 模板元编程：使用模板实现泛型编程，支持不同的关节类型和数值类型
• 关节访问者模式：使用 JointUnaryVisitorBase 统一处理不同类型的关节
• 表达式模板：利用Eigen库的表达式模板技术，减少临时变量
• 内存复用：在 Data 结构中预分配内存，避免运行时内存分配
• 向量化计算：使用Eigen库的向量化功能，提高计算效率
  

6. 应用场景

RNEA算法在以下场景中具有广泛应用：

• 机器人控制：计算实时控制所需的关节力矩
• 轨迹优化：在轨迹规划中考虑动力学约束
• 仿真模拟：进行机器人动力学仿真
• 参数辨识：通过实验数据辨识机器人的惯性参数
• 力控制：实现基于力反馈的机器人控制
  

7. 输入输出示例输入：// 定义模型和数据Model model = // 机器人模型Data data(model);// 输入状态VectorXd q = // 关节位置VectorXd v = // 关节速度VectorXd a = // 关节加速度// 计算关节力矩VectorXd tau = rnea(model, data, q, v, a);输出：// tau 包含了各关节所需的力矩// 例如，对于2关节机器人：// tau = [tau1, tau2]// 其中 tau1 和 tau2 分别是关节1和关节2所需的力矩8. 总结

rnea.hxx 文件实现了一套完整的RNEA算法家族，包括：

• 核心RNEA算法：计算关节力矩
• 非线性效应：计算科氏力和离心力
• 广义重力：计算重力影响
• 静力矩：计算静态平衡力矩
• 科氏矩阵：计算科氏矩阵
  

这些实现采用了现代C++技术，如模板元编程、关节访问者模式和表达式模板，确保了计算效率和代码可维护性。RNEA算法是机器人动力学分析的基础，广泛应用于机器人控制、仿真和优化等领域。

该实现的核心优势在于：

• 计算效率：O(n)时间复杂度，适用于实时应用
• 通用性：支持多种关节类型和机器人结构
• 可扩展性：易于集成到更大的机器人系统中
• 精度：提供准确的动力学计算结果
  

通过理解和应用这些算法，可以更好地分析和控制机器人系统，实现更复杂的机器人任务。

3.2 部分，加上代码块就会好很多