Merge pull request #146 from jcarpent/topic/devel

Improce code efficiency + use shared memory between Numpy and Eigen

CMakeLists.txt

@@ -74,7 +74,7 @@ PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 add_required_dependency("eigen3 >= 3.0.0")
 
 if (BUILD_PYTHON_INTERFACE)
-  ADD_COMPILE_DEPENDENCY("eigenpy >= 2.0")
+  ADD_COMPILE_DEPENDENCY("eigenpy >= 2.2")
 endif ()
 
 # Add a cache variable to allow not compiling and running tests

cmake @ 321eb1cc

-Subproject commit 93ec987ecdc016039c8731e203943e5a83eb96d5
+Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e

include/hpp/fcl/narrowphase/narrowphase.h

@@ -299,6 +299,8 @@ namespace fcl
     mutable Vec3f cached_guess;
   };
 
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wc99-extensions"
   /// \name Shape intersection specializations
   /// \{
 
@@ -403,6 +405,7 @@ namespace fcl
 #undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
 
   /// \}
+#pragma GCC diagnostic pop
 }
 
 } // namespace hpp

package.xml

 <?xml version="1.0"?>
 <package format="2">
   <name>hpp-fcl</name>
-  <version>1.3.0</version>
+  <version>1.4.0</version>
   <description>An extension of the Flexible Collision Library.</description>
   <!-- The maintainer listed here is for the ROS release to receive emails for the buildfarm. 
   Please check the repository URL for full list of authors and maintainers. -->

python/collision-geometries.cc

@@ -35,6 +35,7 @@
 #include <boost/python.hpp>
 
 #include <eigenpy/registration.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 #include "fcl.hh"
 
@@ -138,10 +139,7 @@ void exposeShapes ()
     .def (dv::init<Box>())
     .def (dv::init<Box, FCL_REAL,FCL_REAL,FCL_REAL>())
     .def (dv::init<Box, const Vec3f&>())
-    .add_property("halfSide",
-                  make_getter(&Box::halfSide, return_value_policy<return_by_value>()),
-                  make_setter(&Box::halfSide, return_value_policy<return_by_value>()),
-                  doxygen::class_attrib_doc<Box>("halfSide"))
+    .DEF_RW_CLASS_ATTRIB(Box,halfSide)
     ;
 
   class_ <Capsule, bases<ShapeBase>, shared_ptr<Capsule> >
@@ -263,11 +261,11 @@ void exposeCollisionGeometries ()
   class_<AABB>("AABB",
                "A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points",
                no_init)
-    .def(init<>("Default constructor"))
-    .def(init<Vec3f>(bp::arg("v"),"Creating an AABB at position v with zero size."))
-    .def(init<Vec3f,Vec3f>(bp::args("a","b"),"Creating an AABB with two endpoints a and b."))
-    .def(init<AABB,Vec3f>(bp::args("core","delta"),"Creating an AABB centered as core and is of half-dimension delta."))
-    .def(init<Vec3f,Vec3f,Vec3f>(bp::args("a","b","c"),"Creating an AABB contains three points."))
+    .def(init<>(bp::arg("self"), "Default constructor"))
+    .def(init<Vec3f>(bp::args("self","v"),"Creating an AABB at position v with zero size."))
+    .def(init<Vec3f,Vec3f>(bp::args("self","a","b"),"Creating an AABB with two endpoints a and b."))
+    .def(init<AABB,Vec3f>(bp::args("self","core","delta"),"Creating an AABB centered as core and is of half-dimension delta."))
+    .def(init<Vec3f,Vec3f,Vec3f>(bp::args("self","a","b","c"),"Creating an AABB contains three points."))
   
     .def("contain",
          (bool (AABB::*)(const Vec3f &) const)&AABB::contain,

python/collision.cc

 //
 // Software License Agreement (BSD License)
 //
-//  Copyright (c) 2019 CNRS-LAAS INRIA
+//  Copyright (c) 2019-2020 CNRS-LAAS INRIA
 //  Author: Joseph Mirabel
 //  All rights reserved.
 //
@@ -35,6 +35,7 @@
 #include <boost/python.hpp>
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
+#include <eigenpy/eigen-to-python.hpp>
 #include <eigenpy/registration.hpp>
 
 #include <hpp/fcl/fwd.hh>
@@ -83,7 +84,6 @@ void exposeCollisionAPI ()
         doxygen::class_doc<CollisionRequest>(), no_init)
       .def (dv::init<CollisionRequest>())
       .def (dv::init<CollisionRequest, const CollisionRequestFlag, size_t>())
