feat: bind fcl convex type

Author: mjd3

README.md

@@ -17,6 +17,7 @@ This package also supports most of FCL's object shapes, including:
 * Ellipsoid
 * Capsule
 * Cone
+* Convex
 * Cylinder
 * Half-Space
 * Plane
@@ -25,10 +26,11 @@ This package also supports most of FCL's object shapes, including:
 
 ## Installation
 
-First, install [octomap](https://github.com/OctoMap/octomap), which is necessary using OcTree. For Ubuntu, using `sudo apt-get install liboctomap-dev`   
+First, install [octomap](https://github.com/OctoMap/octomap), which is necessary to use OcTree. For Ubuntu, use `sudo apt-get install liboctomap-dev`.
 Second, install FCL using the instructions provided [here](https://github.com/flexible-collision-library/fcl).
 If you're on Ubuntu 17.04 or newer, you can install FCL using `sudo apt-get install libfcl-dev`.
 Otherwise, just compile FCL from source -- it's quick and easy, and its dependencies are all easily installed via `apt` or `brew`.
+Note: the provided install scripts (under `build_dependencies`) can automate this process as well.
 
 In order to install the Python wrappers for FCL, simply run
 ```shell
@@ -39,8 +41,7 @@ pip install python-fcl
 
 ### Collision Objects
 The primary construct in FCL is the `CollisionObject`, which forms the backbone of all collision and distance computations.
-A `CollisionObject` consists of two components -- its geometry, defined by a `CollisionGeometry` object,
-  and its pose, defined by a `Transform` object.
+A `CollisionObject` consists of two components -- its geometry, defined by a `CollisionGeometry` object, and its pose, defined by a `Transform` object.
 
 #### Collision Geometries
 There are two main types of `CollisionGeometry` objects -- geometric primitives, such as boxes and spheres,
@@ -88,6 +89,20 @@ m.addSubModel(verts, tris)
 m.endModel()
 ```
 
+If the mesh is convex, such as the example above, you can also wrap it in the `Convex` class. Note that the instantiation is a bit different because the `Convex` class supports arbitrary polygons for each face of the convex object.
+```python
+verts = np.array([[1.0, 1.0, 1.0],
+                  [2.0, 1.0, 1.0],
+                  [1.0, 2.0, 1.0],
+                  [1.0, 1.0, 2.0]])
+tris  = np.array([[0,2,1],
+                  [0,3,2],
+                  [0,1,3],
+                  [1,2,3]])
+faces = np.concatenate((3 * np.ones((len(tris), 1), dtype=np.int64), tris), axis=1).flatten()
+c = fcl.Convex(verts, len(tris), faces)
+```
+
 #### Transforms
 In addition to a `CollisionGeometry`, a `CollisionObject` requires a `Transform`, which tells FCL where the `CollisionGeometry` is actually located in the world.
 All `Transform` objects specify a rigid transformation (i.e. a rotation and a translation).

src/fcl/__init__.py

@@ -1,25 +1,26 @@
 try:
     from .fcl import (
-        CollisionObject,
-        CollisionGeometry,
-        Transform,
-        TriangleP,
         Box,
-        Sphere,
-        Ellipsoid,
+        BVHModel,
         Capsule,
+        CollisionGeometry,
+        CollisionObject,
         Cone,
+        Convex,
         Cylinder,
+        DynamicAABBTreeCollisionManager,
+        Ellipsoid,
         Halfspace,
-        Plane,
-        BVHModel,
         OcTree,
-        DynamicAABBTreeCollisionManager,
+        Plane,
+        Sphere,
+        Transform,
+        TriangleP,
         collide,
         continuousCollide,
-        distance,
         defaultCollisionCallback,
         defaultDistanceCallback,
+        distance,
     )
 except ModuleNotFoundError:
     import traceback
@@ -28,21 +29,60 @@
     print("Failed to import fcl.fcl. It is probably not correctly compiled.")
 
 from .collision_data import (
-    OBJECT_TYPE,
     NODE_TYPE,
+    OBJECT_TYPE,
     CCDMotionType,
     CCDSolverType,
-    GJKSolverType,
-    Contact,
-    CostSource,
+    CollisionData,
     CollisionRequest,
     CollisionResult,
+    Contact,
     ContinuousCollisionRequest,
     ContinuousCollisionResult,
+    CostSource,
+    DistanceData,
     DistanceRequest,
     DistanceResult,
-    CollisionData,
-    DistanceData,
+    GJKSolverType,
 )
-
 from .version import __version__
+
+__all__ = [
+    "CollisionObject",
+    "CollisionGeometry",
+    "Transform",
+    "TriangleP",
+    "Box",
+    "Sphere",
+    "Ellipsoid",
+    "Capsule",
+    "Cone",
+    "Convex",
+    "Cylinder",
+    "Halfspace",
+    "Plane",
+    "BVHModel",
+    "OcTree",
+    "DynamicAABBTreeCollisionManager",
+    "collide",
+    "continuousCollide",
+    "distance",
+    "defaultCollisionCallback",
+    "defaultDistanceCallback",
+    "OBJECT_TYPE",
+    "NODE_TYPE",
+    "CCDMotionType",
+    "CCDSolverType",
+    "GJKSolverType",
+    "Contact",
+    "CostSource",
+    "CollisionRequest",
+    "CollisionResult",
+    "ContinuousCollisionRequest",
+    "ContinuousCollisionResult",
+    "DistanceRequest",
+    "DistanceResult",
+    "CollisionData",
+    "DistanceData",
+    "__version__",
+]

src/fcl/fcl.pyx

@@ -332,6 +332,20 @@ cdef class Cone(CollisionGeometry):
         def __set__(self, value):
             (<defs.Coned*> self.thisptr).lz = <double?> value
 
+cdef class Convex(CollisionGeometry):
+    def __cinit__(self, vertices, num_faces, faces):
+        cdef vector[defs.Vector3d] vs
+        cdef vector[int] fs
+        for vert in vertices:
+            vs.push_back(numpy_to_vec3d(vert))
+        for face in faces:
+            fs.push_back(face)
+        self.thisptr = new defs.Convexd(defs.make_shared[vector[defs.Vector3d]](vs), num_faces, defs.make_shared[vector[int]](fs))
+
+    property num_faces:
+        def __get__(self):
+            return (<defs.Convexd*> self.thisptr).getFaceCount()
+
 cdef class Cylinder(CollisionGeometry):
     def __cinit__(self, radius, lz):
         self.thisptr = new defs.Cylinderd(radius, lz)

src/fcl/fcl_defs.pxd

@@ -1,9 +1,10 @@
+cimport octomap_defs as octomap
 from libcpp cimport bool
+from libcpp.memory cimport make_shared, shared_ptr
+from libcpp.set cimport set
 from libcpp.string cimport string
 from libcpp.vector cimport vector
-from libcpp.set cimport set
-from libcpp.memory cimport shared_ptr
-cimport octomap_defs as octomap
+
 
 cdef extern from "Python.h":
        ctypedef struct PyObject
@@ -231,12 +232,8 @@ cdef extern from "fcl/geometry/shape/cylinder.h" namespace "fcl":
 
 cdef extern from "fcl/geometry/shape/convex.h" namespace "fcl":
     cdef cppclass Convexd(ShapeBased):
-        Convexd(Vector3d* plane_nomals_,
-               double* plane_dis_,
-               int num_planes,
-               Vector3d* points_,
-               int num_points_,
-               int* polygons_) except +
+        Convexd(const shared_ptr[const vector[Vector3d]]& vertices, int num_faces, const shared_ptr[const vector[int]]& faces) except +
+        int getFaceCount()
 
 cdef extern from "fcl/geometry/shape/halfspace.h" namespace "fcl":
     cdef cppclass Halfspaced(ShapeBased):