如何将控制器与URDF一起使用

Question

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

我正在尝试理解如何对从URDF加载的模型使用LQR控制器，因此这是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控制器吗

Answer 1

这里有一些问题。第一个，也是产生错误的一个，是您向LinearQuadraticRegulator传递了一个plant_context (仅限)。由于系统具有碰撞几何体，因此需要连接SceneGraph才能对动力学进行求值。您可能希望将包含MultibodyPlant和SceneGraph的图表传递给LQR调用。

但真正的问题将会比这更深。在此模型上，LQR不会为您提供开箱即用的功能。这不是德雷克的问题，这是数学问题。你所描述的系统不仅有碰撞动力学，而且在线性化过程中也是不可控的。当人们使用LQR来稳定轮式机器人时，他们是在最小坐标下进行的，这假设轮子在某一点上与地面相连。在德雷克，这将意味着编写自己的LeafSystem与车辆动力学。