CODEWITHSUNDEEP
drake
Rufus
Rufus

Reputation: 5566

FixInputPort attempts to connect wrong port

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>>());
...

Upvotes: 0

Views: 119

Answers (2)

Rufus
Rufus

Reputation: 5566

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!

Upvotes: 0

Russ Tedrake
Russ Tedrake

Reputation: 5533

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.

Upvotes: 2

Related Questions