Question

我正在使用以下代码创建LQR控制器：

systems::controllers::LinearQuadraticRegulator(
            plant, context, Q, R,
            Eigen::Matrix<double, 0, 0>::Zero() /* No cross state/control costs */,
            actuation_port_index);

plant是类型MultibodyPlant<double>的引用
context是类型Context<double>的引用
Q的类型为Eigen::Matrix<double, 6, 6>
R的类型为Vector1d

使用gdb进行调试表明该错误来自multibody_plant.cc

MultibodyPlant<T>::CalcPointPairPenetrations

我不确定为什么不使用MultibodyPlant<double>::CalcPointPairPenetrations

万一重要，我的plant对象是从sdf文件中加载的，行如下：

    auto pair = AddMultibodyPlantSceneGraph(&builder, std::make_unique<MultibodyPlant<double>>(FLAGS_time_step));

    MultibodyPlant<double>& plant = pair.plant;

    const std::string model_filename = getSrcDir() + "/../res/plant.sdf";
    Parser(&plant, &scene_graph).AddModelFromFile(model_filename);

Answer 1

虽然我同意错误消息有点令人费解，但我怀疑问题确实存在。我敢打赌，您正在建模的系统具有接触动力学（可能是偶然的），而LQR不知道如何处理不流畅的力学问题。

当您将System<double>传递给LQR方法时，它将用ToAutoDiffXd()进行转换，然后评估动力学（以获得线性化）。这就是为什么错误是关于MultibodyPlant<AutoDiffXd>（而不是double）的原因。对于您的系统，动力学要求调用碰撞引擎（尚无法完全区分（尚未）），从而引发错误消息。

设置time_step=0从时间步长模型更改为连续时间模型，并且确实更改了与碰撞引擎的交互。因此在某些情况下可能会有所帮助。

但是，我认为解决方案是确定是否只是偶然地（通过SDF）包括碰撞元素-例如您实际上并没有尝试实现物理学。如果是，请尝试在SDF中将它们注释掉，以解决此问题。也许我们可以/应该提供一些选择，使LQR忽略这些选择更容易。

更新：我已经打开https://github.com/RobotLocomotion/drake/issues/11120来使其变得更好一些。

Answer 2

当FLAGS_time_step大于零时，我有相同的行为。

将FLAGS_time_step设置为0.0可以为我解决问题。

以为，它看起来像个错误。

为MultibodyPlant <double>创建LQR时，“此方法仅支持T = double”

2 个答案: