CODEWITHSUNDEEP
drake
Shubham Vyas
Shubham Vyas

Reputation: 165

Visualization Error Shown during Linearization/LQR for Acrobot but not for Double Pendulum

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.

Upvotes: 0

Views: 166

Answers (1)

Shubham Vyas
Shubham Vyas

Reputation: 165

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.

Upvotes: 1

Related Questions