-    
       .DEF_RW_CLASS_ATTRIB (CollisionRequest, num_max_contacts           )
       .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_contact             )
       .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_distance_lower_bound)
@@ -97,21 +97,15 @@ void exposeCollisionAPI ()
   if(!eigenpy::register_symbolic_link_to_registered_type<Contact>())
   {
     class_ <Contact> ("Contact",
-        doxygen::class_doc<Contact>(), init<>())
+        doxygen::class_doc<Contact>(), init<>(arg("self"),"Default constructor"))
       //.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int>())
       //.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int, Vec3f, Vec3f, FCL_REAL>())
       .DEF_RO_CLASS_ATTRIB (Contact, o1)
       .DEF_RO_CLASS_ATTRIB (Contact, o2)
       .DEF_RW_CLASS_ATTRIB (Contact, b1)
       .DEF_RW_CLASS_ATTRIB (Contact, b2)
-      .add_property("normal",
-          make_getter(&Contact::normal, return_value_policy<return_by_value>()),
-          make_setter(&Contact::normal, return_value_policy<return_by_value>()),
-          doxygen::class_attrib_doc<Contact>("normal"))
-      .add_property("pos",
-          make_getter(&Contact::pos, return_value_policy<return_by_value>()),
-          make_setter(&Contact::pos, return_value_policy<return_by_value>()),
-          doxygen::class_attrib_doc<Contact>("pos"))
+      .DEF_RW_CLASS_ATTRIB (Contact, normal)
+      .DEF_RW_CLASS_ATTRIB (Contact, pos)
       .DEF_RW_CLASS_ATTRIB (Contact, penetration_depth)
       .def (self == self)
       .def (self != self)

python/distance.cc

@@ -36,6 +36,7 @@
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
 #include <eigenpy/registration.hpp>
+#include <eigenpy/eigen-to-python.hpp>
 
 #include "fcl.hh"
 
@@ -65,6 +66,8 @@ using namespace boost::python;
 
 using namespace hpp::fcl;
 
+namespace dv = doxygen::visitor;
+
 struct DistanceRequestWrapper
 {
   static Vec3f getNearestPoint1(const DistanceResult & res) { return res.nearest_points[0]; }
@@ -77,7 +80,10 @@ void exposeDistanceAPI ()
   {
     class_ <DistanceRequest> ("DistanceRequest",
         doxygen::class_doc<DistanceRequest>(),
-        init<optional<bool,FCL_REAL,FCL_REAL> >())
+        init<optional<bool,FCL_REAL,FCL_REAL> >((arg("self"),
+                                                 arg("enable_nearest_points"),
+                                                 arg("rel_err"),
+                                                 arg("abs_err")),"Constructor"))
       .DEF_RW_CLASS_ATTRIB (DistanceRequest, enable_nearest_points)
       .DEF_RW_CLASS_ATTRIB (DistanceRequest, rel_err)
       .DEF_RW_CLASS_ATTRIB (DistanceRequest, abs_err)
@@ -87,12 +93,11 @@ void exposeDistanceAPI ()
   if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>())
   {
     class_ <DistanceResult> ("DistanceResult",
-        doxygen::class_doc<DistanceResult>(), init<>())
+                             doxygen::class_doc<DistanceResult>(),
+                             no_init)
+      .def (dv::init<DistanceResult>())
       .DEF_RW_CLASS_ATTRIB (DistanceResult, min_distance)
-      .add_property("normal",
-          make_getter(&DistanceResult::normal, return_value_policy<return_by_value>()),
-          make_setter(&DistanceResult::normal, return_value_policy<return_by_value>()),
-          doxygen::class_attrib_doc<DistanceResult>("normal"))
+      .DEF_RW_CLASS_ATTRIB(DistanceResult, normal)
       //.def_readwrite ("nearest_points", &DistanceResult::nearest_points)
       .def("getNearestPoint1",&DistanceRequestWrapper::getNearestPoint1,
           doxygen::class_attrib_doc<DistanceResult>("nearest_points"))

python/fcl.cc

