Repository: BerkeleyAutomation/python-fcl (language: python)

Issue:
Make example python3 compatible

nearest points inconsistent
Hello,

I was using this library to do simple 2d collision detection. However, when using the collision managers to get nearest points (witness points), I am seeing some inconsistencies - basically the distance between the nearest points does NOT equal to min_distance. A supposedly reproducible code is here:
```import numpy as np
import fcl
from scipy.spatial.transform import Rotation
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
import matplotlib.patheffects as path_effects
from matplotlib.cm import get_cmap
import seaborn as sns
sns.set()

box1, box1_t = (20., 5.), (-2.0,0.0)
box2, box2_t = (5,5), (-5., 10.)
box3, box3_t = (5, 5), (-15., 15.)
theta1, theta2 = np.pi/3, 0
theta3 = 0


h = 1000
box1_bottom_left, box2_bottom_left, box3_bottom_left = (-box1[0]/2, -box1[1]/2), (-box2[0]/2, -box2[1]/2), (-box3[0]/2, -box3[0]/2)
g1 = fcl.Box(*box1, h)
t1 = fcl.Transform(Rotation.from_rotvec([0., 0., theta1]).as_quat()[[3,0,1,2]], [*box1_t,0])
o1 = fcl.CollisionObject(g1, t1)
g2 = fcl.Box(*box2, h)
t2 = fcl.Transform(Rotation.from_rotvec([0., 0., theta2]).as_quat()[[3,0,1,2]], [*box2_t,0])
o2 = fcl.CollisionObject(g2, t2)
g3 = fcl.Box(*box3, h)
t3 = fcl.Transform(Rotation.from_rotvec([0., 0., theta3]).as_quat()[[3,0,1,2]], [*box3_t,0])
o3 = fcl.CollisionObject(g3, t3)

robot_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects([o1])
robot_manager.setup()
obs_manager = fcl.DynamicAABBTreeCollisionManager()
obs_manager.registerObjects([o2, o3])
# obs_manager.registerObjects([o3, o2])

obs_manager.setup()
ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True))
robot_manager.distance(obs_manager, ddata, fcl.defaultDistanceCallback)
assert ddata.result.min_distance == np.linalg.norm(ddata.result.nearest_points[1][:2]-ddata.result.nearest_points[0][:2]), \
    (ddata.result.min_distance, np.linalg.norm(ddata.result.nearest_points[1][:2]-ddata.result.nearest_points[0][:2]))

## below are just visualization
box1_bottom_left = (Rotation.from_rotvec([0., 0., theta1]).as_matrix() @ np.array([*box1_bottom_left, 0]))[:2]+box1_t
box2_bottom_left = (Rotation.from_rotvec([0., 0., theta2]).as_matrix() @ np.array([*box2_bottom_left, 0]))[:2]+box2_t
box3_bottom_left = (Rotation.from_rotvec([0., 0., theta3]).as_matrix() @ np.array([*box3_bottom_left, 0]))[:2]+box3_t

p_o1 = ddata.result.nearest_points[0][:2]
p_o2 = ddata.result.nearest_points[1][:2]
fig, ax = plt.subplots(1,1)
ax.set_xlim(-30, 30)
ax.set_ylim(-30, 30)
ax.set_aspect('equal', adjustable='box')

cmaps = [get_cmap('Reds'), get_cmap('Blues')]
with sns.axes_style('ticks'):
    ax.add_patch(Rectangle(tuple(box1_bottom_left), *box1, angle=np.degrees(theta1), path_effects=[path_effects.withSimplePatchShadow()], color=cmaps[1](0.5), alpha=0.5))
    ax.add_patch(Rectangle(tuple(box2_bottom_left), *box2, angle=np.degrees(theta2), path_effects=[path_effects.withSimplePatchShadow()], color=cmaps[0](0.5), alpha=0.5))
    ax.add_patch(Rectangle(tuple(box3_bottom_left), *box3, angle=np.degrees(theta3), path_effects=[path_effects.withSimplePatchShadow()], color=cmaps[0](0.5), alpha=0.5))
ax.plot(*p_o1, 'o', color=cmaps[1](0.5), markersize=4)
ax.plot(*p_o2, 'o', color=cmaps[0](0.5), markersize=4)
plt.show(block=True)
```
You can uncomment one of these two lines and you should be able to see the nearest points are not consistent in two runs:
```
obs_manager.registerObjects([o2, o3])
# obs_manager.registerObjects([o3, o2])
```

Interestingly, I tried using raw FCL and it does not have the problem. Code also here:
```#include <fcl/fcl.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include "math.h"

using namespace fcl;
int main(int argc, char** argv)
{
  DistanceResultd result;
  DistanceRequestd request;

  std::shared_ptr<CollisionGeometryd> box_geometry_1(
    new Boxd(20, 5, 1000));
  std::shared_ptr<CollisionGeometryd> box_geometry_2(
    new Boxd(5, 5, 1000));
  std::shared_ptr<CollisionGeometryd> box_geometry_3(
    new Boxd(5, 5, 1000));
  CollisionObjectd box_object_1(
    box_geometry_1, Eigen:: Quaterniond(std::cos(M_PI/6), 0, 0, std::sin(M_PI/6)).matrix(),
    Eigen::Vector3d(-2, 0.000000, 0.00000));
  CollisionObjectd box_object_2(
    box_geometry_2, Eigen:: Quaterniond(1, 0, 0, 0).matrix(),
    Eigen::Vector3d(-5, 10, 0.00000));
  CollisionObjectd box_object_3(
    box_geometry_3, Eigen:: Quaterniond(1, 0, 0, 0).matrix(),
    Eigen::Vector3d(-5, 15, 0.00000));
  
  auto manager1 = new DynamicAABBTreeCollisionManagerd();
  auto manager2 = new DynamicAABBTreeCollisionManagerd();
  std::vector<CollisionObjectd*> robot;
  robot.push_back(&box_object_1);
  std::vector<CollisionObjectd*> obs;
  obs.push_back(&box_object_3);
  obs.push_back(&box_object_2);
  manager1->registerObjects(robot);
  manager2->registerObjects(obs);
  
  DefaultDistanceData<double> ddata;
  manager1->distance(manager2, &ddata, DefaultDistanceFunction<double>);
  std::cout << ddata.result.nearest_points[0] << std::endl << ddata.result.nearest_points[1] << std::endl;
  std::cout << sqrt((ddata.result.nearest_points[1]-ddata.result.nearest_points[0]).squaredNorm()) << std::endl;
  std::cout << ddata.result.min_distance << std::endl;
  return 0;
}
```


enable signed distances for correct nearest points calculation for intersecting objects 
I tried to use the fcl-python bindings for collision avoidance in a gradient based optimization algorithm, but was not able to calculate the nearest points for intersecting objects correctly. I think after exposing the enable_signed_distances parameter to python this should work now.
fix updating of nearest points, etc
The logic of updating the nearest points, object ids, etc. in the function "distance" is problematic, resulting in wrong nearest points as described in #28. This PR provides a straightforward fix. Even though the example in the issue still does not pass, it is merely because fcl 0.5 returns the nearest points in the object frame. 

Also added a function to get the geometry object from a CollisionObject. 

