Add visualization material to geometry (RobotLocomotion#8474)

* Add visualization material to geometry 1. Adds a definition of a *visualization* material (used for simple viz) 2. Modifies declaration of GeometryInstance to include a material. 3. Modifies the solar system example to have colors. 4. Updates LCM generation to account for materials. 5. Updates the RigidBodyPlantBridge to extract visual materials from RBT and pass them onto GeometrySystem. 6. Adds a unit test to the geometry_visualization functionality; confirms message contains expected contents.
mqnc · Apr 10, 2018 · 4c1f0f1 · 4c1f0f1
1 parent 26a20bb
commit 4c1f0f1
Show file tree

Hide file tree

Showing 16 changed files with 374 additions and 42 deletions.
diff --git a/examples/geometry_world/solar_system.cc b/examples/geometry_world/solar_system.cc
@@ -28,6 +28,7 @@ using geometry::GeometrySystem;
 using geometry::Mesh;
 using geometry::SourceId;
 using geometry::Sphere;
+using geometry::VisualMaterial;
 using systems::BasicVector;
 using systems::Context;
 using systems::ContinuousState;
@@ -106,7 +107,7 @@ void SolarSystem<T>::SetDefaultState(const systems::Context<T>&,
 // origin, and the top of the arm is positioned at the given height.
 template <class ParentId>
 void MakeArm(SourceId source_id, ParentId parent_id, double length,
-             double height, double radius,
+             double height, double radius, const VisualMaterial& material,
              GeometrySystem<double>* geometry_system) {
   Isometry3<double> arm_pose = Isometry3<double>::Identity();
   // tilt it horizontally
@@ -116,14 +117,14 @@ void MakeArm(SourceId source_id, ParentId parent_id, double length,
   geometry_system->RegisterGeometry(
       source_id, parent_id,
       make_unique<GeometryInstance>(
-          arm_pose, make_unique<Cylinder>(radius, length)));
+          arm_pose, make_unique<Cylinder>(radius, length), material));
 
   Isometry3<double> post_pose = Isometry3<double>::Identity();
   post_pose.translation() << length, 0, height/2;
   geometry_system->RegisterGeometry(
       source_id, parent_id,
       make_unique<GeometryInstance>(
-          post_pose, make_unique<Cylinder>(radius, height)));
+          post_pose, make_unique<Cylinder>(radius, height), material));
 }
 
 template <typename T>
@@ -132,15 +133,17 @@ void SolarSystem<T>::AllocateGeometry(GeometrySystem<T>* geometry_system) {
   body_offset_.reserve(kBodyCount);
   axes_.reserve(kBodyCount);
 
+  VisualMaterial post_material{Vector4d(0.3, 0.15, 0.05, 1)};
   const double orrery_bottom = -1.5;
   const double pipe_radius = 0.05;
 
   // Allocate the sun.
   // NOTE: we don't store the id of the sun geometry because we have no need
   // for subsequent access (the same is also true for dynamic geometries).
   geometry_system->RegisterAnchoredGeometry(
-      source_id_, make_unique<GeometryInstance>(Isometry3<double>::Identity(),
-                                                make_unique<Sphere>(1.f)));
+      source_id_, make_unique<GeometryInstance>(
+                      Isometry3<double>::Identity(), make_unique<Sphere>(1.f),
+                      VisualMaterial(Vector4d(1, 1, 0, 1))));
 
   // The fixed post on which Sun sits and around which all planets rotate.
   const double post_height = 1;
@@ -149,7 +152,8 @@ void SolarSystem<T>::AllocateGeometry(GeometrySystem<T>* geometry_system) {
   geometry_system->RegisterAnchoredGeometry(
       source_id_, make_unique<GeometryInstance>(
                       post_pose,
-                      make_unique<Cylinder>(pipe_radius, post_height)));
+                      make_unique<Cylinder>(pipe_radius, post_height),
+                      post_material));
 
   // Allocate the "celestial bodies": two planets orbiting on different planes,
   // each with a moon.
@@ -172,10 +176,11 @@ void SolarSystem<T>::AllocateGeometry(GeometrySystem<T>* geometry_system) {
       Translation3<double>{kEarthOrbitRadius, 0, -kEarthBottom}};
   geometry_system->RegisterGeometry(
       source_id_, planet_id,
-      make_unique<GeometryInstance>(X_EGe, make_unique<Sphere>(0.25f)));
+      make_unique<GeometryInstance>(X_EGe, make_unique<Sphere>(0.25f),
+                                    VisualMaterial(Vector4d(0, 0, 1, 1))));
   // Earth's orrery arm.
   MakeArm(source_id_, planet_id, kEarthOrbitRadius, -kEarthBottom, pipe_radius,
-          geometry_system);
+          post_material, geometry_system);
 
   // Luna's frame L is at the center of Earth's geometry (Ge). So, X_EL = X_EGe.
   const Isometry3<double>& X_EL = X_EGe;
