How to use an LQR controller with collision geometry

I am using the examples/atlas/atlas_run_dynamics.cc, and I want to add an LQR controller to make the robot stand. I add my code

  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());

and I got:

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)

Previous questions are here:
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

But still don't understand how to "pass a diagram containing both the MultibodyPlant and the SceneGraph into the LQR call", and set assume_non_continuous_states_are_fixed=true in this case.

can someone offer some specific instruction？ thanks！

updates：

For it's slightly unnatural to call LQR for a system with contact, the more ordinary way is to use a trajectory optimization method to get a trajectory, including joint torque, contact force, etc, and then use a controller, like lqr, to control the system, right?

But drake seems so complicated to me. I don't know how to use direct collocation on a model with floating base and contact points, how to add centroidal momentum constraints to the direct collocation, how to apply the joint trajectory and contact force to the model. I ran acrobot/run_lqr, acrobot/run_swing_up_traj_optimization, but they don't have a floating base, and are not involve with contacts, besides, adding a pin joint also changed the floating base feature, right?

For more traditional software, like Vrep, my understanding is that use some trajectory optimization method to get trajectory data, and use IK/ID to get joint input, then use a controller to control the multibody model. Drake seems more powerful than other software I used, but I am kind of lost in the documents and tutorials.

Solution

I actually have a PR open to better document exactly the error that you are seeing: https://github.com/RobotLocomotion/drake/pull/15437 .

It's true that the LinearQuadraticRegulator does not have assume_non_continuous_states_are_fixed yet. That would be easy to add. What really needs to happen is that i take a full sweep over the various control design algorithms and standardize their options. (so far I've been improving them one request at a time). I've opened an issue here: https://github.com/RobotLocomotion/drake/issues/15464

But the reason we don't have that particular option yet is that it's slightly unnatural to call LQR for a system with contact. By default, linearizing the system around some state will include the dynamics of any active contact forces, but will simply ignore any potential contacts (to be clear, this is a property of the math, not a limitation of the code). Those contact dynamics will be stiff and almost certainly uncontrollable in the original coordinates. There may be limitations in the code, too... not all contact geometries will give analytical gradients yet. But I think the atlas model uses only point contacts on a half-plane, so should be fully supported.

When I've shown the Atlas balancing with LQR in my underactuated lectures, that model had replaced the contact dynamics with a pin joint at the toe (I hope I was clear about that!). This is a dramatically more reasonable approach; it's also the reason we use reduced models for designing controllers for wheeled vehicles / ball bots rather than linearizing through the contact dynamics. And using these minimal models is at the core of the hybrid dynamics approach to legged locomotion.