Drake中SpatialAcceleration能否与Eigen混用及类型转换方法

数据老陈

2026-9-25

Drake中SpatialAcceleration与Eigen向量混用方法

类型转换核心方法

Drake的SpatialAcceleration类内置了直接导出为Eigen向量的接口，无需额外实现转换逻辑：

调用get_coeffs()方法可直接返回Eigen::Matrix<double, 6, 1>类型的向量，向量元素顺序为角速度偏置在前，线速度偏置在后，和增广雅可比的常规排列顺序一致
也可以调用rotational()和translational()方法分别提取3维角速度偏置、线速度偏置的Eigen::Vector3d向量

代码示例

1. 整体转换后运算

// 先计算偏置空间加速度
multibody::SpatialAcceleration<double> mdJ_Aug_dq_robot1 = mplant.CalcBiasSpatialAcceleration(*mContext,
                                  multibody::JacobianWrtVariable::kQDot,
                                  *mFrame_EE,
                                  Eigen::Vector3d::Zero(),
                                  mplant.world_frame(),
                                  mplant.world_frame()
                                  );
// 转换为6维Eigen向量
Eigen::Matrix<double, 6, 1> Jdot_qdot = mdJ_Aug_dq_robot1.get_coeffs();
// 直接和其他6维Eigen向量做运算
Eigen::Matrix<double, 6, 1> acc_diff = your_eigen_6d_vector - Jdot_qdot;

2. 单独提取分量

Eigen::Vector3d angular_bias = mdJ_Aug_dq_robot1.rotational();
Eigen::Vector3d translational_bias = mdJ_Aug_dq_robot1.translational();
// 按需自行拼接为6维向量
Eigen::Matrix<double, 6, 1> custom_Jdot_qdot;
custom_Jdot_qdot.head<3>() = angular_bias;
custom_Jdot_qdot.tail<3>() = translational_bias;