This is the same fix as this [PR](https://github.com/CyrilWaechter/python-fcl/pull/1).


Discussion / hints:


Thanks for the PR! Could you add a unit test with a simple example (i.e. two boxes or spheres or something)?  

Also no big deal but any idea why those wheels got added to this PR? Was it the Github Actions setting or something? I can remove them in a branch, just curious haha. 
I have added a small example where the enable_signed_distance parameter is used. Is that what you where looking for?

Ah yeah, building the wheels and installing the library from there was the only way I could think of to include the changes I made. Probably not the smartest way!
Hi there, I believe this is a super simple fix to a relatively important issue, which hopefully should not take more than 10 minutes to review. The issue should be pretty obvious once seeing the one line of fix. But feel free to close the PR if the corresponding problem has been fixed by other commits. Thanks.

Fix patch:
diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml
index 7858377..d01cb83 100644
--- a/.github/workflows/push.yml
+++ b/.github/workflows/push.yml
@@ -1,8 +1,25 @@
 name: Build
 
-on: [push, pull_request]
+on: [push, pull_request, workflow_dispatch]
 
 jobs:
+  formatting:
+    name: Check Formatting
+    runs-on: ubuntu-latest
+    steps:
+    - name: Check out source repository
+      uses: actions/checkout@v2
+    - name: Set up Python environment
+      uses: actions/setup-python@v2
+      with:
+        python-version: 3.9
+    - name: flake8
+      uses: py-actions/flake8@v2
+    - name: black
+      uses: psf/black@stable
+      with:
+        options: "--check --diff -l 90"
+
   build_wheels:
     name: Build wheel on ${{matrix.platform}}
     runs-on: ${{matrix.platform}}
diff --git a/.travis.yml b/.travis.yml
deleted file mode 100644
index 5172712..0000000
--- a/.travis.yml
+++ /dev/null
@@ -1,31 +0,0 @@
-env:
-  global:
-  - TWINE_USERNAME=mmatl
-matrix:
-  include:
-  - dist: trusty
-    sudo: required
-    language: python
-    python: '3.6'
-    services:
-    - docker
-install:
-- if [ "${TRAVIS_OS_NAME:-}" == "osx" ]; then brew install python3; fi
-- pip3 install cibuildwheel==0.11.1
-script:
-- cibuildwheel --output-dir wheelhouse
-- |
-  if [[ $TRAVIS_TAG ]]; then
-    python -m pip install twine
-    python -m twine upload wheelhouse/*.whl
-  fi
-deploy:
-  provider: pypi
-  skip_existing: true
-  user: mmatl
-  on:
-    tags: true
-    branch: master
-  password:
-    secure: ZPKskVP8SkdEk3biP9Wq1C4kr66WBIm6aT5WidJtx2gR55iz+LJQDNHSAHxRdp3A0q1dSqVbqT9Ltc5Grs4Bv8o7po1fys6isg8UPQtlKP06UsBOgpKoILx4lw1e+5MDr86PBVTjF31cenhcl4PcpvbjecWe8qqhrS6miGLWhoIPoIKsxNOW/wgQym4viYuz5ErSfwY80wK5PTW8+fESZQTemjN+WPAWkJC6aHtdYPNzZCninBVKaC38+8jn6BtuQoA55aPH+QNbWx1pVVEE8VdaAVxNd6vKMbeRSDUEwtAiaYdJwJQzLn6P7SsW9+i560VIUDkH7AXAOvs4DJCLREUmZ6EucYmfO0fbAZQGQQjv61QptsN7TQmWiXN+6cxcWwwRF519uf7I2LGacBRIyagQ0XvzcMCJmZINiN7r+ep1mdTy8M9LP4WB9hxBcpj65QqQIpBTjPof89e3nmXgXSJyyOXYW/gkE8Y8CnLw/2JgbW4TLllv9kMMlViSvFmlDooITF+U+KGQWl+DEy3Ks8NJQnGZXNvERcv2M3Wm+bnOeikppSbfB8y0Ee6YSNCKSYFuvqZ24yjQYYop5fYfea5maWGT5brrnM2Iw2hGlPtAmQSw8DRMWIx2muRwPNnGJAeoh6CclSGm8o1fXVNkjeRWGCOhnwCXeu9pN3RlTak=
-  distributions: sdist bdist_wheel
diff --git a/README.md b/README.md
index bfe9822..5eff2a6 100644
--- a/README.md
+++ b/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).
@@ -347,4 +362,4 @@ for coll_pair in objs_in_collision:
 ```
 >>> Object obj1 in collision with object obj2!
 ```
-For more examples, see `example/example.py`.
+For more examples, see `examples/example.py`.
diff --git a/example/example.py b/example/example.py
deleted file mode 100644
index 4c42710..0000000
--- a/example/example.py
+++ /dev/null
@@ -1,190 +0,0 @@
-import numpy as np
-import fcl
-
-def print_collision_result(o1_name, o2_name, result):
-    print 'Collision between {} and {}:'.format(o1_name, o2_name)
-    print '-'*30
-    print 'Collision?: {}'.format(result.is_collision)
-    print 'Number of contacts: {}'.format(len(result.contacts))
-    print ''
-
-def print_continuous_collision_result(o1_name, o2_name, result):
-    print 'Continuous collision between {} and {}:'.format(o1_name, o2_name)
-    print '-'*30
-    print 'Collision?: {}'.format(result.is_collide)
-    print 'Time of collision: {}'.format(result.time_of_contact)
-    print ''
-
-def print_distance_result(o1_name, o2_name, result):
-    print 'Distance between {} and {}:'.format(o1_name, o2_name)
-    print '-'*30
-    print 'Distance: {}'.format(result.min_distance)
-    print 'Closest Points:'
-    print result.nearest_points[0]
-    print result.nearest_points[1]
-    print ''
-
-# Create simple geometries
-box = fcl.Box(1.0, 2.0, 3.0)
-sphere = fcl.Sphere(4.0)
-cone = fcl.Cone(5.0, 6.0)
-cyl = fcl.Cylinder(2.0, 2.0)
-
-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]])
-
-# Create mesh geometry
-mesh = fcl.BVHModel()
-mesh.beginModel(len(verts), len(tris))
-mesh.addSubModel(verts, tris)
-mesh.endModel()
-
-#=====================================================================
-# Pairwise collision checking
-#=====================================================================
-print '='*60
-print 'Testing Pairwise Collision Checking'
-print '='*60
-print ''
-
-req = fcl.CollisionRequest(enable_contact=True)
-res = fcl.CollisionResult()
-
-n_contacts = fcl.collide(fcl.CollisionObject(box, fcl.Transform()),
-                         fcl.CollisionObject(cone, fcl.Transform()),
-                         req, res)
-print_collision_result('Box', 'Cone', res)
-
-n_contacts = fcl.collide(fcl.CollisionObject(box, fcl.Transform()),
-                         fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0,0.0,0.0]))),
-                         req, res)
-print_collision_result('Box', 'Cylinder', res)
-
-n_contacts = fcl.collide(fcl.CollisionObject(mesh, fcl.Transform(np.array([0.0,0.0,-1.0]))),
-                         fcl.CollisionObject(cyl, fcl.Transform()),
-                         req, res)
-print_collision_result('Box', 'Mesh', res)
-
-#=====================================================================
-# Pairwise distance checking
-#=====================================================================
-print '='*60
-print 'Testing Pairwise Distance Checking'
-print '='*60
-print ''
-
-req = fcl.DistanceRequest(enable_nearest_points=True)
-res = fcl.DistanceResult()
-
-dist = fcl.distance(fcl.CollisionObject(box, fcl.Transform()),
-                    fcl.CollisionObject(cone, fcl.Transform()),
-                    req, res)
-print_distance_result('Box', 'Cone', res)
-
-dist = fcl.distance(fcl.CollisionObject(box, fcl.Transform()),
-                    fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0,0.0,0.0]))),
-                    req, res)
-print_distance_result('Box', 'Cylinder', res)
-
-dist = fcl.distance(fcl.CollisionObject(box, fcl.Transform()),
-                    fcl.CollisionObject(box, fcl.Transform(np.array([1.01,0.0,0.0]))),
-                    req, res)
-print_distance_result('Box', 'Box', res)
-
-#=====================================================================
-# Pairwise continuous collision checking
-#=====================================================================
-print '='*60
-print 'Testing Pairwise Continuous Collision Checking'
-print '='*60
-print ''
-
-req = fcl.ContinuousCollisionRequest()
-res = fcl.ContinuousCollisionResult()
-
-dist = fcl.continuousCollide(fcl.CollisionObject(box, fcl.Transform()),
-                             fcl.Transform(np.array([5.0, 0.0, 0.0])),
-                             fcl.CollisionObject(cyl, fcl.Transform(np.array([5.0,0.0,0.0]))),
-                             fcl.Transform(np.array([0.0, 0.0, 0.0])),
-                             req, res)
-print_continuous_collision_result('Box', 'Cylinder', res)
-
-#=====================================================================
-# Managed collision checking
-#=====================================================================
-print '='*60
-print 'Testing Managed Collision and Distance Checking'
-print '='*60
-print ''
-objs1 = [fcl.CollisionObject(box, fcl.Transform(np.array([20,0,0]))), fcl.CollisionObject(sphere)]
-objs2 = [fcl.CollisionObject(cone), fcl.CollisionObject(mesh)]
-objs3 = [fcl.CollisionObject(box), fcl.CollisionObject(sphere)]
-
-manager1 = fcl.DynamicAABBTreeCollisionManager()
-manager2 = fcl.DynamicAABBTreeCollisionManager()
-manager3 = fcl.DynamicAABBTreeCollisionManager()
-
-manager1.registerObjects(objs1)
-manager2.registerObjects(objs2)
-manager3.registerObjects(objs3)
-
-manager1.setup()
-manager2.setup()
-manager3.setup()
-
-#=====================================================================
-# Managed internal (n^2) collision checking
-#=====================================================================
-cdata = fcl.CollisionData()
-manager1.collide(cdata, fcl.defaultCollisionCallback)
-print 'Collision within manager 1?: {}'.format(cdata.result.is_collision)
-print ''
-
-cdata = fcl.CollisionData()
-manager2.collide(cdata, fcl.defaultCollisionCallback)
-print 'Collision within manager 2?: {}'.format(cdata.result.is_collision)
-print ''
-
-##=====================================================================
-## Managed internal (n^2) distance checking
-##=====================================================================
-ddata = fcl.DistanceData()
-manager1.distance(ddata, fcl.defaultDistanceCallback)
-print 'Closest distance within manager 1?: {}'.format(ddata.result.min_distance)
-print ''
-
-ddata = fcl.DistanceData()
-manager2.distance(ddata, fcl.defaultDistanceCallback)
-print 'Closest distance within manager 2?: {}'.format(ddata.result.min_distance)
-print ''
-
-#=====================================================================
-# Managed one to many collision checking
-#=====================================================================
-req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
-rdata = fcl.CollisionData(request = req)
-
-manager1.collide(fcl.CollisionObject(mesh), rdata, fcl.defaultCollisionCallback)
-print 'Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision)
-print 'Contacts:'
-for c in rdata.result.contacts:
-    print '\tO1: {}, O2: {}'.format(c.o1, c.o2)
-print ''
-
-#=====================================================================
-# Managed many to many collision checking
-#=====================================================================
-rdata = fcl.CollisionData(request = req)
-manager3.collide(manager2, rdata, fcl.defaultCollisionCallback)
-print 'Collision between manager 2 and manager 3?: {}'.format(rdata.result.is_collision)
-print 'Contacts:'
-for c in rdata.result.contacts:
-    print '\tO1: {}, O2: {}'.format(c.o1, c.o2)
-print ''
-
diff --git a/examples/example.py b/examples/example.py
new file mode 100644
index 0000000..dcc51fd
--- /dev/null
+++ b/examples/example.py
@@ -0,0 +1,212 @@
+import numpy as np
+
+import fcl
+
+
+def print_collision_result(o1_name, o2_name, result):
+    print("Collision between {} and {}:".format(o1_name, o2_name))
+    print("-" * 30)
+    print("Collision?: {}".format(result.is_collision))
+    print("Number of contacts: {}".format(len(result.contacts)))
+    print("")
+
+
+def print_continuous_collision_result(o1_name, o2_name, result):
+    print("Continuous collision between {} and {}:".format(o1_name, o2_name))
+    print("-" * 30)
+    print("Collision?: {}".format(result.is_collide))
+    print("Time of collision: {}".format(result.time_of_contact))
+    print("")
+
+
+def print_distance_result(o1_name, o2_name, result):
+    print("Distance between {} and {}:".format(o1_name, o2_name))
+    print("-" * 30)
+    print("Distance: {}".format(result.min_distance))
+    print("Closest Points:")
+    print(result.nearest_points[0])
+    print(result.nearest_points[1])
+    print("")
+
+
+# Create simple geometries
+box = fcl.Box(1.0, 2.0, 3.0)
+sphere = fcl.Sphere(4.0)
+cone = fcl.Cone(5.0, 6.0)
+cyl = fcl.Cylinder(2.0, 2.0)
+
+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]])
+
+# Create mesh geometry
+mesh = fcl.BVHModel()
+mesh.beginModel(len(verts), len(tris))
+mesh.addSubModel(verts, tris)
+mesh.endModel()
+
+# =====================================================================
+# Pairwise collision checking
+# =====================================================================
+print("=" * 60)
+print("Testing Pairwise Collision Checking")
+print("=" * 60)
+print("")
+
+req = fcl.CollisionRequest(enable_contact=True)
+res = fcl.CollisionResult()
+
+n_contacts = fcl.collide(
+    fcl.CollisionObject(box, fcl.Transform()),
+    fcl.CollisionObject(cone, fcl.Transform()),
+    req,
+    res,
+)
+print_collision_result("Box", "Cone", res)
+
+n_contacts = fcl.collide(
+    fcl.CollisionObject(box, fcl.Transform()),
+    fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0, 0.0, 0.0]))),
+    req,
+    res,
+)
+print_collision_result("Box", "Cylinder", res)
+
+n_contacts = fcl.collide(
+    fcl.CollisionObject(mesh, fcl.Transform(np.array([0.0, 0.0, -1.0]))),
+    fcl.CollisionObject(cyl, fcl.Transform()),
+    req,
+    res,
+)
+print_collision_result("Box", "Mesh", res)
+
+# =====================================================================
+# Pairwise distance checking
+# =====================================================================
+print("=" * 60)
+print("Testing Pairwise Distance Checking")
+print("=" * 60)
+print("")
+
+req = fcl.DistanceRequest(enable_nearest_points=True, enable_signed_distance=True)
+res = fcl.DistanceResult()
+
+dist = fcl.distance(
+    fcl.CollisionObject(box, fcl.Transform()),
+    fcl.CollisionObject(cone, fcl.Transform()),
+    req,
+    res,
+)
+print_distance_result("Box", "Cone", res)
+
+dist = fcl.distance(
+    fcl.CollisionObject(box, fcl.Transform()),
+    fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0, 0.0, 0.0]))),
+    req,
+    res,
+)
+print_distance_result("Box", "Cylinder", res)
+
+dist = fcl.distance(
+    fcl.CollisionObject(box, fcl.Transform()),
+    fcl.CollisionObject(box, fcl.Transform(np.array([1.01, 0.0, 0.0]))),
+    req,
+    res,
+)
+print_distance_result("Box", "Box", res)
+
+# =====================================================================
+# Pairwise continuous collision checking
+# =====================================================================
+print("=" * 60)
+print("Testing Pairwise Continuous Collision Checking")
+print("=" * 60)
+print("")
+
+req = fcl.ContinuousCollisionRequest()
+res = fcl.ContinuousCollisionResult()
+
+dist = fcl.continuousCollide(
+    fcl.CollisionObject(box, fcl.Transform()),
+    fcl.Transform(np.array([5.0, 0.0, 0.0])),
+    fcl.CollisionObject(cyl, fcl.Transform(np.array([5.0, 0.0, 0.0]))),
+    fcl.Transform(np.array([0.0, 0.0, 0.0])),
+    req,
+    res,
+)
+print_continuous_collision_result("Box", "Cylinder", res)
+
+# =====================================================================
+# Managed collision checking
+# =====================================================================
+print("=" * 60)
+print("Testing Managed Collision and Distance Checking")
+print("=" * 60)
+print("")
+objs1 = [
+    fcl.CollisionObject(box, fcl.Transform(np.array([20, 0, 0]))),
+    fcl.CollisionObject(sphere),
+]
+objs2 = [fcl.CollisionObject(cone), fcl.CollisionObject(mesh)]
+objs3 = [fcl.CollisionObject(box), fcl.CollisionObject(sphere)]
+
+manager1 = fcl.DynamicAABBTreeCollisionManager()
+manager2 = fcl.DynamicAABBTreeCollisionManager()
+manager3 = fcl.DynamicAABBTreeCollisionManager()
+
+manager1.registerObjects(objs1)
+manager2.registerObjects(objs2)
+manager3.registerObjects(objs3)
+
+manager1.setup()
+manager2.setup()
+manager3.setup()
+
+# =====================================================================
+# Managed internal (n^2) collision checking
+# =====================================================================
+cdata = fcl.CollisionData()
+manager1.collide(cdata, fcl.defaultCollisionCallback)
+print("Collision within manager 1?: {}".format(cdata.result.is_collision))
+print("")
+
+cdata = fcl.CollisionData()
+manager2.collide(cdata, fcl.defaultCollisionCallback)
+print("Collision within manager 2?: {}".format(cdata.result.is_collision))
+print("")
+
+# =====================================================================
+# Managed internal (n^2) distance checking
+# =====================================================================
+ddata = fcl.DistanceData()
+manager1.distance(ddata, fcl.defaultDistanceCallback)
+print("Closest distance within manager 1?: {}".format(ddata.result.min_distance))
+print("")
+
+ddata = fcl.DistanceData()
+manager2.distance(ddata, fcl.defaultDistanceCallback)
+print("Closest distance within manager 2?: {}".format(ddata.result.min_distance))
+print("")
+
+# =====================================================================
+# Managed one to many collision checking
+# =====================================================================
+req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
+rdata = fcl.CollisionData(request=req)
+
+manager1.collide(fcl.CollisionObject(mesh), rdata, fcl.defaultCollisionCallback)
+print("Collision between manager 1 and Mesh?: {}".format(rdata.result.is_collision))
+print("Contacts:")
+for c in rdata.result.contacts:
+    print("\tO1: {}, O2: {}".format(c.o1, c.o2))
+print("")
+
+# =====================================================================
+# Managed many to many collision checking
+# =====================================================================
+rdata = fcl.CollisionData(request=req)
+manager3.collide(manager2, rdata, fcl.defaultCollisionCallback)
+print("Collision between manager 2 and manager 3?: {}".format(rdata.result.is_collision))
+print("Contacts:")
+for c in rdata.result.contacts:
+    print("\tO1: {}, O2: {}".format(c.o1, c.o2))
+print("")
diff --git a/pyproject.toml b/pyproject.toml
index ef88f68..d0847cf 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -5,7 +5,7 @@ build-backend = "setuptools.build_meta"
 [tool.cibuildwheel]
 skip = "pp*"
 test-requires = "pytest"
-test-command = "pytest {package}/test"
+test-command = "pytest {package}/tests"
 
 [tool.cibuildwheel.linux]
 before-all = "bash build_dependencies/install_linux.sh"
diff --git a/setup.cfg b/setup.cfg
index 4b431f7..c7a244e 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -41,4 +41,17 @@ install_requires =
 * = *.pyx, *.pxd, *.dll
 
 [options.packages.find]
-where = src
\ No newline at end of file
+where = src
+
+[flake8]
+max_line_length = 90
+ignore = W503,W504,E203,E266,E501,E731,C901,B903,B009,B010
+max-complexity = 18
+select = B,C,E,F,W,T4,B9
+exclude =
+    .git
+    __pycache__
+    build
+    dist
+    tests
+    .eggs
\ No newline at end of file
diff --git a/setup.py b/setup.py
index a7fb666..15e69dd 100644
--- a/setup.py
+++ b/setup.py
@@ -1,8 +1,8 @@
 import os
 import sys
 
-from setuptools import Extension, setup
 from Cython.Build import cythonize
+from setuptools import Extension, setup
 
 INSTALL_PREFIX_WIN = "deps\\install"
 
diff --git a/src/fcl/__init__.py b/src/fcl/__init__.py
index 5a6e4af..6fa2688 100644
--- a/src/fcl/__init__.py
+++ b/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__",
+]
diff --git a/src/fcl/collision_data.py b/src/fcl/collision_data.py
index bf01996..0297173 100644
--- a/src/fcl/collision_data.py
+++ b/src/fcl/collision_data.py
@@ -2,14 +2,35 @@
 
 import numpy as np
 
+
 class OBJECT_TYPE:
     OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT = range(5)
 
 
 class NODE_TYPE:
-    BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24, \
-    GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, \
-    GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT = range(21)
+    (
+        BV_UNKNOWN,
+        BV_AABB,
+        BV_OBB,
+        BV_RSS,
+        BV_kIOS,
+        BV_OBBRSS,
+        BV_KDOP16,
+        BV_KDOP18,
+        BV_KDOP24,
+        GEOM_BOX,
+        GEOM_SPHERE,
+        GEOM_ELLIPSOID,
+        GEOM_CAPSULE,
+        GEOM_CONE,
+        GEOM_CYLINDER,
+        GEOM_CONVEX,
+        GEOM_PLANE,
+        GEOM_HALFSPACE,
+        GEOM_TRIANGLE,
+        GEOM_OCTREE,
+        NODE_COUNT,
+    ) = range(21)
 
 
 class CCDMotionType:
@@ -17,12 +38,18 @@ class CCDMotionType:
 
 
 class CCDSolverType:
-    CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER = range(4)
+    (
+        CCDC_NAIVE,
+        CCDC_CONSERVATIVE_ADVANCEMENT,
+        CCDC_RAY_SHOOTING,
+        CCDC_POLYNOMIAL_SOLVER,
+    ) = range(4)
 
 
 class GJKSolverType:
     GST_LIBCCD, GST_INDEP = range(2)
 
+
 class Contact:
     def __init__(self):
         self.o1 = None
@@ -33,6 +60,7 @@ def __init__(self):
         self.pos = np.array([0.0, 0.0, 0.0])
         self.penetration_depth = 0.0
 
+
 class CostSource:
     def __init__(self):
         self.aabb_min = np.array([0.0, 0.0, 0.0])
@@ -40,14 +68,17 @@ def __init__(self):
         self.cost_density = 0.0
         self.total_cost = 0.0
 
+
 class CollisionRequest:
-    def __init__(self,
-                 num_max_contacts=1,
-                 enable_contact=False,
-                 num_max_cost_sources=1,
-                 enable_cost=False,
-                 use_approximate_cost=True,
-                 gjk_solver_type=GJKSolverType.GST_LIBCCD):
+    def __init__(
+        self,
+        num_max_contacts=1,
+        enable_contact=False,
+        num_max_cost_sources=1,
+        enable_cost=False,
+        use_approximate_cost=True,
+        gjk_solver_type=GJKSolverType.GST_LIBCCD,
+    ):
         self.num_max_contacts = num_max_contacts
         self.enable_contact = enable_contact
         self.num_max_cost_sources = num_max_cost_sources
@@ -55,37 +86,48 @@ def __init__(self,
         self.use_approximate_cost = use_approximate_cost
         self.gjk_solver_type = gjk_solver_type
 
+
 class CollisionResult:
     def __init__(self, is_collision=False):
         self.is_collision = False
         self.contacts = []
         self.cost_sources = []
 
+
 class ContinuousCollisionRequest:
-    def __init__(self,
-                 num_max_iterations=10,
-                 toc_err=0.0001,
-                 ccd_motion_type=CCDMotionType.CCDM_TRANS,
-                 gjk_solver_type=GJKSolverType.GST_LIBCCD,
-                 ccd_solver_type=CCDSolverType.CCDC_CONSERVATIVE_ADVANCEMENT):
+    def __init__(
+        self,
+        num_max_iterations=10,
+        toc_err=0.0001,
+        ccd_motion_type=CCDMotionType.CCDM_TRANS,
+        gjk_solver_type=GJKSolverType.GST_LIBCCD,
+        ccd_solver_type=CCDSolverType.CCDC_CONSERVATIVE_ADVANCEMENT,
+    ):
         self.num_max_iterations = num_max_iterations
         self.toc_err = toc_err
         self.ccd_motion_type = ccd_motion_type
         self.gjk_solver_type = gjk_solver_type
         self.ccd_solver_type = ccd_solver_type
 
+
 class ContinuousCollisionResult:
     def __init__(self, is_collide=False, time_of_contact=1.0):
         self.is_collide = is_collide
         self.time_of_contact = time_of_contact
 
+
 class DistanceRequest:
-    def __init__(self,
-                 enable_nearest_points=False,
-                 gjk_solver_type=GJKSolverType.GST_LIBCCD):
+    def __init__(
+        self,
+        enable_nearest_points=False,
+        enable_signed_distance=False,
+        gjk_solver_type=GJKSolverType.GST_LIBCCD,
+    ):
         self.enable_nearest_points = enable_nearest_points
+        self.enable_signed_distance = enable_signed_distance
         self.gjk_solver_type = gjk_solver_type
 
+
 class DistanceResult:
     def __init__(self, min_distance_=sys.float_info.max):
         self.min_distance = min_distance_
@@ -95,6 +137,7 @@ def __init__(self, min_distance_=sys.float_info.max):
         self.b1 = -1
         self.b2 = -1
 
+
 class CollisionData:
     def __init__(self, request=None, result=None):
         if request is None:
@@ -105,6 +148,7 @@ def __init__(self, request=None, result=None):
         self.result = result
         self.done = False
 
+
 class DistanceData:
     def __init__(self, request=None, result=None):
         if request is None:
@@ -114,4 +158,3 @@ def __init__(self, request=None, result=None):
         self.request = request
         self.result = result
         self.done = False
-
diff --git a/src/fcl/fcl.pyx b/src/fcl/fcl.pyx
index ff8209e..7738bad 100644
--- a/src/fcl/fcl.pyx
+++ b/src/fcl/fcl.pyx
@@ -1,20 +1,35 @@
 # cython: language_level=2
+from libc.stdlib cimport free
+from libc.string cimport memcpy
 from libcpp cimport bool
 from libcpp.string cimport string
 from libcpp.vector cimport vector
-from libc.stdlib cimport free
-from libc.string cimport memcpy
+
 import inspect
 
-from cython.operator cimport dereference as deref, preincrement as inc, address
 cimport numpy as np
+from cython.operator cimport address
+from cython.operator cimport dereference as deref
+from cython.operator cimport preincrement as inc
+
 import numpy
+
 ctypedef np.float64_t DOUBLE_t
 
 cimport fcl_defs as defs
-cimport octomap_defs as octomap 
-cimport std_defs as std 
-from collision_data import Contact, CostSource, CollisionRequest, ContinuousCollisionRequest, CollisionResult, ContinuousCollisionResult, DistanceRequest, DistanceResult
+cimport octomap_defs as octomap
+cimport std_defs as std
+
+from collision_data import (
+    CollisionRequest,
+    CollisionResult,
+    Contact,
+    ContinuousCollisionRequest,
+    ContinuousCollisionResult,
+    CostSource,
+    DistanceRequest,
+    DistanceResult,
+)
 
 """
 Eigen::Transform linear and translation parts are returned as Eigen::Block
@@ -317,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)
@@ -440,7 +469,7 @@ cdef class OcTree(CollisionGeometry):
         cdef vector[char] vd = data
         ss.write(vd.data(), len(data))
 
-        self.tree = new octomap.OcTree(r) 
+        self.tree = new octomap.OcTree(r)
         self.tree.readBinaryData(ss)
         self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree))
 
@@ -650,17 +679,19 @@ def distance(CollisionObject o1, CollisionObject o2,
     cdef double dis = defs.distance(o1.thisptr, o2.thisptr,
                                     defs.DistanceRequestd(
                                         <bool?> request.enable_nearest_points,
+                                        <bool?> request.enable_signed_distance,
                                         <defs.GJKSolverType?> request.gjk_solver_type
                                     ),
                                     cresult)
 
-    result.min_distance = min(cresult.min_distance, result.min_distance)
-    result.nearest_points = [vec3d_to_numpy(cresult.nearest_points[0]),
-                             vec3d_to_numpy(cresult.nearest_points[1])]
-    result.o1 = c_to_python_collision_geometry(cresult.o1, o1, o2)
-    result.o2 = c_to_python_collision_geometry(cresult.o2, o1, o2)
-    result.b1 = cresult.b1
-    result.b2 = cresult.b2
+    if result.min_distance > cresult.min_distance:
+        result.min_distance = cresult.min_distance
+        result.nearest_points = [vec3d_to_numpy(cresult.nearest_points[0]),
+                                vec3d_to_numpy(cresult.nearest_points[1])]
+        result.o1 = c_to_python_collision_geometry(cresult.o1, o1, o2)
+        result.o2 = c_to_python_collision_geometry(cresult.o2, o1, o2)
+        result.b1 = cresult.b1
+        result.b2 = cresult.b2
     return dis
 
 ###############################################################################
diff --git a/src/fcl/fcl_defs.pxd b/src/fcl/fcl_defs.pxd
index 2bbe78a..cb1205c 100644
--- a/src/fcl/fcl_defs.pxd
+++ b/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
@@ -135,8 +136,9 @@ cdef extern from "fcl/narrowphase/distance_result.h" namespace "fcl":
 cdef extern from "fcl/narrowphase/distance_request.h" namespace "fcl":
     cdef cppclass DistanceRequestd:
         bool enable_nearest_points
+        bool enable_signed_distance
         GJKSolverType gjk_solver_type
-        DistanceRequestd(bool enable_nearest_points_, GJKSolverType gjk_solver_type_) except +
+        DistanceRequestd(bool enable_nearest_points_, bool enable_signed_distance, GJKSolverType gjk_solver_type_) except +
 
 cdef extern from "fcl/geometry/collision_geometry.h" namespace "fcl":
     cdef enum OBJECT_TYPE:
@@ -230,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):
@@ -408,4 +406,3 @@ cdef extern from "fcl/geometry/octree/octree.h" namespace "fcl":
         # Constructing
         OcTreed(double resolution) except +
         OcTreed(shared_ptr[octomap.OcTree]& tree_) except +
-
diff --git a/src/fcl/octomap_defs.pxd b/src/fcl/octomap_defs.pxd
index 0240ecc..196b37c 100644
--- a/src/fcl/octomap_defs.pxd
+++ b/src/fcl/octomap_defs.pxd
@@ -1,4 +1,5 @@
-cimport std_defs as std 
+cimport std_defs as std
+
 
 cdef extern from "octomap/OccupancyOcTreeBase.h" namespace "octomap":
     # Cython only accepts type template parameters.
@@ -14,6 +15,5 @@ cdef extern from "octomap/OccupancyOcTreeBase.h" namespace "octomap":
 
 cdef extern from "octomap/OcTree.h" namespace "octomap":
     cdef cppclass OcTree(OccupancyOcTreeBase):
-        # Constructing 
+        # Constructing
         OcTree(double resolution) except +
-
diff --git a/src/fcl/std_defs.pxd b/src/fcl/std_defs.pxd
index e1431e5..5b2cd68 100644
--- a/src/fcl/std_defs.pxd
+++ b/src/fcl/std_defs.pxd
@@ -8,5 +8,3 @@ cdef extern from "<iostream>" namespace "std":
 
     cdef cppclass stringstream(iostream):
         stringstream() except +
-
-
diff --git a/src/fcl/version.py b/src/fcl/version.py
index 8411e55..43c4ab0 100644
--- a/src/fcl/version.py
+++ b/src/fcl/version.py
@@ -1,1 +1,1 @@
-__version__ = '0.6.1'
+__version__ = "0.6.1"


Test patch:
diff --git a/test/test_fcl.py b/tests/test_fcl.py
similarity index 53%
rename from test/test_fcl.py
rename to tests/test_fcl.py
index 65a180b..11e5932 100644
--- a/test/test_fcl.py
+++ b/tests/test_fcl.py
@@ -1,63 +1,93 @@
 import unittest
-import fcl
-import sys
 
 import numpy as np
 
-class TestFCL(unittest.TestCase):
+import fcl
+
 
+class TestFCL(unittest.TestCase):
     def setUp(self):
-        verts = np.array([[0.0, 0.0, 0.0],
-                          [1.0, 0.0, 0.0],
-                          [0.0, 1.0, 0.0],
-                          [0.0, 0.0, 1.0]])
-        tris  = np.array([[0,2,1],
-                          [0,3,2],
-                          [0,1,3],
-                          [1,2,3]])
+        verts = np.array(
+            [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
+        )
+        tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]])
         mesh = fcl.BVHModel()
         mesh.beginModel(len(verts), len(tris))
         mesh.addSubModel(verts, tris)
         mesh.endModel()
 
+        verts = np.array(
+            [
+                [-0.5, -0.5, -0.5],
+                [-0.5, 0.5, -0.5],
+                [-0.5, 0.5, 0.5],
+                [-0.5, -0.5, 0.5],
+                [0.5, -0.5, -0.5],
+                [0.5, 0.5, -0.5],
+                [0.5, 0.5, 0.5],
+                [0.5, -0.5, 0.5],
+            ]
+        )
+        quads = np.array(
+            [
+                [0, 3, 2, 1],
+                [4, 5, 6, 7],
+                [4, 7, 3, 0],
+                [5, 1, 2, 6],
+                [4, 0, 1, 5],
+                [7, 6, 2, 3],
+            ]
+        )
+
+        faces = np.concatenate(
+            (np.ones((len(quads), 1), dtype=np.int32) * 4, quads), axis=1
+        ).flatten()
+        convex = fcl.Convex(verts, len(quads), faces)
+
         self.geometry = {
-            'box'      : fcl.Box(1.0, 1.0, 1.0),
-            'sphere'   : fcl.Sphere(1.0),
-            'cone'     : fcl.Cone(1.0, 1.0),
-            'cylinder' : fcl.Cylinder(1.0, 1.0),
-            'mesh'     : mesh
+            "box": fcl.Box(1.0, 1.0, 1.0),
+            "sphere": fcl.Sphere(1.0),
+            "cone": fcl.Cone(1.0, 1.0),
+            "cylinder": fcl.Cylinder(1.0, 1.0),
+            "mesh": mesh,
+            "convex": convex,
         }
 
-        self.crequest = fcl.CollisionRequest(
-            num_max_contacts = 100,
-            enable_contact = True
-        )
+        self.crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
         self.drequest = fcl.DistanceRequest(
-            enable_nearest_points = True
+            enable_nearest_points=True, enable_signed_distance=True
         )
 
-        self.x_axis_rot = np.array([[1.0, 0.0, 0.0],
-                                    [0.0, 0.0, -1.0],
-                                    [0.0, 1.0, 0.0]])
-
+        self.x_axis_rot = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]])
 
     def test_pairwise_collisions(self):
         result = fcl.CollisionResult()
 
-        box = fcl.CollisionObject(self.geometry['box'])
-        cone = fcl.CollisionObject(self.geometry['cone'])
-        mesh = fcl.CollisionObject(self.geometry['mesh'])
+        box = fcl.CollisionObject(self.geometry["box"])
+        cone = fcl.CollisionObject(self.geometry["cone"])
+        mesh = fcl.CollisionObject(self.geometry["mesh"])
+        convex = fcl.CollisionObject(self.geometry["convex"])
 
         result = fcl.CollisionResult()
         ret = fcl.collide(box, cone, self.crequest, result)
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
+        result = fcl.CollisionResult()
+        ret = fcl.collide(convex, cone, self.crequest, result)
+        self.assertTrue(ret > 0)
+        self.assertTrue(result.is_collision)
+
         result = fcl.CollisionResult()
         ret = fcl.collide(box, mesh, self.crequest, result)
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
+        result = fcl.CollisionResult()
+        ret = fcl.collide(convex, mesh, self.crequest, result)
+        self.assertTrue(ret > 0)
+        self.assertTrue(result.is_collision)
+
         result = fcl.CollisionResult()
         ret = fcl.collide(cone, mesh, self.crequest, result)
         self.assertTrue(ret > 0)
@@ -70,6 +100,11 @@ def test_pairwise_collisions(self):
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
+        result = fcl.CollisionResult()
+        ret = fcl.collide(convex, cone, self.crequest, result)
+        self.assertTrue(ret > 0)
+        self.assertTrue(result.is_collision)
+
         result = fcl.CollisionResult()
         ret = fcl.collide(cone, mesh, self.crequest, result)
         self.assertTrue(ret == 0)
@@ -83,6 +118,11 @@ def test_pairwise_collisions(self):
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
+        result = fcl.CollisionResult()
+        ret = fcl.collide(convex, cone, self.crequest, result)
+        self.assertTrue(ret > 0)
+        self.assertTrue(result.is_collision)
+
         cone.setTranslation(np.array([0.0, -1.1, 0.0]))
 
         result = fcl.CollisionResult()
@@ -90,20 +130,36 @@ def test_pairwise_collisions(self):
         self.assertTrue(ret == 0)
         self.assertFalse(result.is_collision)
 
+        result = fcl.CollisionResult()
+        ret = fcl.collide(convex, cone, self.crequest, result)
+        self.assertTrue(ret == 0)
+        self.assertFalse(result.is_collision)
+
     def test_pairwise_distances(self):
         result = fcl.DistanceResult()
 
-        box = fcl.CollisionObject(self.geometry['box'])
-        cone = fcl.CollisionObject(self.geometry['cone'])
-        mesh = fcl.CollisionObject(self.geometry['mesh'])
+        box = fcl.CollisionObject(self.geometry["box"])
+        cone = fcl.CollisionObject(self.geometry["cone"])
+        mesh = fcl.CollisionObject(self.geometry["mesh"])
+        convex = fcl.CollisionObject(self.geometry["convex"])
 
         result = fcl.DistanceResult()
-        ret = fcl.distance(box, cone, self.drequest, result)
-        self.assertTrue(ret < 0)
+        ret_box = fcl.distance(box, cone, self.drequest, result)
+        self.assertTrue(ret_box < 0)
 
         result = fcl.DistanceResult()
-        ret = fcl.distance(box, mesh, self.drequest, result)
-        self.assertTrue(ret < 0)
+        ret_convex = fcl.distance(convex, cone, self.drequest, result)
+        self.assertTrue(ret_convex < 0)
+        self.assertAlmostEqual(ret_convex, ret_box, places=6)
+
+        result = fcl.DistanceResult()
+        ret_box = fcl.distance(box, mesh, self.drequest, result)
+        self.assertTrue(ret_box < 0)
+
+        result = fcl.DistanceResult()
+        ret_convex = fcl.distance(convex, mesh, self.drequest, result)
+        self.assertTrue(ret_convex < 0)
+        self.assertAlmostEqual(ret_convex, ret_box, places=6)
 
         result = fcl.DistanceResult()
         ret = fcl.distance(cone, mesh, self.drequest, result)
@@ -117,50 +173,72 @@ def test_pairwise_distances(self):
 
         result = fcl.DistanceResult()
         ret = fcl.distance(cone, mesh, self.drequest, result)
-        self.assertAlmostEqual(ret, 0.1)
+        self.assertAlmostEqual(ret, 0.1, places=6)
 
         cone.setTranslation(np.array([0.0, -0.9, 0.0]))
         cone.setRotation(self.x_axis_rot)
 
         result = fcl.DistanceResult()
-        ret = fcl.distance(box, cone, self.drequest, result)
-        self.assertTrue(ret < 0)
+        ret_box = fcl.distance(box, cone, self.drequest, result)
+        self.assertTrue(ret_box < 0)
+
+        result = fcl.DistanceResult()
+        ret_convex = fcl.distance(convex, cone, self.drequest, result)
+        self.assertTrue(ret_convex < 0)
+        self.assertAlmostEqual(ret_box, ret_convex, places=6)
 
         cone.setTranslation(np.array([0.0, -1.1, 0.0]))
 
         result = fcl.DistanceResult()
         ret = fcl.distance(box, cone, self.drequest, result)
-        self.assertAlmostEqual(ret, 0.1)
+        self.assertAlmostEqual(ret, 0.1, places=6)
+
+        result = fcl.DistanceResult()
+        ret = fcl.distance(convex, cone, self.drequest, result)
+        self.assertAlmostEqual(ret, 0.1, places=6)
 
     def test_pairwise_continuous_collisions(self):
         request = fcl.ContinuousCollisionRequest()
         result = fcl.ContinuousCollisionResult()
 
-        box = fcl.CollisionObject(self.geometry['box'])
-        cone = fcl.CollisionObject(self.geometry['cone'],
-                                   fcl.Transform(np.array([0.0, 0.0, -2.0])))
+        box = fcl.CollisionObject(self.geometry["box"])
+        cone = fcl.CollisionObject(
+            self.geometry["cone"], fcl.Transform(np.array([0.0, 0.0, -2.0]))
+        )
 
-        ret = fcl.continuousCollide(box, fcl.Transform(),
-                                    cone, fcl.Transform(),
-                                    request, result)
+        ret = fcl.continuousCollide(
+            box, fcl.Transform(), cone, fcl.Transform(), request, result
+        )
 
-        '''
+        """
         ## WHY DOES THIS FAIL ##
         self.assertTrue(result.is_collide)
         self.assertAlmostEqual(0.625, ret)
-        '''
+        """
 
     def test_managed_collisions(self):
         manager1 = fcl.DynamicAABBTreeCollisionManager()
         manager2 = fcl.DynamicAABBTreeCollisionManager()
         manager3 = fcl.DynamicAABBTreeCollisionManager()
 
-        objs1 = [fcl.CollisionObject(self.geometry['box']),
-                 fcl.CollisionObject(self.geometry['cylinder'])]
-        objs2 = [fcl.CollisionObject(self.geometry['cone'], fcl.Transform(np.array([0.0, 0.0, 5.0]))),
-                 fcl.CollisionObject(self.geometry['cylinder'], fcl.Transform(np.array([0.0, 0.0, -5.0])))]
-        objs3 = [fcl.CollisionObject(self.geometry['mesh']),
-                 fcl.CollisionObject(self.geometry['box'], fcl.Transform(np.array([0.0, 0.0, -5.0])))]
+        objs1 = [
+            fcl.CollisionObject(self.geometry["box"]),
+            fcl.CollisionObject(self.geometry["cylinder"]),
+        ]
+        objs2 = [
+            fcl.CollisionObject(
+                self.geometry["cone"], fcl.Transform(np.array([0.0, 0.0, 5.0]))
+            ),
+            fcl.CollisionObject(
+                self.geometry["cylinder"], fcl.Transform(np.array([0.0, 0.0, -5.0]))
+            ),
+        ]
+        objs3 = [
+            fcl.CollisionObject(self.geometry["mesh"]),
+            fcl.CollisionObject(
+                self.geometry["convex"], fcl.Transform(np.array([0.0, 0.0, -5.0]))
+            ),
+        ]
 
         manager1.registerObjects(objs1)
         manager2.registerObjects(objs2)
@@ -174,10 +252,11 @@ def test_managed_collisions(self):
         self.assertTrue(len(manager2.getObjects()) == 2)
         self.assertTrue(len(manager3.getObjects()) == 2)
 
-
         # One-to-many
-        o1 = fcl.CollisionObject(self.geometry['box'])
-        o2 = fcl.CollisionObject(self.geometry['cylinder'], fcl.Transform(np.array([0.0, 0.0, -4.6])))
+        o1 = fcl.CollisionObject(self.geometry["box"])
+        o2 = fcl.CollisionObject(
+            self.geometry["cylinder"], fcl.Transform(np.array([0.0, 0.0, -4.6]))
+        )
 
         cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
         manager1.collide(o1, cdata, fcl.defaultCollisionCallback)
@@ -220,8 +299,12 @@ def test_managed_collisions(self):
     def test_updates(self):
         manager = fcl.DynamicAABBTreeCollisionManager()
 
-        objs = [fcl.CollisionObject(self.geometry['sphere']),
-                fcl.CollisionObject(self.geometry['sphere'], fcl.Transform(np.array([0.0, 0.0, -5.0])))]
+        objs = [
+            fcl.CollisionObject(self.geometry["sphere"]),
+            fcl.CollisionObject(
+                self.geometry["sphere"], fcl.Transform(np.array([0.0, 0.0, -5.0]))
+            ),
+        ]
 
         manager.registerObjects(objs)
         manager.setup()
@@ -242,9 +325,15 @@ def test_updates(self):
     def test_many_objects(self):
         manager = fcl.DynamicAABBTreeCollisionManager()
 
-        objs = [fcl.CollisionObject(self.geometry['sphere']),
-                fcl.CollisionObject(self.geometry['sphere'], fcl.Transform(np.array([0.0, 0.0, -5.0]))),
-                fcl.CollisionObject(self.geometry['sphere'], fcl.Transform(np.array([0.0, 0.0, 5.0])))]
+        objs = [
+            fcl.CollisionObject(self.geometry["sphere"]),
+            fcl.CollisionObject(
+                self.geometry["sphere"], fcl.Transform(np.array([0.0, 0.0, -5.0]))
+            ),
+            fcl.CollisionObject(
+                self.geometry["sphere"], fcl.Transform(np.array([0.0, 0.0, 5.0]))
+            ),
+        ]
 
         manager.registerObjects(objs)
         manager.setup()
@@ -285,12 +374,24 @@ def test_managed_distances(self):
         manager2 = fcl.DynamicAABBTreeCollisionManager()
         manager3 = fcl.DynamicAABBTreeCollisionManager()
 
-        objs1 = [fcl.CollisionObject(self.geometry['box']),
-                 fcl.CollisionObject(self.geometry['cylinder'])]
-        objs2 = [fcl.CollisionObject(self.geometry['cone'], fcl.Transform(np.array([0.0, 0.0, 5.0]))),
-                 fcl.CollisionObject(self.geometry['cylinder'], fcl.Transform(np.array([0.0, 0.0, -5.0])))]
-        objs3 = [fcl.CollisionObject(self.geometry['mesh']),
-                 fcl.CollisionObject(self.geometry['box'], fcl.Transform(np.array([0.0, 0.0, -5.0])))]
+        objs1 = [
+            fcl.CollisionObject(self.geometry["box"]),
+            fcl.CollisionObject(self.geometry["cylinder"]),
+        ]
+        objs2 = [
+            fcl.CollisionObject(
+                self.geometry["cone"], fcl.Transform(np.array([0.0, 0.0, 5.0]))
+            ),
+            fcl.CollisionObject(
+                self.geometry["cylinder"], fcl.Transform(np.array([0.0, 0.0, -5.0]))
+            ),
+        ]
+        objs3 = [
+            fcl.CollisionObject(self.geometry["mesh"]),
+            fcl.CollisionObject(
+                self.geometry["convex"], fcl.Transform(np.array([0.0, 0.0, -5.0]))
+            ),
+        ]
 
         manager1.registerObjects(objs1)
         manager2.registerObjects(objs2)
@@ -305,9 +406,10 @@ def test_managed_distances(self):
         self.assertTrue(len(manager3.getObjects()) == 2)
 
         # One-to-many
-        o1 = fcl.CollisionObject(self.geometry['box'])
-        o2 = fcl.CollisionObject(self.geometry['cylinder'],
-                                 fcl.Transform(np.array([0.0, 0.0, -4.6])))
+        o1 = fcl.CollisionObject(self.geometry["box"])
+        o2 = fcl.CollisionObject(
+            self.geometry["cylinder"], fcl.Transform(np.array([0.0, 0.0, -4.6]))
+        )
 
         cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
         manager1.distance(o1, cdata, fcl.defaultDistanceCallback)
@@ -315,11 +417,11 @@ def test_managed_distances(self):
 
         cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
         manager1.distance(o2, cdata, fcl.defaultDistanceCallback)
-        assert abs(cdata.result.min_distance - 3.6) < 1e-4
-        
+        self.assertAlmostEqual(cdata.result.min_distance, 3.6, places=6)
+
         cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
         manager2.distance(o1, cdata, fcl.defaultDistanceCallback)
-        self.assertAlmostEqual(cdata.result.min_distance, 4.0)
+        self.assertAlmostEqual(cdata.result.min_distance, 4.0, places=6)
 
         cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
         manager2.distance(o2, cdata, fcl.defaultDistanceCallback)
@@ -332,12 +434,12 @@ def test_managed_distances(self):
 
         cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
         manager2.distance(cdata, fcl.defaultDistanceCallback)
-        self.assertAlmostEqual(cdata.result.min_distance, 9.0)
+        self.assertAlmostEqual(cdata.result.min_distance, 9.0, places=6)
 
         # Many-to-many, grouped
         cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
         manager1.distance(manager2, cdata, fcl.defaultDistanceCallback)
-        self.assertAlmostEqual(cdata.result.min_distance, 4.0)
+        self.assertAlmostEqual(cdata.result.min_distance, 4.0, places=6)
 
         cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult())
         manager2.distance(manager3, cdata, fcl.defaultDistanceCallback)
@@ -347,5 +449,52 @@ def test_managed_distances(self):
         manager1.distance(manager3, cdata, fcl.defaultDistanceCallback)
         self.assertTrue(cdata.result.min_distance < 0)
 
-if __name__ == '__main__':
+    # Tests with boxes to make sure nearest points are independent of manager order
+    def test_nearest_points(self):
+        box1, box1_t = (20.0, 5.0), (-2.0, 0.0)
+        box2, box2_t = (5, 5), (-5.0, 10.0)
+        box3, box3_t = (5, 5), (-15.0, 15.0)
+        q1 = np.array([0.8660254, 0.0, 0.0, 0.5])  # np.pi / 3 z-ax rotation
+        q2 = np.array([1.0, 0.0, 0.0, 0.0])  # no rotation
+
+        h = 1000
+        g1 = fcl.Box(*box1, h)
+        t1 = fcl.Transform(q1, [*box1_t, 0])
+        o1 = fcl.CollisionObject(g1, t1)
+        g2 = fcl.Box(*box2, h)
+        t2 = fcl.Transform(q2, [*box2_t, 0])
+        o2 = fcl.CollisionObject(g2, t2)
+        g3 = fcl.Box(*box3, h)
+        t3 = fcl.Transform(q2, [*box3_t, 0])
+        o3 = fcl.CollisionObject(g3, t3)
+
+        manager1 = fcl.DynamicAABBTreeCollisionManager()
+        manager1.registerObjects([o1])
+        manager1.setup()
+
+        manager2 = fcl.DynamicAABBTreeCollisionManager()
+        manager2r = fcl.DynamicAABBTreeCollisionManager()
+        manager2.registerObjects([o2, o3])
+        manager2r.registerObjects([o3, o2])
+        manager2.setup()
+        manager2r.setup()
+
+        ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True))
+        manager1.distance(manager2, ddata, fcl.defaultDistanceCallback)
+        self.assertAlmostEqual(
+            ddata.result.min_distance,
+            np.linalg.norm(np.subtract(*ddata.result.nearest_points)),
+            places=6,
+        )
+
+        ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True))
+        manager1.distance(manager2r, ddata, fcl.defaultDistanceCallback)
+        self.assertAlmostEqual(
+            ddata.result.min_distance,
+            np.linalg.norm(np.subtract(*ddata.result.nearest_points)),
+            places=6,
+        )
+
+
+if __name__ == "__main__":
     unittest.main()