Pinocchio机器人动力学库中固定关节质量对质心计算的影响分析

概述

在使用Pinocchio机器人动力学库进行机器人质心(CoM)计算时，开发者可能会遇到一个常见问题：当模型包含固定关节(fixed joints)时，这些关节所连接部件的质量似乎没有被纳入质心计算。本文将通过一个实际案例详细分析这一现象的原因，并提供正确的解决方案。

问题现象

在使用Pinocchio的centerOfMass()函数计算Unitree Go2四足机器人模型的质心时，发现修改固定关节(如Head_upper)的质量参数后，计算结果并未发生相应变化。这看似表明固定关节的质量没有被计入总质心计算。

技术原理

Pinocchio库在设计时遵循以下重要原则：

1. 运动质量与静态质量区分：Pinocchio默认只计算与机器人运动状态相关的质量，即那些会随机器人配置(configuration)变化而改变位置的部件质量。
2. 固定关节的特殊处理：当固定关节直接连接到世界坐标系(World frame)时，其质量被视为静态质量，不参与机器人运动状态下的质心计算。

问题根源

在上述案例中，问题并非出在Pinocchio的计算逻辑上，而是由于模型构建时的一个常见疏忽：

• 模型没有添加自由浮动基座(FreeFlyer joint)，导致固定关节直接连接到世界坐标系
• 这种情况下，固定关节被视为环境的一部分，而非机器人运动链的一部分

解决方案

正确的做法是在构建模型时显式添加自由浮动基座，使所有关节(包括固定关节)都成为机器人运动链的一部分。修改后的代码示例如下：

import pinocchio as pin

# 加载URDF模型并添加自由浮动基座
model = pin.buildModelFromUrdf(urdf_path, pin.JointModelFreeFlyer())
data = model.createData()

# 其余计算步骤保持不变
q = pin.neutral(model)
pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
pin.computeTotalMass(model, data)

com_W = pin.centerOfMass(model, data, q)

技术要点

1. 自由浮动基座的作用：JointModelFreeFlyer()为机器人添加了一个6自由度的基座关节，使机器人可以在三维空间中自由移动和旋转。
2. 固定关节的处理：当固定关节连接到自由浮动基座而非直接连接到世界坐标系时，其质量会被正确计入机器人总质量。
3. 计算流程优化：完整的质心计算应包含前向运动学、帧位姿更新和总质量计算等步骤。

实际应用建议

1. 对于四足机器人等复杂系统，建议总是添加自由浮动基座
2. 检查模型构建时各关节的连接关系，确保固定关节连接到运动链而非世界坐标系
3. 使用pin.computeTotalMass()验证模型总质量是否包含预期部件的质量

结论

Pinocchio库的质心计算功能设计合理，但需要正确理解其运动质量与静态质量的区别。通过添加自由浮动基座，可以确保所有部件质量(包括固定关节连接的质量)被正确计入质心计算。这一解决方案不仅适用于Unitree Go2机器人，也适用于其他包含固定关节的机器人模型。