如何使用 autoDiffToGradientMatrix 求解 drake 中的科里奥利矩阵？

Question

我正在尝试根据我在网上找到的以下方法为我的机器人获取科里奥利矩阵（控制器明确需要矩阵）：

plant_.CalcBiasTerm(*context, &Cv_);
auto jac = autoDiffToGradientMatrix(Cv_);
C = 0.5*jac.rightCols(n_v_);

其中 Cv_, plant_, context 是 AutoDiffXd，n_v_ 是广义速度的数量。所以基本上我有一个 62 关节机器人从 URDF 加载到 drake 中，它是一个自由体（浮动基础系统）。完成机器人后，我使用 DiagramBuilder.Build() 方法，然后使用 CreateDefaultContext() 来获取上下文。接下来，我尝试像这样设置 AutoDiff 环境：

plant_autodiff = drake::systems::System<double>::ToAutoDiffXd(*multibody_plant);
context_autodiff = plant_autodiff->CreateDefaultContext();
context_autodiff->SetTimeStateAndParametersFrom(*diagram_context);

以上代码包含在初始化设置代码中。在另一个方法中，在更新事件上调用，编写了以下代码行：

drake::AutoDiffVecXd c_auto_diff_ = drake::AutoDiffVecXd::Zero(62);
plant_autodiff->CalcBiasTerm(*context_autodiff, &c_auto_diff_);
MatrixXd jac = drake::math::autoDiffToGradientMatrix(c_auto_diff_);
auto C = 0.5*jac.rightCols(jac.size());

此设置编译并运行，但是 jac 矩阵的大小为 0，而我期望为 62x62。我也在提取然后公开科里奥利向量，它是 62x1，似乎或多或少是正确的。 c_auto_diff_ 变量也是 62x1，但是所有元素都是 0。明明是我弄错了，就是不知道具体错在哪里。

感谢任何帮助，

谢谢大家，

罗伯特

Answer 1

你很接近。您需要告诉 autodiff 管道您想要对什么进行导数。在这种情况下，我相信你想要

auto v = drake::math::initializeAutoDiff(Eigen::VectorXd::Zero(62))
plant_autodiff->SetVelocities(context_autodiff.get(), v);

通过调用 initializeAutoDiff，您将自动微分项初始化为单位矩阵，这表示您想要对 v 求导。那么你应该得到非零导数。

顺便说一句 - 我通常会使用

plant_autodiff = multibody_plant->ToAutoDiffXd();

但我想你所拥有的一定也有用！

如何使用 autoDiffToGradientMatrix 求解 drake 中的科里奥利矩阵？

How to use autoDiffToGradientMatrix to solve for Coriolis Matrix in drake?

drake