你应该如何使用带有URDF的控制器

我试着用LQR控制器来移动这个非常简单的URDF链接的轮子 here

我正在尝试理解如何使用LQR控制器从一个URDF加载的模型,所以这是一个混合 LQR example PR2 Example

  systems::DiagramBuilder<double> builder;
  auto pair = AddMultibodyPlantSceneGraph(
      &builder,
      std::make_unique<MultibodyPlant<double>>(
          FLAGS_mbp_discrete_update_period));
  MultibodyPlant<double>& plant = pair.plant;

  const std::string full_name ="doublependulum/fetch/urdf/freight.urdf";
  auto parser = multibody::Parser(&plant);
  parser.package_map().PopulateFromFolder("/root/");
  parser.AddModelFromFile(full_name);

  // Add model of the ground.
  const double static_friction = 0.5;
  const Vector4<double> green(0.5, 1.0, 0.5, 1.0);
  plant.RegisterVisualGeometry(plant.world_body(), RigidTransformd(),
                               geometry::HalfSpace(), "GroundVisualGeometry",
                               green);
  // For a time-stepping model only static friction is used.
  const multibody::CoulombFriction<double> ground_friction(static_friction,
                                                           static_friction);
  plant.RegisterCollisionGeometry(plant.world_body(), RigidTransformd(),
                                  geometry::HalfSpace(),
                                  "GroundCollisionGeometry", ground_friction);

  plant.Finalize();
  plant.set_penetration_allowance(FLAGS_penetration_allowance);

  // Set the speed tolerance (m/s) for the underlying Stribeck friction model
  // For two points in contact, this is the maximum allowable drift speed at the
  // edge of the friction cone, an approximation to true stiction.
  plant.set_stiction_tolerance(FLAGS_stiction_tolerance);

  const drake::multibody::Body<double>& base = plant.GetBodyByName("base_link");


  ConnectContactResultsToDrakeVisualizer(&builder, plant);

  geometry::DrakeVisualizer::AddToBuilder(&builder, pair.scene_graph);
  auto diagram = builder.Build();

  /// Need to add actuators to the models in this form
  // Create a context for this system:
  std::unique_ptr<systems::Context<double>> diagram_context =
      diagram->CreateDefaultContext();
  systems::Context<double>& plant_context =
      diagram->GetMutableSubsystemContext(plant, diagram_context.get());

  Eigen::MatrixXd Q(2, 2);
  Q << 10, 0, 0, 1;
  Eigen::MatrixXd R(1, 1);
  R << 1;

  auto controller =
      builder.AddSystem(systems::controllers::LinearQuadraticRegulator(
          plant, plant_context, Q, R));

  builder.Connect(plant.get_state_output_port(),
                  controller->get_input_port());
  builder.Connect(controller->get_output_port(), plant.get_input_port());

但是,我得到一个运行时错误:

  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)

有人能解释一下如何对模型使用LQR控制器吗