How are you supposed to use a controller with a URDF

Question

I'm trying to use the LQR Controller to move the wheels on this really simple URDF linked here

I'm trying to understand how to use the LQR controller for a model loaded from a URDF so this is a mix of the 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());

However, I get a runtime error:

  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)

Can someone explain how to use the LQR controller for a Model

Answer 1

There are a few problems here. The first, and the one generating the error, is the fact that you've passed a plant_context (only) into the LinearQuadraticRegulator. Because your system has collision geometry, you need a SceneGraph to be connected for the dynamics to be evaluated. You would want to pass the diagram containing both the MultibodyPlant and the SceneGraph into the LQR call.

But the real problem is going to be deeper than that. LQR is not going to work out of the box for you on this model. That's not a Drake issue, it's a math issue. The system you've described not only has collision dynamics, but also is not going to be controllable in the linearization. When people use LQR to stabilizing wheeled robots, they do it in a minimal coordinates which assumes that the wheels are attached to the ground at a point. In drake, that would mean writing your own LeafSystem with the vehicle dynamics.