Merge pull request #2119 from jorisv/topic/add_geometry_material

Add material to GeometryObject

CHANGELOG.md

@@ -6,6 +6,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+### Added
+- Add `GeometryObject::meshMaterial` attribute ([#2084](https://github.com/stack-of-tasks/pinocchio/issues/2084))
+
 ### Fixed
 
 - Use bp::ssize_t for recent version of Windows compilers ([#2102](https://github.com/stack-of-tasks/pinocchio/pull/2102))

bindings/python/pinocchio/visualize/meshcat_visualizer.py

@@ -446,19 +446,29 @@ def loadViewerGeometryObject(self, geometry_object, geometry_type, color=None):
         elif isinstance(obj, meshcat.geometry.Geometry):
             material = meshcat.geometry.MeshPhongMaterial()
             # Set material color from URDF, converting for triplet of doubles to a single int.
+
+            def to_material_color(rgba) -> int:
+                """Convert rgba color as list into rgba color as int"""
+                return (int(rgba[0] * 255) * 256**2
+                        + int(rgba[1] * 255) * 256
+                        + int(rgba[2] * 255)
+                        )
+
             if color is None:
                 meshColor = geometry_object.meshColor
             else:
                 meshColor = color
-            material.color = (
-                int(meshColor[0] * 255) * 256**2
-                + int(meshColor[1] * 255) * 256
-                + int(meshColor[2] * 255)
-            )
             # Add transparency, if needed.
+            material.color = to_material_color(meshColor)
+
             if float(meshColor[3]) != 1.0:
                 material.transparent = True
                 material.opacity = float(meshColor[3])
+            geom_material = geometry_object.meshMaterial
+            if geometry_object.overrideMaterial and isinstance(geom_material, pin.GeometryPhongMaterial):
+                material.emissive = to_material_color(geom_material.meshEmissionColor)
+                material.specular = to_material_color(geom_material.meshSpecularColor)
+                material.shininess = geom_material.meshShininess*100.
             self.viewer[viewer_name].set_object(obj, material)
 
         if is_mesh:  # Apply the scaling

examples/meshcat-viewer.py

@@ -19,7 +19,8 @@
 # urdf_filename = "talos_reduced.urdf"
 # urdf_model_path = join(join(model_path,"talos_data/robots"),urdf_filename)
 urdf_filename = "solo.urdf"
-urdf_model_path = join(join(model_path, "solo_description/robots"), urdf_filename)
+urdf_model_path = join(
+    join(model_path, "solo_description/robots"), urdf_filename)
 
 model, collision_model, visual_model = pin.buildModelsFromUrdf(
     urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
@@ -48,19 +49,29 @@
 # Display a robot configuration.
 q0 = pin.neutral(model)
 viz.display(q0)
-viz.displayCollisions(True)
-viz.displayVisuals(False)
+viz.displayVisuals(True)
 
+# Create a convex shape from solo main body
 mesh = visual_model.geometryObjects[0].geometry
 mesh.buildConvexRepresentation(True)
 convex = mesh.convex
 
+# Place the convex object on the scene and display it
 if convex is not None:
     placement = pin.SE3.Identity()
     placement.translation[0] = 2.0
     geometry = pin.GeometryObject("convex", 0, convex, placement)
     geometry.meshColor = np.ones((4))
+    # Add a PhongMaterial to the convex object
+    geometry.overrideMaterial = True
+    geometry.meshMaterial = pin.GeometryPhongMaterial()
+    geometry.meshMaterial.meshEmissionColor = np.array([1., 0.1, 0.1, 1.])
+    geometry.meshMaterial.meshSpecularColor = np.array([0.1, 1., 0.1, 1.])
+    geometry.meshMaterial.meshShininess = 0.8
     visual_model.addGeometryObject(geometry)
+    # After modifying the visual_model we must rebuild
+    # associated data inside the visualizer
+    viz.rebuildData()
 
 # Display another robot.
 viz2 = MeshcatVisualizer(model, collision_model, visual_model)
@@ -72,7 +83,8 @@
 
 # standing config
 q1 = np.array(
-    [0.0, 0.0, 0.235, 0.0, 0.0, 0.0, 1.0, 0.8, -1.6, 0.8, -1.6, -0.8, 1.6, -0.8, 1.6]
+    [0.0, 0.0, 0.235, 0.0, 0.0, 0.0, 1.0, 0.8, -
+        1.6, 0.8, -1.6, -0.8, 1.6, -0.8, 1.6]
 )
 
 v0 = np.random.randn(model.nv) * 2

include/pinocchio/bindings/python/multibody/geometry-object.hpp

@@ -19,26 +19,99 @@ namespace pinocchio
   {
     namespace bp = boost::python;
 
+    namespace
+    {
+      /// Convert GeometryMaterial boost variant to a Python object.
+      /// This converter copy the GeometryMaterial content.
+      struct GeometryMaterialValueToObject : boost::static_visitor<PyObject*>
+      {
+        static result_type convert(GeometryMaterial const & gm)
+        {
+          return apply_visitor(GeometryMaterialValueToObject(), gm);
+        }
+
+        template<typename T>
+        result_type operator()(T & t) const
+        {
+          return bp::incref(bp::object(t).ptr());
+        }
+      };
+
+      /// Convert GeometryMaterial boost variant to a Python object.
+      /// This converter return the GeometryMaterial reference.
+      /// The code the create the reference holder is taken from \see boost::python::to_python_indirect.
+      struct GeometryMaterialRefToObject : boost::static_visitor<PyObject*>
+      {
+        static result_type convert(GeometryMaterial const & gm)
+        {
+          return apply_visitor(GeometryMaterialRefToObject(), gm);
+        }
+
+        template<typename T>
+        result_type operator()(T & t) const
+        {
+          return bp::detail::make_reference_holder::execute(&t);
+        }
+      };
+
+      /// Converter used in \see ReturnInternalVariant.
+      /// This is inspired by \see boost::python::reference_existing_object.
+      ///  It will call GeometryMaterialRefToObject to extract the variant reference.
+      struct GeometryMaterialConverter
+      {
+        template <class T>
+        struct apply
+        {
+          struct type
+          {
+            inline PyObject* operator()(const GeometryMaterial& gm) const
+            {
+              return GeometryMaterialRefToObject::convert(gm);
+            }
+
+#ifndef BOOST_PYTHON_NO_PY_SIGNATURES
+            inline PyTypeObject const* get_pytype()const
+            {
+              return bp::converter::registered_pytype<GeometryMaterial>::get_pytype();
+            }
+#endif
+          };
+        };
+      };
+
+      /// Variant of \see boost::python::return_internal_reference that
+      /// extract GeometryMaterial variant before converting it into a PyObject*
+      struct GeometryMaterialReturnInternalVariant : bp::return_internal_reference<> {
+       public:
+        typedef GeometryMaterialConverter result_converter;
+      };
+    }
+
+
+
     struct GeometryObjectPythonVisitor
     : public boost::python::def_visitor< GeometryObjectPythonVisitor >
     {
+
       typedef GeometryObject::CollisionGeometryPtr CollisionGeometryPtr;
 
       template<class PyClass>
       void visit(PyClass& cl) const
       {
         cl
         .def(bp::init<std::string,FrameIndex,JointIndex,CollisionGeometryPtr,SE3,
-                      bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string> >
+                      bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string,GeometryMaterial> >
              (
              bp::args("self","name","parent_frame","parent_joint","collision_geometry",
-                      "placement", "mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path"),
+                      "placement", "mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path",
+                      "mesh_material"),
              "Full constructor of a GeometryObject."))
         .def(bp::init<std::string,JointIndex,CollisionGeometryPtr,SE3,
-                      bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string> >
+                      bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string,GeometryMaterial> >
              (
               bp::args("self","name","parent_joint","collision_geometry",
-                       "placement", "mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path"),
+                       "placement", "mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path",
+                       "mesh_material"),
               "Reduced constructor of a GeometryObject. This constructor does not require to specify the parent frame index."
               ))
         .def(bp::init<const GeometryObject&>
@@ -74,6 +147,11 @@ namespace pinocchio
                        "Path to the mesh texture file.")
         .def_readwrite("disableCollision", &GeometryObject::disableCollision,
                        "If true, no collision or distance check will be done between the Geometry and any other geometry.")
+        .add_property("meshMaterial",
+                      bp::make_getter(&GeometryObject::meshMaterial,
+                                      GeometryMaterialReturnInternalVariant()),
+                      bp::make_setter(&GeometryObject::meshMaterial),
+                      "Material associated to the mesh (applied only if overrideMaterial is True)")
 
         .def(bp::self == bp::self)
         .def(bp::self != bp::self)
@@ -104,8 +182,39 @@ namespace pinocchio
       }
 #endif // PINOCCHIO_WITH_HPP_FCL
 
+      static GeometryMaterial get_content(const GeometryMaterial& gm)
+      {
+        return gm;
+      }
+
       static void expose()
       {
+        /// Define material types
+        bp::class_<GeometryNoMaterial>("GeometryNoMaterial", bp::init<>())
+        .def(bp::init<GeometryNoMaterial>());
+        bp::class_<GeometryPhongMaterial>("GeometryPhongMaterial", bp::init<>())
+        .def(bp::init<GeometryPhongMaterial>())
+        .def(bp::init<Eigen::Vector4d, Eigen::Vector4d, double>())
+        .add_property("meshEmissionColor",
+                      bp::make_getter(&GeometryPhongMaterial::meshEmissionColor,
+                                      bp::return_internal_reference<>()),
+                      bp::make_setter(&GeometryPhongMaterial::meshEmissionColor),
+                      "RGBA emission (ambient) color value of the mesh")
+        .add_property("meshSpecularColor",
+                      bp::make_getter(&GeometryPhongMaterial::meshSpecularColor,
+                                      bp::return_internal_reference<>()),
+                      bp::make_setter(&GeometryPhongMaterial::meshSpecularColor),
+                      "RGBA specular value of the mesh")
+        .def_readwrite("meshShininess", &GeometryPhongMaterial::meshShininess,
+                       "Shininess associated to the specular lighting model (between 0 and 1)");
+
+        /// Define material conversion from C++ variant to python object
+        bp::to_python_converter<GeometryMaterial, GeometryMaterialValueToObject>();
+
+        /// Define material conversion from python object to C++ object
+        bp::implicitly_convertible<GeometryNoMaterial, GeometryMaterial>();
+        bp::implicitly_convertible<GeometryPhongMaterial, GeometryMaterial>();
+
         bp::class_<GeometryObject>("GeometryObject",
                                    "A wrapper on a collision geometry including its parent joint, parent frame, placement in parent joint's frame.\n\n",
                                    bp::no_init

include/pinocchio/multibody/fcl.hpp

@@ -9,6 +9,12 @@
 #include "pinocchio/multibody/fwd.hpp"
 #include "pinocchio/container/aligned-vector.hpp"
 
+/// Be carefull to include this header after fwd.hpp.
+/// fwd.hpp contains some define to change the boost::variant max size.
+/// If we don't include it before, default size is choosed that can
+/// make all the build fail.
+#include <boost/variant.hpp>
+
 #ifdef PINOCCHIO_WITH_HPP_FCL
 
   #if(WIN32)
@@ -119,6 +125,38 @@ enum GeometryType
   COLLISION
 };
 
+/// No material associated to a geometry.
+struct GeometryNoMaterial
+{
+};
+
+/// Mesh material based on the Phong lighting model.
+/// Diffuse color is stored in \p GeometryObject::meshColor.
+struct GeometryPhongMaterial
+{
+  GeometryPhongMaterial() = default;
+  GeometryPhongMaterial(const Eigen::Vector4d& meshEmissionColor,
+                        const Eigen::Vector4d& meshSpecularColor,
+                        double meshShininess)
+    : meshEmissionColor(meshEmissionColor)
+    , meshSpecularColor(meshSpecularColor)
+    , meshShininess(meshShininess)
+  {}
+
+  /// \brief RGBA emission (ambient) color value of the GeometryObject::geometry object.
+  Eigen::Vector4d meshEmissionColor{Eigen::Vector4d(0., 0., 0., 1.)};
+
+  /// \brief RGBA specular color value of the GeometryObject::geometry object.
+  Eigen::Vector4d meshSpecularColor{Eigen::Vector4d(0., 0., 0., 1.)};
+
+  /// \brief Shininess associated to the specular lighting model.
+  ///
+  /// This value must normalized between 0 and 1.
+  double meshShininess{0.};
+};
+
+typedef boost::variant<GeometryNoMaterial, GeometryPhongMaterial> GeometryMaterial;
+
 struct GeometryObject
 {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -148,18 +186,23 @@ struct GeometryObject
   /// \brief Position of geometry object in parent joint frame
   SE3 placement;
 
-  /// \brief Absolute path to the mesh file (if the fcl pointee is also a Mesh)
+  /// \brief Absolute path to the mesh file (if the geometry pointee is also a Mesh)
   std::string meshPath;
 
-  /// \brief Scaling vector applied to the GeometryObject::fcl object.
+  /// \brief Scaling vector applied to the GeometryObject::geometry object.
   Eigen::Vector3d meshScale;
 
   /// \brief Decide whether to override the Material.
   bool overrideMaterial;
 
-  /// \brief RGBA color value of the GeometryObject::fcl object.
+  /// \brief RGBA diffuse color value of the GeometryObject::geometry object.
   Eigen::Vector4d meshColor;
 
+  /// \brief Material associated to the mesh.
+  /// This material should be used only if overrideMaterial is set to true.
+  /// In other case, the mesh default material must be used.
+  GeometryMaterial meshMaterial;
+
   /// \brief Absolute path to the mesh texture file.
   std::string meshTexturePath;
 
@@ -181,6 +224,7 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   /// \param[in] overrideMaterial If true, this option allows to overrite the material [if applicable].
   /// \param[in] meshColor Color of the mesh [if applicable].
   /// \param[in] meshTexturePath Path to the file containing the texture information [if applicable].
+  /// \param[in] meshMaterial Material of the mesh [if applicable].
   ///
   GeometryObject(const std::string & name,
                  const FrameIndex parent_frame,
@@ -191,7 +235,8 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
                  const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
                  const bool overrideMaterial = false,
                  const Eigen::Vector4d & meshColor = Eigen::Vector4d(0,0,0,1),
-                 const std::string & meshTexturePath = "")
+                 const std::string & meshTexturePath = "",
+                 const GeometryMaterial& meshMaterial = GeometryNoMaterial())
   : name(name)
   , parentFrame(parent_frame)
   , parentJoint(parent_joint)
@@ -202,6 +247,7 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   , meshScale(meshScale)
   , overrideMaterial(overrideMaterial)
   , meshColor(meshColor)
+  , meshMaterial(meshMaterial)
   , meshTexturePath(meshTexturePath)
   , disableCollision(false)
   {}
@@ -222,6 +268,7 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   /// \param[in] overrideMaterial If true, this option allows to overrite the material [if applicable].
   /// \param[in] meshColor Color of the mesh [if applicable].
   /// \param[in] meshTexturePath Path to the file containing the texture information [if applicable].
+  /// \param[in] meshMaterial Material of the mesh [if applicable].
   ///
   GeometryObject(const std::string & name,
                  const JointIndex parent_joint,
@@ -231,7 +278,8 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
                  const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
                  const bool overrideMaterial = false,
                  const Eigen::Vector4d & meshColor = Eigen::Vector4d::Ones(),
-                 const std::string & meshTexturePath = "")
+                 const std::string & meshTexturePath = "",
+                 const GeometryMaterial& meshMaterial = GeometryNoMaterial())
   : name(name)
   , parentFrame(std::numeric_limits<FrameIndex>::max())
   , parentJoint(parent_joint)
@@ -242,6 +290,7 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
   , meshScale(meshScale)
   , overrideMaterial(overrideMaterial)
   , meshColor(meshColor)
+  , meshMaterial(meshMaterial)
   , meshTexturePath(meshTexturePath)
   , disableCollision(false)
   {}
@@ -268,6 +317,7 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP
     meshScale           = other.meshScale;
     overrideMaterial    = other.overrideMaterial;
     meshColor           = other.meshColor;
+    meshMaterial        = other.meshMaterial;
     meshTexturePath     = other.meshTexturePath;
     disableCollision   = other.disableCollision;
     return *this;