0

I am trying to port the scripts I made for the double pendulum to the acrobot (moving towards more underactuation). While doing this, I saw that the LQR Controller script (inspired mainly from this notebook from the underactuated course), gave errors during linearization/creating LQR Controller. The code is identical except for the changes for a single actuator instead of 2.

What is strange is that the error is about connecting geometry ports during the Linearization/creating LQR command which does not appear when using the double pendulum. The error given during linearization/LQR is:

RuntimeError: The geometry query input port (see MultibodyPlant::get_geometry_query_input_port()) of this MultibodyPlant is not connected. Please connect thegeometry query output port of a SceneGraph object (see SceneGraph::get_query_output_port()) to this plants input port in a Diagram.

I assumed that since the script worked for the double pendulum, it would work for the acrobot but I seemed to have probably made a mistake I guess. Commenting out the lines for LQR and its connections in the diagram makes the simulation run with the given initial conditions, which makes it even more confusing to me. I can't inspect the diagram visualization as the error appears before I can run builder.Build().

A google colab notebook to reproduce the error can be seen here. The first section has the acrobot script with the error and the second section is the same script for the double pendulum which runs as expected.

Edit: Another method which I tried but didn't work:

builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
plant = builder.AddSystem(MultibodyPlant(0.001))
plant.RegisterAsSourceForSceneGraph(scene_graph)


# Load the Acrobot description from URDF file
parser = Parser(plant)
# parser = Parser(plant, scene_graph)
parser.AddModelFromFile("Acrobot.urdf")

# # Reduce/remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0,0])

plant.Finalize()
assert plant.geometry_source_is_registered()


builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port())
builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id()))

Here, I explicitly connect the plant's geometry query input port to the scene graph's query output port but still arrive at the same error during linearization.

Shubham Vyas
  • 163
  • 10
  • I haven't checked your code, but it looks like you have introduced new collision geometry into the problem, which means that MultibodyPlant now needs to be connected to a SceneGraph (which is the collision engine). – Russ Tedrake Dec 13 '20 at 02:26
  • I assumed that the `AddMultibodyPlantSceneGraph()` function handled these connections. For example, there is collision geometry and collision in the one_d_hopper exercise in Chapter 4 of the Underactuated Robotics course but the diagram builder there only uses the `AddMultibodyPlantSceneGraph()` function. – Shubham Vyas Dec 13 '20 at 16:43
  • Furthermore, if I follow the error suggestion and run `builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port())`, then it raises an error saying `RuntimeError: Input port is already wired.`. I thought that this is done by `AddMultibodyPlantSceneGraph()`. – Shubham Vyas Dec 13 '20 at 18:26
  • In that case, are you passing in a `Context` that does not have the `SceneGraph` attached? You'll want to create the context of the diagram, and then pass in the subcontext... (e.g with `GetMyContextFromRoot`). – Russ Tedrake Dec 13 '20 at 20:03
  • I'll check this. However, since the error occurs at adding the `LinearQuadraticRegulator` system which is done before the diagram is built, the plant context cannot be obtained from the diagram context using `GetMyContextFromRoot` as the diagram context doesn't exist yet. The context is created using `plant.CreateDefaultContext()` after wiring the plant to the scene graph. – Shubham Vyas Dec 13 '20 at 21:05
  • That's the problem. The context that you get from `plant.CreateDefaultContext` does not have the input ports connected. – Russ Tedrake Dec 14 '20 at 01:14
  • 1
    That makes sense indeed. Is there then a way to create context from a diagram with the LQR controller inside it before building the diagram? I guess it then makes sense to create a diagram with just the MultibodyPlant and Scene_graph and expose the ports required for LQR and then create another diagram that connects the built diagram with LQR using the context of the built diagram? – Shubham Vyas Dec 14 '20 at 10:17

1 Answers1

1

One solution to my problem was removing the collision geometries from the urdf file. The linearization was successful using either Linearize or LinearQuadraticRegulator functions.

Update: I now understand a bit more:

Drake uses scene_graph for collision checking/simulation and hence if collision geometries are used in the urdf, scene_graph needs to be connected to the geometry query input port to communicate information about collisions to the plant. For the Linearization/LQR, if the collision geometry is in the urdf, it expects this port to be connected for simulation (naturally). But since only the plant is passed into the Linearization/LQR function, it does not know about this as the scene_graph is not present.

For my use-case, I do not have any collisions at the points where I want to Linearize/control using LQR (also, generally it is probably a bad idea to use LQR when collisions are involved). So, since no collisions are simulated, I simply removed the collision geometry parts in the urdf used for Linearization. I include them for other purposes such as Trajectory Optimization and simulation.

Shubham Vyas
  • 163
  • 10