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

EN

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

先前的问题如下：

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似乎比我使用的其他软件更强大，但我有点迷失在文档和教程中。