Add multibodyPlant::RegisterGeometry

multibody/parsing/detail_multibody_plant_subgraph.cc

@@ -347,8 +347,6 @@ void MultibodyPlantElementsMap::CopyGeometryById(
  const auto* body_src = plant_src_->GetBodyFromFrameId(frame_id_src);
  DRAKE_DEMAND(body_src != nullptr);
  const auto* body_dest = bodies_.at(body_src);
- // const auto frame_id_dest =
- // plant_dest_->GetBodyFrameIdOrThrow(body_dest->index());
  auto geometry_instance_dest =
  inspector_src.CloneGeometryInstance(geometry_id_src);
 
@@ -360,7 +358,8 @@ void MultibodyPlantElementsMap::CopyGeometryById(
 
  std::string prefix_dest;
  if (body_dest->model_instance() != world_model_instance()) {
- prefix_dest = plant_src_->GetModelInstanceName(body_src->model_instance());
+ prefix_dest =
+ plant_dest_->GetModelInstanceName(body_dest->model_instance());
  }
 
  auto scoped_name_src =
@@ -370,6 +369,12 @@ void MultibodyPlantElementsMap::CopyGeometryById(
  auto scoped_name_dest =
  parsing::PrefixName(prefix_dest, scoped_name_src.name);
  geometry_instance_dest->set_name(scoped_name_dest);
+
+ // TODO(eric.cousineau): How to relax this constraint? How can we
+ // register with SceneGraph only?
+ // See: https://github.com/RobotLocomotion/drake/issues/13445
+ // TODO(eric.cousineau): Try Ale's potential fix here:
+ // https://github.com/RobotLocomotion/drake/pull/13371
  const auto* proximity_properties =
  geometry_instance_dest->proximity_properties();
  geometry::GeometryId geometry_id_dest;
@@ -380,20 +385,10 @@ void MultibodyPlantElementsMap::CopyGeometryById(
  *body_dest, geometry_instance_dest->pose(),
  geometry_instance_dest->shape(), scoped_name_src.name,
  *proximity_properties);
- } else if (geometry_instance_dest->illustration_properties() != nullptr) {
- // illustration and perception are currently mutually required roles.
- // TODO(azeey) This might mean that we lose perception-specific roles.
- DRAKE_DEMAND(geometry_instance_dest->perception_properties() != nullptr);
- geometry_id_dest = plant_dest_->RegisterVisualGeometry(
- *body_dest, geometry_instance_dest->pose(),
- geometry_instance_dest->shape(), scoped_name_src.name,
- *geometry_instance_dest->illustration_properties());
  } else {
- // Currently, RegisterVisualGeometry also assigns the Perception role to a
- // geometry, thus, the common use case of geometries that have both
- // Illustration and Preception roles is covered by the `else if` branch
- // above.
- // TODO(azeey) Handle geometries with Perception as their only role.
+ // Register as normal.
+ geometry_id_dest = plant_dest_->RegisterGeometry(
+ *body_dest, std::move(geometry_instance_dest));
  }
  geometry_ids_.insert({geometry_id_src, geometry_id_dest});
 }

multibody/plant/multibody_plant.cc

@@ -499,10 +499,11 @@ geometry::GeometryId MultibodyPlant<T>::RegisterVisualGeometry(
  // TODO(amcastro-tri): Consider doing this after finalize so that we can
  // register geometry that has a fixed path to world to the world body (i.e.,
  // as anchored geometry).
- GeometryId id =
- RegisterGeometry(body, X_BG, shape,
- GetScopedName(*this, body.model_instance(), name));
- scene_graph_->AssignRole(*source_id_, id, properties);
+ std::unique_ptr<geometry::GeometryInstance> geometry_instance =
+ std::make_unique<GeometryInstance>(
+ X_BG, shape.Clone(),
+ GetScopedName(*this, body.model_instance(), name));
+ geometry_instance->set_illustration_properties(properties);
 
  // TODO(SeanCurtis-TRI): Eliminate the automatic assignment of perception
  // and illustration in favor of a protocol that allows definition.
@@ -522,12 +523,8 @@ geometry::GeometryId MultibodyPlant<T>::RegisterVisualGeometry(
  "renderer", "accepting",
  properties.GetProperty<std::set<std::string>>("renderer", "accepting"));
  }
- scene_graph_->AssignRole(*source_id_, id, perception_props);
-
- DRAKE_ASSERT(static_cast<int>(visual_geometries_.size()) == num_bodies());
- visual_geometries_[body.index()].push_back(id);
- ++num_visual_geometries_;
- return id;
+ geometry_instance->set_perception_properties(perception_props);
+ return RegisterGeometry(body, std::move(geometry_instance));
 }
 
 template <typename T>
