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

如何解决如何使用 autoDiffToGradientMatrix 求解德雷克中的科里奥利矩阵？

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

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

其中 Cv_,plant_,context 是 AutoDiffXd，n_v_ 是广义速度的数量。所以基本上我有一个从 URDF 加载到 drake 的 62 关节机器人，这是一个自由体（浮动底座系统）。完成机器人后，我使用 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。 我显然犯了一个错误，但我不知道具体在哪里。

感谢任何帮助，

谢谢大家，

罗伯特

解决方法

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

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

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

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

plant_autodiff = multibody_plant->ToAutoDiffXd();

但我想你所拥有的也必须有效！