InverseDynamicsController与InverseDynamics输出异常问题求助

观测到的行为 #

• 机械臂未尝试趋近固定的desired_state，轨迹呈现类似无重力补偿的随机掉落状态
• InverseDynamicsController输出的generalized_force不符合预期

预期行为 #

将机械臂的desired_state固定为初始位置（速度为0），desired_acceleration设为0，期望机械臂保持初始位置不动

已完成的排查工作 #

• 确认控制器连接拓扑正确：InverseDynamicsController的generalized_force输出已连接到HardwareStation的actuation端口
• 验证HardwareStation的estimated_state、PidController输出及Adder输出均正常
• 机械臂挂载在关节锁定的移动基座上（即使将基座焊死在世界坐标系中，问题仍存在），无额外外力作用，理论上逆动力学仅需输出重力补偿力即可维持位置
• 已定位问题出在InverseDynamics的CalcOutputForce()函数，但查阅MultibodyTree<T>::CalcInverseDynamics()未找到有效线索

问题求助 #

请问InverseDynamics模块可能存在哪些问题？

内容的提问来源于stack exchange，提问作者Michael Zeng