如何用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);

上面的代码包含在初始化设置代码中。在对update事件调用的另一种方法中，编写了以下代码行：

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();

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