Drake中AutoDiffXd多体系统质量矩阵求逆报错排查

问题根源 #

np.linalg.inv()和Python的**(-1)运算符仅支持numpy原生数值类型（如float64），无法识别Drake的AutoDiffXd自动微分类型，因此会触发类型转换错误。Drake的MatrixX<AutoDiffXd>基于Eigen库实现，自带完整的线性代数操作接口，无需依赖numpy。

解决方案 #

直接使用Drake矩阵对象的原生求逆方法，以下两种方式均可行：

方法1：直接调用inv()成员函数

适用于非奇异矩阵，代码简洁：

# 获取AutoDiffXd类型的质量矩阵
M = plant.CalcMassMatrixViaInverseDynamics(context)
# 调用Drake矩阵的inv()方法求逆
M_inv = M.inv()

方法2：基于LDLT分解的稳健求逆（推荐）

机械手的质量矩阵通常是对称正定的，使用LDLT分解求逆更稳定，还能处理数值奇异的边缘情况：

from pydrake.matrix import MatrixX

M = plant.CalcMassMatrixViaInverseDynamics(context)
# 对对称正定矩阵做LDLT分解
ldlt_decomp = M.ldlt()
# 通过求解单位矩阵得到逆矩阵
M_inv = ldlt_decomp.solve(MatrixX[AutoDiffXd].Identity(M.rows(), M.cols()))

验证说明 #

上述两种方法都会保留AutoDiffXd类型的自动微分信息，后续可以直接用于机械手动力学方程的自动微分计算，完全兼容Drake的多体系统框架。

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