Add CheckSubgraphInvariants

multibody/parsing/detail_multibody_plant_subgraph.cc

@@ -20,6 +20,12 @@ std::vector<const T*> GetPlantAggregate(int num_items, GetFunction get_func) {
  }
  return items;
 }
+std::vector<ModelInstanceIndex> GetModelInstances(
+ const MultibodyPlant<double>& plant) {
+ std::vector<ModelInstanceIndex> items(plant.num_model_instances());
+ std::iota(items.begin(), items.end(), ModelInstanceIndex(0));
+ return items;
+}
 
 std::vector<const Body<double>*> GetBodies(
  const MultibodyPlant<double>& plant) {
@@ -321,8 +327,76 @@ void MultibodyPlantElementsMap::CopyGeometryById(
  geometry_ids_.insert({geometry_id_src, geometry_id_dest});
 }
 
-// TODO(azeey) implement this.
-void CheckSubgraphInvariants(const MultibodyPlantElements&){}
+template <typename T, typename GetFunction>
+void CheckPlantAggregate(GetFunction get_func, const T* item) {
+ DRAKE_DEMAND(get_func(item->index()) == item);
+}
+
+template <typename Item>
+bool ContainsItem(const std::set<Item>& container, const Item& item){
+ return std::find(container.begin(), container.end(), item) != container.end();
+}
+
+void CheckSubgraphInvariants(const MultibodyPlantElements& elem){
+ auto plant_model_instances = GetModelInstances(*elem.plant());
+ DRAKE_DEMAND(std::includes(
+ plant_model_instances.begin(), plant_model_instances.end(),
+ elem.model_instances().begin(), elem.model_instances().end()));
+
+ const MultibodyPlant<double> *plant = elem.plant();
+ // Check bodies.
+ for (const auto *body : elem.bodies()){
+ CheckPlantAggregate([&](auto i) { return &plant->get_body(i); }, body);
+ DRAKE_DEMAND(ContainsItem(elem.model_instances(), body->model_instance()));
+ }
+
+ // Check frames.
+ for (const auto *frame : elem.frames()){
+ CheckPlantAggregate([&](auto i) { return &plant->get_frame(i); }, frame);
+ DRAKE_DEMAND(ContainsItem(elem.bodies(), &frame->body()));
+ DRAKE_DEMAND(ContainsItem(elem.model_instances(), frame->model_instance()));
+ }
+
+ // Check joints.
+ for (const auto *joint : elem.joints()){
+ CheckPlantAggregate([&](auto i) { return &plant->get_joint(i); }, joint);
+ IsJointSolelyConnectedTo(joint, elem.bodies());
+ DRAKE_DEMAND(ContainsItem(elem.model_instances(), joint->model_instance()));
+ }
+
+ // Check actuators.
+ for (const auto *joint_actuator : elem.joint_actuators()){
+ CheckPlantAggregate([&](auto i) { return &plant->get_joint_actuator(i); },
+ joint_actuator);
+ DRAKE_DEMAND(ContainsItem(elem.joints(), &joint_actuator->joint()));
+ DRAKE_DEMAND(
+ ContainsItem(elem.model_instances(), joint_actuator->model_instance()));
+ }
+
+ // # Check geometries.
+ // if scene_graph is not None:
+ // assert plant.geometry_source_is_registered()
+ // inspector = scene_graph.model_inspector()
+ // for geometry_id in elem.geometry_ids:
+ // assert isinstance(geometry_id, GeometryId)
+ // frame_id = inspector.GetFrameId(geometry_id)
+ // body = plant.GetBodyFromFrameId(frame_id)
+ // assert body in elem.bodies
+ // else:
+ // assert elem.geometry_ids == set(), elem.geometry_ids
+ // Check geometries.
+ if (elem.scene_graph() != nullptr) {
+ DRAKE_DEMAND(plant->geometry_source_is_registered());
+ const auto &inspector = elem.scene_graph()->model_inspector();
+ for (const auto geometry_id: elem.geometry_ids()){
+ auto frame_id = inspector.GetFrameId(geometry_id);
+ auto body = plant->GetBodyFromFrameId(frame_id);
+ DRAKE_DEMAND(ContainsItem(elem.bodies(), body));
+ }
+ } else {
+ DRAKE_DEMAND(elem.geometry_ids().empty());
+ }
+}
 
 ModelInstanceIndex GetOrCreateModelInstanceByName(
  MultibodyPlant<double>* plant, const std::string& model_name) {
@@ -347,8 +421,8 @@ std::string FrameNameRenameSameName(const MultibodyPlant<double>&,
 MultibodyPlantElementsMap MultibodyPlantSubgraph::AddTo(
  MultibodyPlant<double>* plant_dest, RemapFunction model_instance_remap,
  FrameNameRemapFunction frame_name_remap) const {
- const auto* plant_src = &elem_src_.plant();
- const auto* scene_graph_src = &elem_src_.scene_graph();
+ const auto* plant_src = elem_src_.plant();
+ const auto* scene_graph_src = elem_src_.scene_graph();
 
  MultibodyPlantElementsMap src_to_dest(plant_src, plant_dest, scene_graph_src);
 

multibody/parsing/detail_multibody_plant_subgraph.h

@@ -64,7 +64,7 @@ class MultibodyPlantElements {
  }
  void PrintModels(std::ostream& out) const {
  for (const auto& model : model_instances()) {
- out << "Model: " << model << " " << plant().GetModelInstanceName(model)
+ out << "Model: " << model << " " << plant()->GetModelInstanceName(model)
  << std::endl;
  }
  }
@@ -76,9 +76,9 @@ class MultibodyPlantElements {
  PrintJoints(out);
  }
 
- const MultibodyPlant<double>& plant() const { return *plant_; }
- const geometry::SceneGraph<double>& scene_graph() const {
- return *scene_graph_;
+ const MultibodyPlant<double>* plant() const { return plant_; }
+ const geometry::SceneGraph<double>* scene_graph() const {
+ return scene_graph_;
  }
 
  const std::set<const Body<double>*>& bodies() const { return bodies_; }
@@ -185,7 +185,11 @@ class MultibodyPlantElementsMap {
  std::map<geometry::GeometryId, geometry::GeometryId> geometry_ids_;
 };
 
-void CheckSubgraphInvariants(const MultibodyPlantElements&);
+// Ensures that current elements / topology satisifes subgraph invariants.
+// @param elem
+// The MultibodyPlantElements object to be checked.
+// @throws std::exception if any of the invariant checks fails.
+void CheckSubgraphInvariants(const MultibodyPlantElements& elem);
 
 ModelInstanceIndex GetOrCreateModelInstanceByName(
  MultibodyPlant<double>* plant, const std::string& model_name);