@@ -195,8 +200,9 @@ void SolarSystem<T>::AllocateGeometry(GeometrySystem<T>* geometry_system) {
       Vector3<double>(-1, 0.5, 0.5).normalized() * kLunaOrbitRadius;
   Isometry3<double> X_LGl{Translation3<double>{luna_position}};
   geometry_system->RegisterGeometry(
-      source_id_, luna_id,
-      make_unique<GeometryInstance>(X_LGl, make_unique<Sphere>(0.075f)));
+      source_id_, luna_id, make_unique<GeometryInstance>(
+                               X_LGl, make_unique<Sphere>(0.075f),
+                               VisualMaterial(Vector4d(0.5, 0.5, 0.35, 1))));
 
   // Mars's frame M lies directly *below* the sun (to account for the orrery
   // arm).
@@ -215,7 +221,8 @@ void SolarSystem<T>::AllocateGeometry(GeometrySystem<T>* geometry_system) {
       Translation3<double>{kMarsOrbitRadius, 0, -orrery_bottom}};
   GeometryId mars_geometry_id = geometry_system->RegisterGeometry(
       source_id_, planet_id,
-      make_unique<GeometryInstance>(X_MGm, make_unique<Sphere>(kMarsSize)));
+      make_unique<GeometryInstance>(X_MGm, make_unique<Sphere>(kMarsSize),
+                                    VisualMaterial(Vector4d(0.9, 0.1, 0, 1))));
 
   std::string rings_absolute_path =
       FindResourceOrThrow("drake/examples/geometry_world/planet_rings.obj");
@@ -224,11 +231,12 @@ void SolarSystem<T>::AllocateGeometry(GeometrySystem<T>* geometry_system) {
   geometry_system->RegisterGeometry(
       source_id_, mars_geometry_id,
       make_unique<GeometryInstance>(
-          X_GmGr, make_unique<Mesh>(rings_absolute_path, kMarsSize)));
+          X_GmGr, make_unique<Mesh>(rings_absolute_path, kMarsSize),
+          VisualMaterial(Vector4d(0.45, 0.9, 0, 1))));
 
   // Mars's orrery arm.
   MakeArm(source_id_, planet_id, kMarsOrbitRadius, -orrery_bottom, pipe_radius,
-          geometry_system);
+          post_material, geometry_system);
 
   // Phobos's frame P is at the center of Mars's geometry (Gm).
   // So, X_MP = X_MGm. The normal of the plane is negated so it orbits in the
@@ -245,8 +253,9 @@ void SolarSystem<T>::AllocateGeometry(GeometrySystem<T>* geometry_system) {
   const double kPhobosOrbitRadius = 0.34;
   Isometry3<double> X_PGp{Translation3<double>{kPhobosOrbitRadius, 0, 0}};
   geometry_system->RegisterGeometry(
-      source_id_, phobos_id,
-      make_unique<GeometryInstance>(X_PGp, make_unique<Sphere>(0.06f)));
+      source_id_, phobos_id, make_unique<GeometryInstance>(
+                                 X_PGp, make_unique<Sphere>(0.06f),
+                                 VisualMaterial(Vector4d(0.65, 0.6, 0.8, 1))));
 
   DRAKE_DEMAND(static_cast<int>(body_ids_.size()) == kBodyCount);
 }

diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel
@@ -85,12 +85,23 @@ drake_cc_library(
     hdrs = ["geometry_instance.h"],
     deps = [
         ":geometry_ids",
+        ":materials",
         ":shape_specification",
         "//common:copyable_unique_ptr",
         "//common:essential",
     ],
 )
 
