1

I encounter the error message when doing DirectCollocation with collision geometry:

RuntimeError: InputPort::Eval(): required InputPort[0] (geometry_query) of System ::drake/multibody/MultibodyPlant@0000000003550740 (MultibodyPlant<drake::AutoDiffXd>) is not connected

Based on the previous question, I have added both connections as follows, but still not fix the bug.

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

This is my code skeleton:

builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Make and add the model
tmp = make_acrobot_plant(params, finalize, scene_graph)
plant = builder.AddSystem(tmp)
plant.Finalize()

# https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html
builder.Connect(plant.get_geometry_poses_output_port(),
                scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
                plant.get_geometry_query_input_port())

num_time_samples = 40
dircol = DirectCollocation(
    plant,
    context,
    num_time_samples=num_time_samples,
    minimum_timestep=0.01,
    maximum_timestep=0.05,
    input_port_index=plant.get_actuation_input_port().get_index())
dircol.AddEqualTimeIntervalsConstraints()
u = dircol.input()
dircol.AddRunningCost(Ru * u.dot(u))
dircol.SetInitialTrajectory(traj_init_u, traj_init_x)
result = Solve(dircol)
assert result.is_success()

# Connect
# Simulate u
controls = builder.AddSystem(TrajectorySource(u_trajectory))
builder.Connect(
    controls.get_output_port(0), plant.get_actuation_input_port())
'''
# Simulate x
source = builder.AddSystem(TrajectorySource(x_trajectory))
pos_to_pose = builder.AddSystem(
    MultibodyPositionToGeometryPose(plant, input_multibody_state=True))
builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port())
builder.Connect(pos_to_pose.get_output_port(),
                scene_graph.get_source_pose_port(plant.get_source_id()))
'''
# ...
# Visualization:
builder.Connect(scene_graph.get_pose_bundle_output_port(),
                planar_scene_graph_vis.get_input_port(0))
diagram = builder.Build()
Shinnnn
  • 99
  • 5

1 Answers1

1

You are close, but you have only passed the plant into DirectCollocation. You need to pass the diagram with plant+scenegraph into DirectCollocation. (And you will also need to assume_non_continuous_states_are_fixed=true to ignore the SceneGraph's abstract state).

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
  • Could you explain more about passing `diagram`? I tried `diagram = builder.Build()` and pass `diagram` into the 1st argument of `DirectCollocation` but it complains `System::get_input_port(): there is no input port`. Should I add input to diagram? (I think the scenegraph is already connected to the `plant` since I pass it into `make_acrobot_plant`.) – Shinnnn May 06 '20 at 15:27
  • I happen to have written a similar test in c++ recently for `FittedValueIteration`, but the workflow is basically the same: https://github.com/RobotLocomotion/drake/blob/6761809c64237d54ac8a8bb37d4daccdbe21fb55/systems/controllers/test/dynamic_programming_test.cc#L205 – Russ Tedrake May 07 '20 at 10:49