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

Question

我正在尝试使用 LQR 控制器在这个非常简单的 URDF 链接上移动轮子 here

我正在尝试了解如何将 LQR 控制器用于从 URDF 加载的模型，因此这是 LQR example and the 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 控制器用于模型

Answer 1

这里有一些问题。第一个，也是产生错误的一个，是您将 plant_context（仅）传递给 LinearQuadraticRegulator。因为您的系统具有碰撞几何体，所以您需要连接 SceneGraph 才能评估动力学。您可能希望将包含 MultibodyPlant 和 SceneGraph 的图表传递到 LQR 调用中。

但真正的问题远不止于此。 LQR 不会在这个模型上为您开箱即用。这不是德雷克的问题，这是一个数学问题。您所描述的系统不仅具有碰撞动力学，而且在线性化方面也不可控。当人们使用 LQR 来稳定轮式机器人时，他们是在一个最小坐标下进行的，假设轮子在一个点上附着在地面上。在 drake 中，这意味着用车辆动力学编写您自己的 LeafSystem。

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

How are you supposed to use a controller with a URDF

c++

drake