+drake_cc_library(
+    # NOTE: The material library and file name don't appear to match. This
+    # library will eventually contain multiple types of materials (e.g.,
+    # contact, rendering, depth, etc.)
+    name = "materials",
+    srcs = ["visual_material.cc"],
+    hdrs = ["visual_material.h"],
+    deps = ["//common"],
+)
+
 drake_cc_library(
     name = "geometry_state",
     srcs = ["geometry_state.cc"],
@@ -158,6 +169,7 @@ drake_cc_library(
     deps = [
         ":geometry_ids",
         ":geometry_index",
+        ":materials",
         ":shape_specification",
         "//common:copyable_unique_ptr",
         "//common:essential",
@@ -234,6 +246,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "geometry_visualization_test",
+    deps = [
+        ":geometry_visualization",
+        "//common/test_utilities:expect_throws_message",
+    ],
+)
+
 drake_cc_googletest(
     name = "identifier_test",
     deps = [":identifier"],
@@ -255,4 +275,11 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "visual_material_test",
+    deps = [
+        ":materials",
+    ],
+)
+
 add_lint_tests()
diff --git a/geometry/geometry_instance.cc b/geometry/geometry_instance.cc
@@ -7,7 +7,15 @@ namespace geometry {
 
 GeometryInstance::GeometryInstance(const Isometry3<double>& X_PG,
                                    std::unique_ptr<Shape> shape)
-    : id_(GeometryId::get_new_id()), X_PG_(X_PG), shape_(std::move(shape)) {}
+    : GeometryInstance(X_PG, std::move(shape), VisualMaterial()) {}
+
+GeometryInstance::GeometryInstance(const Isometry3<double>& X_PG,
+                                   std::unique_ptr<Shape> shape,
+                                   const VisualMaterial& vis_material)
+    : id_(GeometryId::get_new_id()),
+      X_PG_(X_PG),
+      shape_(std::move(shape)),
+      visual_material_(vis_material) {}
 
 }  // namespace geometry
 }  // namespace drake
diff --git a/geometry/geometry_instance.h b/geometry/geometry_instance.h
@@ -8,6 +8,7 @@
 #include "drake/common/eigen_types.h"
 #include "drake/geometry/geometry_ids.h"
 #include "drake/geometry/shape_specification.h"
+#include "drake/geometry/visual_material.h"
 
 namespace drake {
 namespace geometry {
@@ -20,11 +21,19 @@ class GeometryInstance {
  public:
   DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(GeometryInstance)
 
-  /** Constructor.
+  /** Constructor with default visual material (see VisualMaterial default
+   constructor for details on what that color is).
    @param X_PG   The pose of this geometry (`G`) in its parent's frame (`P`).
    @param shape  The underlying shape for this geometry instance. */
   GeometryInstance(const Isometry3<double>& X_PG, std::unique_ptr<Shape> shape);
 
+  /** Constructor.
+   @param X_PG   The pose of this geometry (`G`) in its parent's frame (`P`).
+   @param shape  The underlying shape for this geometry instance.
+   @param vis_material The visual material to apply to this geometry. */
+  GeometryInstance(const Isometry3<double>& X_PG, std::unique_ptr<Shape> shape,
+                   const VisualMaterial& vis_material);
+
   /** Returns the globally unique id for this geometry specification. Every
    instantiation of %GeometryInstance will contain a unique id value. The id
    value is preserved across copies. After successfully registering this
@@ -43,6 +52,8 @@ class GeometryInstance {
   /** Releases the shape from the instance. */
   std::unique_ptr<Shape> release_shape() { return std::move(shape_); }
 
+  const VisualMaterial& visual_material() const { return visual_material_; }
+
  private:
   // The *globally* unique identifier for this instance. It is functionally
   // const (i.e. defined in construction) but not marked const to allow for
@@ -54,6 +65,9 @@ class GeometryInstance {
 
   // The shape associated with this instance.
   copyable_unique_ptr<Shape> shape_;
+
+  // The "rendering" material -- e.g., OpenGl contexts and the like.
+  VisualMaterial visual_material_;
 };
 }  // namespace geometry
 }  // namespace drake
diff --git a/geometry/geometry_state.cc b/geometry/geometry_state.cc
@@ -268,7 +268,8 @@ GeometryId GeometryState<T>::RegisterGeometry(
   geometries_.emplace(
       geometry_id,
       InternalGeometry(geometry->release_shape(), frame_id, geometry_id,
-                       geometry->pose(), engine_index));
+                       geometry->pose(), engine_index,
+                       geometry->visual_material()));
   // TODO(SeanCurtis-TRI): Enforcing the invariant that the indexes are
   // compactly distributed. Is there a more robust way to do this?
   DRAKE_ASSERT(static_cast<int>(X_FG_.size()) == engine_index);
@@ -354,8 +355,9 @@ GeometryId GeometryState<T>::RegisterAnchoredGeometry(
   anchored_geometry_index_id_map_.push_back(geometry_id);
   anchored_geometries_.emplace(
       geometry_id,
-      InternalAnchoredGeometry(geometry->release_shape(), geometry_id,
-                               geometry->pose(), engine_index));
+      InternalAnchoredGeometry(
+          geometry->release_shape(), geometry_id, geometry->pose(),
+          engine_index, geometry->visual_material()));
   return geometry_id;
 }
 

