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

Question

我正在使用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, 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)

之前的问题在这里：
/>

但仍然不明白如何“将包含 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，但是他们没有浮动底座，不涉及触点，另外，加了一个pin joint也改变了浮动底座的特性吧？
对于比较传统的软件，比如Vrep，我的理解是用一些轨迹优化的方法得到轨迹数据，然后用IK/ID得到关节输入，然后用控制器控制多体模型。 Drake 似乎比我使用的其他软件更强大，但我有点迷失在文档和教程中。

Answer 1

实际上我有一个 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 时，该模型已经用脚趾处的销接头取代了接触动力学（我希望我对此很清楚！）。这是一种更为合理的方法；这也是我们使用简化模型来设计轮式车辆/球形机器人控制器而不是通过接触动力学进行线性化的原因。使用这些最小模型是腿部运动混合动力学方法的核心。

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

How to use an LQR controller with collision geometry

c++

robotics

drake