如何使用具有碰撞几何体的 LQR 控制器

如何解决如何使用具有碰撞几何体的 LQR 控制器

我正在使用 examples/atlas/atlas_run_dynamics.cc，我想添加一个 LQR 控制器以使机器人站立。我添加我的代码

  int num_act,num_states;
  num_act = plant.num_actuators();
  num_states = plant.num_multibody_states();

  std::cout<<"num_actuators: "<<num_act<<std::endl;
  std::cout<<"num_multibody_states: "<<num_states<<std::endl;

  Eigen::MatrixXd Q = 10*Eigen::MatrixXd::Identity(num_states,num_states);
  Eigen::MatrixXd R = Eigen::MatrixXd::Identity(num_act,num_act);

  const InputPort<double>& actuation_port = plant.get_actuation_input_port();

  auto controller = systems::controllers::LinearQuadraticRegulator(
      plant,plant_context,Q,R,Eigen::Matrix<double,0>::Zero() /* No cross state/control costs */,actuation_port.get_index());

我得到了：

what(): System::FixInputPortTypeCheck(): expected value of type drake::geometry::QueryObject<drake::autodiffXd> for input port 'geometry_query' (index 0) but the actual type was drake::geometry::QueryObject<double>. (System ::plant)

以前的问题在这里：
https://stackoverflow.com/questions/65471497/how-are-you-supposed-to-use-a-controller-with-a-urdf/65471632#65471632
https://stackoverflow.com/questions/61626553/when-doing-direct-collocation-inputporteval-required-inputport0-geometr#comment109060518_61633651

但仍然不明白如何“将包含 MultibodyPlant 和 SceneGraph 的图表传递到 LQR 调用中”，并在这种情况下设置 assume_non_continuous_states_are_fixed=true。

有人可以提供一些具体的说明吗？谢谢！

更新：

对于it's slightly unnatural to call LQR for a system with contact，比较普通的方法是用轨迹优化的方法得到一个轨迹，包括关节力矩、接触力等，然后用lqr这样的控制器来控制系统，对?

但德雷克对我来说似乎很复杂。我不知道如何在具有浮动底座和接触点的模型上使用直接搭配，如何在直接搭配中添加质心动量约束，如何将关节轨迹和接触力应用于模型。我跑了acrobot/run_lqr,acrobot/run_swing_up_traj_optimization，但是他们没有浮动底座，也不涉及触点，此外，添加销接头也改变了浮动底座特性，对吗？

对于比较传统的软件，比如Vrep，我的理解是用一些轨迹优化的方法得到轨迹数据，用IK/ID得到关节输入，然后用一个控制器来控制多体模型。 Drake 似乎比我用过的其他软件更强大，但我有点迷失在文档和教程中。

解决方法

我实际上有一个 PR 可以更好地准确记录您所看到的错误：https://github.com/RobotLocomotion/drake/pull/15437 .

确实，LinearQuadraticRegulator 还没有 assume_non_continuous_states_are_fixed。那将很容易添加。真正需要发生的是我全面了解各种控制设计算法并标准化它们的选项。（到目前为止，我一直在一次改进他们一个请求）。我在这里打开了一个问题：https://github.com/RobotLocomotion/drake/issues/15464

但我们还没有那个特定选项的原因是，为有联系的系统调用 LQR 有点不自然。默认情况下，围绕某个状态对系统进行线性化将包括任何主动接触力的动力学，但将简单地忽略任何潜在接触（需要明确的是，这是数学的属性，而不是代码的限制）。这些接触动力学将是僵硬的，并且在原始坐标中几乎肯定是无法控制的。代码中也可能存在限制……并非所有接触几何图形都将提供分析梯度。但是我觉得atlas模型只使用了半平面上的点接触，所以应该是完全支持的。

当我在我的欠驱动讲座中展示了 Atlas 与 LQR 的平衡时，那个模型已经用脚趾处的销关节代替了接触动力学（我希望我清楚这一点！）。这是一个明显更合理的方法；这也是我们使用简化模型来设计轮式车辆/球形机器人的控制器而不是通过接触动力学进行线性化的原因。使用这些最小模型是腿运动的混合动力学方法的核心。