diff --git a/geometry/geometry_state.h b/geometry/geometry_state.h
@@ -19,11 +19,14 @@
 #include "drake/geometry/proximity_engine.h"
 
 namespace drake {
+
 namespace geometry {
 
 #ifndef DRAKE_DOXYGEN_CXX
 namespace internal {
 
+class GeometryVisualizationImpl;
+
 // A const range iterator through the keys of an unordered map.
 template <typename K, typename V>
 class MapKeyRange {
@@ -429,8 +432,10 @@ class GeometryState {
     convert(source.X_WF_, &X_WF_);
   }
 
-  // Allow geometry dispatch to peek into GeometryState.
-  friend void DispatchLoadMessage(const GeometryState<double>&);
+  // NOTE: This friend class is responsible for evaluating the internals of
+  // a GeometryState and translating it into the appropriate visualization
+  // mechanism.
+  friend class internal::GeometryVisualizationImpl;
 
   // Allow GeometrySystem unique access to the state members to perform queries.
   friend class GeometrySystem<T>;

diff --git a/geometry/geometry_visualization.cc b/geometry/geometry_visualization.cc
@@ -10,7 +10,6 @@
 #include "drake/geometry/shape_specification.h"
 #include "drake/lcm/drake_lcm.h"
 #include "drake/lcmt_viewer_geometry_data.hpp"
-#include "drake/lcmt_viewer_load_robot.hpp"
 #include "drake/math/rotation_matrix.h"
 
 namespace drake {
@@ -112,10 +111,12 @@ lcmt_viewer_geometry_data MakeGeometryData(const Shape& shape,
 
 }  // namespace
 
-void DispatchLoadMessage(const GeometryState<double>& state) {
+namespace internal {
+
+lcmt_viewer_load_robot GeometryVisualizationImpl::BuildLoadMessage(
+    const GeometryState<double>& state) {
   using internal::InternalAnchoredGeometry;
   using internal::InternalGeometry;
-  using lcm::DrakeLcm;
 
   lcmt_viewer_load_robot message{};
   // Populate the message.
@@ -129,8 +130,6 @@ void DispatchLoadMessage(const GeometryState<double>& state) {
   message.num_links = total_link_count;
   message.link.resize(total_link_count);
 
-  Eigen::Vector4d default_color(0.8, 0.8, 0.8, 1.0);
-
   int link_index = 0;
   // Load anchored geometry into the world frame.
   {
@@ -143,8 +142,10 @@ void DispatchLoadMessage(const GeometryState<double>& state) {
       for (const auto& pair : state.anchored_geometries_) {
         const InternalAnchoredGeometry& geometry = pair.second;
         const Shape& shape = geometry.get_shape();
+        const Eigen::Vector4d& color =
+            geometry.get_visual_material().diffuse();
         message.link[0].geom[geom_index] = MakeGeometryData(
-            shape, geometry.get_pose_in_parent(), default_color);
+            shape, geometry.get_pose_in_parent(), color);
         ++geom_index;
       }
       link_index = 1;
@@ -168,16 +169,28 @@ void DispatchLoadMessage(const GeometryState<double>& state) {
     int geom_index = 0;
     for (GeometryId geom_id : frame.get_child_geometries()) {
       const InternalGeometry& geometry = state.geometries_.at(geom_id);
-      GeometryIndex index = state.geometries_.at(geom_id).get_engine_index();
-      const Shape& shape = geometry.get_shape();
+      GeometryIndex index = geometry.get_engine_index();
       const Isometry3<double> X_FG = state.X_FG_.at(index);
+      const Shape& shape = geometry.get_shape();
+      const Eigen::Vector4d& color =
+          geometry.get_visual_material().diffuse();
       message.link[link_index].geom[geom_index] =
-          MakeGeometryData(shape, X_FG, default_color);
+          MakeGeometryData(shape, X_FG, color);
       ++geom_index;
     }
     ++link_index;
   }
 
+  return message;
+}
+
+}  // namespace internal
+
+void DispatchLoadMessage(const GeometryState<double>& state) {
+  using lcm::DrakeLcm;
+
+  lcmt_viewer_load_robot message =
+      internal::GeometryVisualizationImpl::BuildLoadMessage(state);
   // Send a load message.
   DrakeLcm lcm;
   Publish(&lcm, "DRAKE_VIEWER_LOAD_ROBOT", message);