@@ -549,14 +546,12 @@ geometry::GeometryId MultibodyPlant<T>::RegisterCollisionGeometry(
  // TODO(amcastro-tri): Consider doing this after finalize so that we can
  // register geometry that has a fixed path to world to the world body (i.e.,
  // as anchored geometry).
- GeometryId id = RegisterGeometry(
- body, X_BG, shape, GetScopedName(*this, body.model_instance(), name));
-
- scene_graph_->AssignRole(*source_id_, id, std::move(properties));
- DRAKE_ASSERT(static_cast<int>(collision_geometries_.size()) == num_bodies());
- collision_geometries_[body.index()].push_back(id);
- ++num_collision_geometries_;
- return id;
+ std::unique_ptr<geometry::GeometryInstance> geometry_instance =
+ std::make_unique<GeometryInstance>(
+ X_BG, shape.Clone(),
+ GetScopedName(*this, body.model_instance(), name));
+ geometry_instance->set_proximity_properties(std::move(properties));
+ return RegisterGeometry(body, std::move(geometry_instance));
 }
 
 template <typename T>
@@ -592,6 +587,34 @@ geometry::GeometrySet MultibodyPlant<T>::CollectRegisteredGeometries(
  return geometry_set;
 }
 
+template <typename T>
+geometry::GeometryId MultibodyPlant<T>::RegisterGeometry(
+ const Body<T>& body,
+ std::unique_ptr<geometry::GeometryInstance> geometry_instance) {
+ DRAKE_MBP_THROW_IF_FINALIZED();
+ DRAKE_THROW_UNLESS(geometry_source_is_registered());
+ DRAKE_THROW_UNLESS(body_has_registered_frame(body));
+ bool is_collision = (geometry_instance->proximity_properties() != nullptr);
+ bool is_visual = (geometry_instance->illustration_properties() != nullptr);
+
+ GeometryId geometry_id = scene_graph_->RegisterGeometry(
+ source_id_.value(), body_index_to_frame_id_[body.index()],
+ std::move(geometry_instance));
+ geometry_id_to_body_index_[geometry_id] = body.index();
+ if (is_collision) {
+ DRAKE_ASSERT(static_cast<int>(collision_geometries_.size()) ==
+ num_bodies());
+ collision_geometries_[body.index()].push_back(geometry_id);
+ ++num_collision_geometries_;
+ }
+ if (is_visual) {
+ DRAKE_ASSERT(static_cast<int>(visual_geometries_.size()) == num_bodies());
+ visual_geometries_[body.index()].push_back(geometry_id);
+ ++num_visual_geometries_;
+ }
+ return geometry_id;
+}
+
 template <typename T>
 std::vector<const Body<T>*> MultibodyPlant<T>::GetBodiesWeldedTo(
  const Body<T>& body) const {
@@ -616,25 +639,6 @@ std::unordered_set<BodyIndex> MultibodyPlant<T>::GetFloatingBaseBodies() const {
  return floating_bodies;
 }
 
-template <typename T>
-geometry::GeometryId MultibodyPlant<T>::RegisterGeometry(
- const Body<T>& body, const math::RigidTransform<double>& X_BG,
- const geometry::Shape& shape,
- const std::string& name) {
- DRAKE_ASSERT(!is_finalized());
- DRAKE_ASSERT(geometry_source_is_registered());
- DRAKE_ASSERT(body_has_registered_frame(body));
-
- // Register geometry in the body frame.
- std::unique_ptr<geometry::GeometryInstance> geometry_instance =
- std::make_unique<GeometryInstance>(X_BG, shape.Clone(), name);
- GeometryId geometry_id = scene_graph_->RegisterGeometry(
- source_id_.value(), body_index_to_frame_id_[body.index()],
- std::move(geometry_instance));
- geometry_id_to_body_index_[geometry_id] = body.index();
- return geometry_id;
-}
-
 template <typename T>
 void MultibodyPlant<T>::RegisterGeometryFramesForAllBodies() {
  DRAKE_ASSERT(geometry_source_is_registered());

multibody/plant/multibody_plant.h

@@ -1386,6 +1386,36 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
  geometry::GeometrySet CollectRegisteredGeometries(
  const std::vector<const Body<T>*>& bodies) const;
 
+ /// (Advanced) Registers geometry in a SceneGraph with a given
+ /// geometry::GeometryInstance. The Properties associated with the role of the 
+ /// geometry must be set by the user in the geometry::GeometryInstance.
+ /// 
+ /// The registration includes:
+ /// 1. Register geometry for the corresponding FrameId associated with `body`.
+ /// 2. Update the geometry_id_to_body_index_ map associating the new GeometryId
+ /// to the BodyIndex of `body`.
+ /// 3. Update collision_geometries_ and num_collision_geometries_ if the 
+ /// geometry_instance has proximity properties
+ /// 3. Update visual_geometries_ and num_visual_geometries_ if the 
+ /// geometry_instance has illustration properties
+ /// This assumes:
+ /// 1. Finalize() was not called on `this` plant.
+ /// 2. RegisterAsSourceForSceneGraph() was called on `this` plant.
+ /// 3. `scene_graph` points to the same SceneGraph instance previously
+ /// passed to RegisterAsSourceForSceneGraph().
+ /// 
+ /// @param[in] body
+ /// The body for which geometry is being registered.
+ /// @param[in] geometry_instance
+ /// The geometry::GeometryInstance used for visualization or contact 
+ /// modeling.
+ /// @throws std::exception if called post-finalize.
+ /// @throws std::exception if `scene_graph` does not correspond to the same
+ /// instance with which RegisterAsSourceForSceneGraph() was called.
+ /// @returns the id for the registered geometry.
+ geometry::GeometryId RegisterGeometry(const Body<T>& body,
+ std::unique_ptr<geometry::GeometryInstance> geometry_instance);
+
  /// Given a geometry frame identifier, returns a pointer to the body
  /// associated with that id (nullptr if there is no such body).
  const Body<T>* GetBodyFromFrameId(geometry::FrameId frame_id) const {
@@ -4491,21 +4521,6 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
  void AddJointActuationForces(
  const systems::Context<T>& context, MultibodyForces<T>* forces) const;
 
- // Helper method to register geometry for a given body, either visual or
- // collision. The registration includes:
- // 1. Register geometry for the corresponding FrameId associated with `body`.
- // 2. Update the geometry_id_to_body_index_ map associating the new GeometryId
- // to the BodyIndex of `body`.
- // This assumes:
- // 1. Finalize() was not called on `this` plant.
- // 2. RegisterAsSourceForSceneGraph() was called on `this` plant.
- // 3. `scene_graph` points to the same SceneGraph instance previously
- // passed to RegisterAsSourceForSceneGraph().
- geometry::GeometryId RegisterGeometry(
- const Body<T>& body, const math::RigidTransform<double>& X_BG,
- const geometry::Shape& shape,
- const std::string& name);
-
  // Registers a geometry frame for every body. If the body already has a
  // geometry frame, it is unchanged. This registration is part of finalization.
  // This requires RegisterAsSourceForSceneGraph() was called on `this` plant.