@@ -64,10 +64,10 @@ void exposeMeshLoader ()
   {
     class_ <MeshLoader, boost::shared_ptr<MeshLoader> > ("MeshLoader",
         doxygen::class_doc<MeshLoader>(),
-        init< optional< NODE_TYPE> >(arg("node_type"),
+        init< optional< NODE_TYPE> >((arg("self"),arg("node_type")),
           doxygen::constructor_doc<MeshLoader, const NODE_TYPE&>()))
       .def ("load",&MeshLoader::load,
-            load_overloads(args("filename","scale"),
+            load_overloads((arg("self"),arg("filename"),arg("scale")),
                            doxygen::member_func_doc(&MeshLoader::load)))
       ;
   }
@@ -77,7 +77,7 @@ void exposeMeshLoader ()
     class_ <CachedMeshLoader, bases<MeshLoader>, boost::shared_ptr<CachedMeshLoader> > (
         "CachedMeshLoader",
         doxygen::class_doc<MeshLoader>(),
-        init< optional< NODE_TYPE> >(arg("node_type"),
+        init< optional< NODE_TYPE> >((arg("self"),arg("node_type")),
           doxygen::constructor_doc<CachedMeshLoader, const NODE_TYPE&>()))
     ;
   }

python/math.cc

@@ -80,7 +80,7 @@ void exposeMaths ()
   eigenpy::enableEigenPySpecific<Matrix3f>();
   eigenpy::enableEigenPySpecific<Vec3f   >();
 
-  class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), init<>(doxygen::constructor_doc<Transform3f>()))
+  class_ <Transform3f> ("Transform3f", doxygen::class_doc<Transform3f>(), init<>(arg("self"),doxygen::constructor_doc<Transform3f>()))
     .def (init<Matrix3f, Vec3f>    (doxygen::constructor_doc<Transform3f, const Matrix3f::MatrixBase&, const Vec3f::MatrixBase&>()))
     .def (init<Quaternion3f, Vec3f>(doxygen::constructor_doc<Transform3f, const Quaternion3f&        , const Vec3f::MatrixBase&>()))
     .def (init<Matrix3f>           (doxygen::constructor_doc<Transform3f, const Matrix3f&                                      >()))

src/narrowphase/details.h

@@ -2019,7 +2019,7 @@ namespace fcl {
     //                            FCL_REAL* penetration_depth, Vec3f* normal)
     {
       static const FCL_REAL eps (sqrt (std::numeric_limits<FCL_REAL>::epsilon()));
-      Plane new_s2 = transform(s2, tf2);
+      const Plane new_s2 = transform(s2, tf2);
 
       const Matrix3f& R = tf1.getRotation();
       const Vec3f& T = tf1.getTranslation();
@@ -2027,7 +2027,7 @@ namespace fcl {
       const Vec3f Q (R.transpose() * new_s2.n);
       const Vec3f A (Q.cwiseProduct(s1.halfSide));
 
-      FCL_REAL signed_dist = new_s2.signedDistance(T);
+      const FCL_REAL signed_dist = new_s2.signedDistance(T);
       distance = std::abs(signed_dist) - A.lpNorm<1>();
       if(distance > 0) {
         // Is the box above or below the plane
@@ -2048,11 +2048,6 @@ namespace fcl {
         return false;
       }
 
-      Vec3f axis[3];
-      axis[0] = R.col(0);
-      axis[1] = R.col(1);
-      axis[2] = R.col(2);
-
       // find the deepest point
       Vec3f p = T;
 
@@ -2065,29 +2060,29 @@ namespace fcl {
          std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>())
         {
           int sign2 = (A[0] > 0) ? -sign : sign;
-          p += R.col(0) * (s1.halfSide[0] * sign2);
+          p.noalias() += R.col(0) * (s1.halfSide[0] * sign2);
         }
       else if(std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() ||
               std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>())
         {
           int sign2 = (A[1] > 0) ? -sign : sign;
-          p += R.col(1) * (s1.halfSide[1] * sign2);
+          p.noalias() += R.col(1) * (s1.halfSide[1] * sign2);
         }
       else if(std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() ||
               std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>())
         {
           int sign2 = (A[2] > 0) ? -sign : sign;
-          p += R.col(2) * (s1.halfSide[2] * sign2);
+          p.noalias() += R.col(2) * (s1.halfSide[2] * sign2);
         }
       else
         {
           Vec3f tmp(sign * R * s1.halfSide);
-          p += (A.array() > 0).select (- tmp, tmp);
+          p.noalias() += (A.array() > 0).select (- tmp, tmp);
         }
 
       // compute the contact point by project the deepest point onto the plane
       if (signed_dist > 0) normal = -new_s2.n; else normal = new_s2.n;
-      p1 = p2 = p - new_s2.n * new_s2.signedDistance(p);
+      p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p);
 
       return true;
     }