Merge pull request #121 from jmirabel/devel

Add CollisionObject::geometry, replaceGeometryByConvexHull and prepare deprecation of collisionObjects

Merge pull request #121 from jmirabel/devel
Add CollisionObject::geometry, replaceGeometryByConvexHull and prepare deprecation of collisionObjects
fb4563cf · Joseph Mirabel · GitHub · 85a21189 · 30397793 · fb4563cf
Unverified Commit fb4563cf authored May 06, 2020 by Joseph Mirabel Committed by GitHub May 06, 2020
--- a/include/hpp/pinocchio/collision-object.hh
+++ b/include/hpp/pinocchio/collision-object.hh
@@ -62,6 +62,9 @@ namespace hpp {
      /// Access to pinocchio object
      const GeometryObject & pinocchio () const;
      GeometryObject &       pinocchio ();
+
+      /// Access to fcl object
+      CollisionGeometryPtr_t geometry () const;
      
      /// Access to fcl object
      FclConstCollisionObjectPtr_t fcl (const GeomData& data) const;
@@ -90,7 +93,7 @@ namespace hpp {
      /// manually moved, this will return the non-update transform.
      /// \note If object is not attached to a joint, use move() to update
      /// transform between hpp and fcl.
-      const fcl::Transform3f& getFclTransform () const;
+      fcl::Transform3f getFclTransform () const;
      const Transform3f&      getTransform () const;
      const Transform3f&      getTransform (DeviceData& d) const;


--- a/include/hpp/pinocchio/device.hh
+++ b/include/hpp/pinocchio/device.hh
@@ -341,6 +341,9 @@ namespace hpp {
      Pool<DeviceData> datas_;
    }; // class Device

+    void replaceGeometryByConvexHull (GeomModel& geomModel,
+        const std::vector<std::string>& geometryNames);
+
    inline std::ostream& operator<< (std::ostream& os, const hpp::pinocchio::Device& device)
    { return device.print(os); }


--- a/include/hpp/pinocchio/fwd.hh
+++ b/include/hpp/pinocchio/fwd.hh
@@ -96,6 +96,8 @@ namespace hpp {
    typedef boost::shared_ptr<Body> BodyPtr_t;
    typedef boost::shared_ptr<const Body> BodyConstPtr_t;
    //typedef std::vector<BodyPtr_t> BodyVector_t;
+    typedef fcl::CollisionGeometry CollisionGeometry;
+    typedef boost::shared_ptr<CollisionGeometry> CollisionGeometryPtr_t;
    typedef       fcl::CollisionObject   FclCollisionObject;
    typedef       fcl::CollisionObject * FclCollisionObjectPtr_t;
    typedef const fcl::CollisionObject * FclConstCollisionObjectPtr_t;

--- a/src/collision-object.cc
+++ b/src/collision-object.cc
@@ -21,6 +21,7 @@

 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/multibody/geometry.hpp>
+#include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>

 #include <hpp/pinocchio/device.hh>
 #include <hpp/pinocchio/joint.hh>
@@ -63,6 +64,9 @@ namespace hpp {
    ::pinocchio::GeometryObject & CollisionObject::pinocchio ()
    { return geomModel_->geometryObjects[geomInModelIndex]; }

+    CollisionGeometryPtr_t CollisionObject::geometry () const
+    { return pinocchio().geometry; }
+
    FclCollisionObjectPtr_t CollisionObject::fcl (GeomData& geomData) const
    {
      return & geomData.collisionObjects[geomInModelIndex];
@@ -102,8 +106,8 @@ namespace hpp {
    const Transform3f& CollisionObject::
    positionInJointFrame () const { return pinocchio().placement; }

-    const fcl::Transform3f& CollisionObject::getFclTransform () const
-    { return geomData().collisionObjects[geomInModelIndex].getTransform(); }
+    fcl::Transform3f CollisionObject::getFclTransform () const
+    { return ::pinocchio::toFclTransform3f(geomData().oMg[geomInModelIndex]); }
    const Transform3f& CollisionObject::getTransform () const
    { return geomData().oMg[geomInModelIndex];  }
    const Transform3f& CollisionObject::getTransform (DeviceData& d) const

--- a/src/device.cc
+++ b/src/device.cc
@@ -23,6 +23,7 @@
 #include <Eigen/Core>

 #include <hpp/fcl/BV/AABB.h>
+#include <hpp/fcl/BVH/BVH_model.h>

 #include <pinocchio/algorithm/geometry.hpp>
 #include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch
@@ -503,5 +504,24 @@ namespace hpp {
      }
      return aabb;
    }
+
+    void replaceGeometryByConvexHull (GeomModel& gmodel,
+        const std::vector<std::string>& gnames)
+    {
+      for (std::size_t i = 0; i < gnames.size(); ++i) {
+        if (!gmodel.existGeometryName(gnames[i]))
+          throw std::invalid_argument("Geometry " + gnames[i] + " does not "
+              "exist.");
+        GeomIndex gid = gmodel.getGeometryId(gnames[i]);
+        GeometryObject& go = gmodel.geometryObjects[gid];
+        if (go.geometry->getObjectType() == fcl::OT_BVH) {
+          boost::shared_ptr<fcl::BVHModelBase> bvh =
+            HPP_DYNAMIC_PTR_CAST(fcl::BVHModelBase, go.geometry);
+          assert(bvh);
+          bvh->buildConvexHull(false, "Qx");
+          go.geometry = bvh->convex;
+        }
+      }
+    }
  } // namespace pinocchio
 } // namespace hpp