FixInputPort attempts to connect wrong port

Question

I have a custom LeafSystem that has input ports and output ports as follows

template <typename T>
MyLeafSystem<T>::MyLeafSystem() :
    systems::LeafSystem<T>(systems::SystemTypeTag<MyLeafSystem>{}),
    input1_idx(this->DeclareVectorInputPort("input1", systems::BasicVector<T>(1)).get_index()),
    input2_idx(this->DeclareVectorInputPort("input2", systems::BasicVector<T>(1)).get_index()),
    output_idx(this->DeclareVectorOutputPort("output", systems::BasicVector<T>(2), &TorqueCombiner::convert).get_index())
{}

I add it to my system via

auto my_leaf_system = builder.AddSystem(std::make_unique<MyLeafSystem<double>>());
// Connect one of the input ports of my_leaf_system
// Connect one of the output port of my_leaf_system
auto diagram = builder.Build();

For the remaining port, I wish to connect fix the input port for now. I do so via

auto diagram = builder.Build();

std::unique_ptr<systems::Context<double>> diagram_context = diagram->CreateDefaultContext();
Context<double>& context = diagram->GetMutableSubsystemContext(plant, diagram_context.get());

context.FixInputPort(my_leaf_system->get_second_input_port().get_index(), Vector1d::Zero());

However, it throws a std::logic_error when I run FixInputPort

terminate called after throwing an instance of 'std::logic_error'
  what():  System::FixInputPortTypeCheck(): expected value of type drake::geometry::QueryObject<double> for input port 'geometry_query' (index 0) but the actual type was drake::systems::BasicVector<double>. (System ::_::drake/multibody/MultibodyPlant@000055e087146ec0)

The strange thing is

int(my_leaf_system->get_second_input_port().get_index())) == 0

and

int(plant.get_geometry_query_input_port().get_index())) == 0

My plant is added as follows

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

drake::multibody::MultibodyPlant<double>& plant = pair.plant;
drake::geometry::SceneGraph<double>& scene_graph = pair.scene_graph;

// Make and add the model.
drake::multibody::Parser(&plant, &scene_graph).AddModelFromFile(model_filename);

// Connect scene graph to visualizer
drake::geometry::ConnectDrakeVisualizer(&builder, scene_graph);

plant.Finalize();



auto my_leaf_system = builder.AddSystem(std::make_unique<MyLeafSystem<double>>());
...

Updated title to better reflect the source of my problem – Rufus May 13 '19 at 03:04 — Rufus, May 13 '19 at 03:04

score 2 · Answer 1 · answered May 12 '19 at 09:54

2

There are several inconsistent thing with your post. For starters, the error message implies that the error is generated when attempting to fix an input port of multibodyplant (not your leaf system). And the type of that input port is not a vector, so the error is clear.

Is suspect that you’ve crossed your wires (or pointers) somewhere. Fixing a vector input port of a derived leafsystem should be aok — it’s extremely common.

answered May 12 '19 at 09:54

Russ Tedrake

4,703
1
7
10

Upon further debugging, I found out that `int(my_leaf_system->get_second_input_port().get_index())) == int(plant.get_geometry_query_input_port().get_index())) == 0`, I don't understand why that would happen.. – Rufus May 12 '19 at 15:58
In case it isn't already clear -- input ports are numbered from 0 in each subsystem. – Sherm May 12 '19 at 19:46
I must be missing something.. If both of them are 0, how does `context.FixInputPort(0, Vector1d::Zero())` know whether it is to connect `my_leaf_system.input2` or to connect `plant.get_geometry_query_input_port()`? – Rufus May 13 '19 at 02:24
I think my concept of `Context` is messed up. I believe I should be using `my_leaf_system_context.FixInputPort` or `my_leaf_system->get_second_input_port().FixValue(my_leaf_system_context, ...)` (btw, is there any difference between the 2?) – Rufus May 13 '19 at 02:49

score 0 · Answer 2 · answered May 13 '19 at 03:01

The error I made is in

Context<double>& context = diagram->GetMutableSubsystemContext(plant, diagram_context.get());

context.FixInputPort(my_leaf_system->get_second_input_port().get_index(), Vector1d::Zero());

Here, I am using the context for the plant subsystem to FixInputPort on my_leaf_system.

The correct code should be

Context<double>& my_leaf_system_context = diagram->GetMutableSubsystemContext(*my_leaf_system, diagram_context.get());

my_leaf_system_context.FixInputPort(my_leaf_system->get_mip_input_port().get_index(), Vector1d::Zero());

Thx Russ, Sherm!

FixInputPort attempts to connect wrong port

2 Answers2