0

Background

I am trying to simulate a double pendulum mounted on a cart. I used the examples/multibody/cart_pole/cart_pole_simulation.cc as a starting point.

I created a new .sdf based on cart_pole.sdf and added for my double pendulum cart. I've verified (visually, through drake-visualizer) that the sdf file appears correct (changing the joint values moves the model as expected)

Due to the addition of a new joint, I added the following line to my source file (where mid_joint is the name of the joint between the first link of the pendulum and the second link of the pendulum.

  const RevoluteJoint<double>& mid_joint = tribot.GetJointByName<RevoluteJoint>("mid_joint");

  mid_joint.set_angle(&tribot_context, 0.0);

Problem

My program compiles correctly, but I get the following error when I try to run my system.

terminate called after throwing an instance of 'std::logic_error'
  what():  InputPort::Eval(): required InputPort[0] (geometry_query) of System ::_::drake/multibody/MultibodyPlant@0000560ea69178f0 (MultibodyPlant<double>) is not connected

Any pointers on what this error is pointing to would be greatly appreciated.

Thanks in advance

(PS:: If anyone could tell me how to more formally verify an sdf file, that would be helpful)

Solution (thanks to Russ's answer)

Below are the changes required to fix the error

-    SceneGraph<double>& scene_graph = *builder.AddSystem<SceneGraph>();
+    auto pair = AddMultibodyPlantSceneGraph(&builder, std::make_unique<MultibodyPlant<double>>(FLAGS_time_step));
+    MultibodyPlant<double>& plant = pair.plant;
+
+    SceneGraph<double>& scene_graph = pair.scene_graph;

-    MultibodyPlant<double>& plant =
-        *builder.AddSystem<MultibodyPlant>(FLAGS_time_step);

-    builder.Connect(
-            plant.get_geometry_poses_output_port(),
-            scene_graph.get_source_pose_port(plant.get_source_id().value()));
Community
  • 1
  • 1
Rufus
  • 5,111
  • 4
  • 28
  • 45

1 Answers1

2

I think that you're close. The reason that your code has departed from the examples/multibody/cart_pole/cart_pole_simulation.cc starting point is that your cartpole.sdf almost certainly added some collision geometry, which was not present in the original example. As such, your MultibodyPlant system needs to be connected to your SceneGraph object in the other direction, too... so that MultibodyPlant can ask geometry questions about collisions.

The simple solution is to change the few lines that create your MultibodyPlant/SceneGraph to look more like the example in e.g. examples/multibody/inclined_plant/inclined_plane_run_dynamics.cc.

I think we want to improve the experience of people that might use your workflow, and have opened https://github.com/RobotLocomotion/drake/issues/10874 .

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10