CODEWITHSUNDEEP
drake
Rufus
Rufus

Reputation: 5566

"This method only supports T = double" when creating LQR for MultibodyPlant<double>

I am creating an LQR controller with the code below:

systems::controllers::LinearQuadraticRegulator(
            plant, context, Q, R,
            Eigen::Matrix<double, 0, 0>::Zero() /* No cross state/control costs */,
            actuation_port_index);

Debugging with gdb shows that the error comes from multibody_plant.cc

MultibodyPlant<T>::CalcPointPairPenetrations

I'm not sure why it's not using MultibodyPlant<double>::CalcPointPairPenetrations

In case it matters, my plant object was loaded from an sdf file with the following lines

    auto pair = AddMultibodyPlantSceneGraph(&builder, std::make_unique<MultibodyPlant<double>>(FLAGS_time_step));

    MultibodyPlant<double>& plant = pair.plant;

    const std::string model_filename = getSrcDir() + "/../res/plant.sdf";
    Parser(&plant, &scene_graph).AddModelFromFile(model_filename);

Upvotes: 1

Views: 170

Answers (2)

Russ Tedrake
Russ Tedrake

Reputation: 5533

While I agree that the error message is a bit obtuse, I suspect that the problem is real... I'll bet you are modeling a system that has contact dynamics (perhaps by accident) and LQR doesn't know how to deal with the non-smooth mechanics.

When you pass in a System<double> to the LQR method, it converts it with ToAutoDiffXd(), then evaluates the dynamics (to get the linearization). That's why the error is about MultibodyPlant<AutoDiffXd> (not double). For your system, the dynamics require calling the collision engine, which is not fully differentiable (yet), so that error message is thrown.

Setting the time_step=0 changes from a time-stepping model to a continuous time model and does change the interaction with the collision engine. So it is possible that can help in some cases.

But the solution, I think, is to decide if you are including the collision elements only accidentally (via your SDF) -- e.g. you're not actually trying to implement physics. If yes, try commenting them out in your SDF to get around this. Perhaps we could/should offer some options that make it easier to have LQR ignore these.

UPDATE: I've opened https://github.com/RobotLocomotion/drake/issues/11120 to make this slightly better.

Upvotes: 2

asQue
asQue

Reputation: 49

I have the same behavior when FLAGS_time_step is greater than zero.

Setting FLAGS_time_step to 0.0 resolves the issue for me.

Thought, it looks like a bug.

Upvotes: 0

Related Questions