如何用MultibodyPlant公开API计算机械臂末端空间力（无需仿真）

核心思路 #

避开伪逆的低效计算，直接利用MultibodyPlant的公开逆动力学、雅可比计算接口，结合数值稳定的线性代数求解器映射关节广义力到操作空间力，同时支持自动求导框架。

具体实现步骤（C++示例） #

假设你已完成MultibodyPlant的构建与Finalize()，并拥有对应Context实例：

1. 设置机械臂状态
   为逆动力学计算提供关节位置、速度、加速度：

   const Eigen::VectorXd q = ...; // 关节位置
   const Eigen::VectorXd v = ...; // 关节速度
   const Eigen::VectorXd a = ...; // 关节加速度（对应末端期望加速度的映射值）
   
   plant->SetPositions(context.get(), q);
   plant->SetVelocities(context.get(), v);
   plant->SetAccelerations(context.get(), a);
   

2. 计算末端连杆的空间雅可比
   使用CalcBodyJacobianSpatialVelocity获取末端连杆相对于世界坐标系的空间雅可比（6×7矩阵，对应末端6维空间运动与7关节速度的映射）：

   const Body<double>& end_effector = plant->GetBodyByName("end_effector_link");
   Eigen::MatrixXd J_spatial(6, plant->num_velocities());
   
   plant->CalcBodyJacobianSpatialVelocity(
       *context,
       JacobianWrtVariable::kV,
       end_effector,
       Eigen::Vector3d::Zero(), // 力的作用点（末端连杆质心，可改为工具点坐标）
       plant->world_frame(),
       plant->world_frame(),
       &J_spatial);
   

3. 计算关节空间逆动力学广义力
   调用公开APICalcInverseDynamics计算关节所需的广义力tau：

   Eigen::VectorXd tau(plant->num_velocities());
   plant->CalcInverseDynamics(*context, &tau);
   

4. 映射为操作空间空间力
   由于7自由度机械臂的雅可比列满秩（6×7），使用QR分解求解J_spatial^T * F = tau的最小二乘解，比直接计算伪逆更高效且数值稳定：

   Eigen::VectorXd F = J_spatial.transpose().fullPivHouseholderQr().solve(tau);
   

优化：直接计算操作空间逆动力学项（避免关节力映射） #

如果需要更高效的计算（尤其是求导场景），可以直接计算操作空间的惯性力、科氏力、重力项：

• 操作空间惯性矩阵：通过关节空间惯性矩阵M和雅可比计算M_x = (J_spatial * M.inverse() * J_spatial.transpose()).inverse()
• 科氏力与重力项：计算关节空间的科氏力+重力项Cv_plus_G，再映射到操作空间Cx_plus_Gx = J_spatial.transpose().fullPivHouseholderQr().solve(Cv_plus_G)
• 最终操作空间力：F = M_x * a_des + Cx_plus_Gx（其中a_des是末端期望空间加速度）

对应的公开API调用：

// 计算关节空间惯性矩阵
Eigen::MatrixXd M(plant->num_velocities(), plant->num_velocities());
plant->CalcMassMatrix(*context, &M);

// 计算科氏力+重力项
Eigen::VectorXd Cv_plus_G(plant->num_velocities());
plant->CalcBiasTerm(*context, &Cv_plus_G);

自动求导支持 #

若要对模型参数求导，只需将double替换为AutoDiffXd类型，上述所有API均支持自动求导，无需额外修改，计算效率远高于手动对伪逆求导。

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