From 5098f907f1de08a53227fd269aa55650b9215cca Mon Sep 17 00:00:00 2001 From: Dmitry Neverov Date: Mon, 27 Apr 2020 16:16:34 +0900 Subject: [PATCH 01/54] Buildable version with reduced functionality: Updated header file locations Disabled functions not present in Eigen objects Disabled distance function Set Eigen scalar template parameter to `double` everywhere --- fcl/__init__.py | 4 +- fcl/fcl.pyx | 370 ++++++++++++++++++------------------- fcl/fcl_defs.pxd | 463 ++++++++++++++++++++++++----------------------- setup.py | 2 +- 4 files changed, 426 insertions(+), 413 deletions(-) diff --git a/fcl/__init__.py b/fcl/__init__.py index e852be0..fae3eb8 100644 --- a/fcl/__init__.py +++ b/fcl/__init__.py @@ -1,5 +1,5 @@ -from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, distance, defaultCollisionCallback, defaultDistanceCallback - +from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, defaultCollisionCallback, defaultDistanceCallback +#distance from .collision_data import OBJECT_TYPE, NODE_TYPE, CCDMotionType, CCDSolverType, GJKSolverType, Contact, CostSource, CollisionRequest, CollisionResult, ContinuousCollisionRequest, ContinuousCollisionResult, DistanceRequest, DistanceResult, CollisionData, DistanceData from .version import __version__ diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx index ab28ca8..c867aa7 100644 --- a/fcl/fcl.pyx +++ b/fcl/fcl.pyx @@ -20,36 +20,36 @@ from collision_data import Contact, CostSource, CollisionRequest, ContinuousColl # Transforms ############################################################################### cdef class Transform: - cdef defs.Transform3f *thisptr + cdef defs.Transform3d *thisptr def __cinit__(self, *args): if len(args) == 0: - self.thisptr = new defs.Transform3f() + self.thisptr = new defs.Transform3d() elif len(args) == 1: if isinstance(args[0], Transform): - self.thisptr = new defs.Transform3f(deref(( args[0]).thisptr)) - else: - data = numpy.array(args[0]) - if data.shape == (3,3): - self.thisptr = new defs.Transform3f(numpy_to_mat3f(data)) - elif data.shape == (4,): - self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(data)) - elif data.shape == (3,): - self.thisptr = new defs.Transform3f(numpy_to_vec3f(data)) - else: - raise ValueError('Invalid input to Transform().') + self.thisptr = new defs.Transform3d(deref(( args[0]).thisptr)) +# else: +# data = numpy.array(args[0]) +# if data.shape == (3,3): +# self.thisptr = new defs.Transform3d(numpy_to_mat3d(data)) +# elif data.shape == (4,): +# self.thisptr = new defs.Transform3d(numpy_to_quaternion3d(data)) +# elif data.shape == (3,): +# self.thisptr = new defs.Transform3d(numpy_to_vec3d(data)) +# else: +# raise ValueError('Invalid input to Transform().') elif len(args) == 2: rot = numpy.array(args[0]) trans = numpy.array(args[1]).squeeze() if not trans.shape == (3,): raise ValueError('Translation must be (3,).') - if rot.shape == (3,3): - self.thisptr = new defs.Transform3f(numpy_to_mat3f(rot), numpy_to_vec3f(trans)) - elif rot.shape == (4,): - self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(rot), numpy_to_vec3f(trans)) - else: - raise ValueError('Invalid input to Transform().') +# if rot.shape == (3,3): +# self.thisptr = new defs.Transform3d(numpy_to_mat3d(rot), numpy_to_vec3d(trans)) +# elif rot.shape == (4,): +# self.thisptr = new defs.Transform3d(numpy_to_quaternion3d(rot), numpy_to_vec3d(trans)) +# else: +# raise ValueError('Invalid input to Transform().') else: raise ValueError('Too many arguments to Transform().') @@ -57,30 +57,30 @@ cdef class Transform: if self.thisptr: free(self.thisptr) - def getRotation(self): - return mat3f_to_numpy(self.thisptr.getRotation()) +# def getRotation(self): +# return mat3d_to_numpy(self.thisptr.getRotation()) - def getTranslation(self): - return vec3f_to_numpy(self.thisptr.getTranslation()) +# def getTranslation(self): +# return vec3d_to_numpy(self.thisptr.getTranslation()) - def getQuatRotation(self): - return quaternion3f_to_numpy(self.thisptr.getQuatRotation()) +# def getQuatRotation(self): +# return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) - def setRotation(self, R): - self.thisptr.setRotation(numpy_to_mat3f(R)) +# def setRotation(self, R): +# self.thisptr.setRotation(numpy_to_mat3d(R)) - def setTranslation(self, T): - self.thisptr.setTranslation(numpy_to_vec3f(T)) +# def setTranslation(self, T): +# self.thisptr.setTranslation(numpy_to_vec3d(T)) - def setQuatRotation(self, q): - self.thisptr.setQuatRotation(numpy_to_quaternion3f(q)) +# def setQuatRotation(self, q): +# self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) ############################################################################### # Collision objects and geometries ############################################################################### cdef class CollisionObject: - cdef defs.CollisionObject *thisptr + cdef defs.CollisionObjectd *thisptr cdef defs.PyObject *geom cdef bool _no_instance @@ -92,9 +92,9 @@ cdef class CollisionObject: self._no_instance = _no_instance if geom.getNodeType() is not None and not self._no_instance: if tf is not None: - self.thisptr = new defs.CollisionObject(defs.shared_ptr[defs.CollisionGeometry](geom.thisptr), deref(tf.thisptr)) + self.thisptr = new defs.CollisionObjectd(defs.shared_ptr[defs.CollisionGeometryd](geom.thisptr), deref(tf.thisptr)) else: - self.thisptr = new defs.CollisionObject(defs.shared_ptr[defs.CollisionGeometry](geom.thisptr)) + self.thisptr = new defs.CollisionObjectd(defs.shared_ptr[defs.CollisionGeometryd](geom.thisptr)) self.thisptr.setUserData( self.geom) # Save the python geometry object for later retrieval else: if not self._no_instance: @@ -112,24 +112,24 @@ cdef class CollisionObject: return self.thisptr.getNodeType() def getTranslation(self): - return vec3f_to_numpy(self.thisptr.getTranslation()) + return vec3d_to_numpy(self.thisptr.getTranslation()) def setTranslation(self, vec): - self.thisptr.setTranslation(numpy_to_vec3f(vec)) + self.thisptr.setTranslation(numpy_to_vec3d(vec)) self.thisptr.computeAABB() def getRotation(self): - return mat3f_to_numpy(self.thisptr.getRotation()) + return mat3d_to_numpy(self.thisptr.getRotation()) - def setRotation(self, mat): - self.thisptr.setRotation(numpy_to_mat3f(mat)) - self.thisptr.computeAABB() +# def setRotation(self, mat): +# self.thisptr.setRotation(numpy_to_mat3d(mat)) +# self.thisptr.computeAABB() - def getQuatRotation(self): - return quaternion3f_to_numpy(self.thisptr.getQuatRotation()) +# def getQuatRotation(self): +# return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) def setQuatRotation(self, q): - self.thisptr.setQuatRotation(numpy_to_quaternion3f(q)) + self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) self.thisptr.computeAABB() def getTransform(self): @@ -151,7 +151,7 @@ cdef class CollisionObject: return self.thisptr.isUncertain() cdef class CollisionGeometry: - cdef defs.CollisionGeometry *thisptr + cdef defs.CollisionGeometryd *thisptr def __cinit__(self): pass @@ -175,7 +175,7 @@ cdef class CollisionGeometry: property aabb_center: def __get__(self): if self.thisptr: - return vec3f_to_numpy(self.thisptr.aabb_center) + return vec3d_to_numpy(self.thisptr.aabb_center) else: return None def __set__(self, value): @@ -188,153 +188,153 @@ cdef class CollisionGeometry: cdef class TriangleP(CollisionGeometry): def __cinit__(self, a, b, c): - self.thisptr = new defs.TriangleP(numpy_to_vec3f(a), numpy_to_vec3f(b), numpy_to_vec3f(c)) + self.thisptr = new defs.TrianglePd(numpy_to_vec3d(a), numpy_to_vec3d(b), numpy_to_vec3d(c)) property a: def __get__(self): - return vec3f_to_numpy(( self.thisptr).a) + return vec3d_to_numpy(( self.thisptr).a) def __set__(self, value): - ( self.thisptr).a[0] = value[0] - ( self.thisptr).a[1] = value[1] - ( self.thisptr).a[2] = value[2] + ( self.thisptr).a[0] = value[0] + ( self.thisptr).a[1] = value[1] + ( self.thisptr).a[2] = value[2] property b: def __get__(self): - return vec3f_to_numpy(( self.thisptr).b) + return vec3d_to_numpy(( self.thisptr).b) def __set__(self, value): - ( self.thisptr).b[0] = value[0] - ( self.thisptr).b[1] = value[1] - ( self.thisptr).b[2] = value[2] + ( self.thisptr).b[0] = value[0] + ( self.thisptr).b[1] = value[1] + ( self.thisptr).b[2] = value[2] property c: def __get__(self): - return vec3f_to_numpy(( self.thisptr).c) + return vec3d_to_numpy(( self.thisptr).c) def __set__(self, value): - ( self.thisptr).c[0] = value[0] - ( self.thisptr).c[1] = value[1] - ( self.thisptr).c[2] = value[2] + ( self.thisptr).c[0] = value[0] + ( self.thisptr).c[1] = value[1] + ( self.thisptr).c[2] = value[2] cdef class Box(CollisionGeometry): def __cinit__(self, x, y, z): - self.thisptr = new defs.Box(x, y, z) + self.thisptr = new defs.Boxd(x, y, z) property side: def __get__(self): - return vec3f_to_numpy(( self.thisptr).side) + return vec3d_to_numpy(( self.thisptr).side) def __set__(self, value): - ( self.thisptr).side[0] = value[0] - ( self.thisptr).side[1] = value[1] - ( self.thisptr).side[2] = value[2] + ( self.thisptr).side[0] = value[0] + ( self.thisptr).side[1] = value[1] + ( self.thisptr).side[2] = value[2] cdef class Sphere(CollisionGeometry): def __cinit__(self, radius): - self.thisptr = new defs.Sphere(radius) + self.thisptr = new defs.Sphered(radius) property radius: def __get__(self): - return ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value cdef class Ellipsoid(CollisionGeometry): def __cinit__(self, a, b, c): - self.thisptr = new defs.Ellipsoid( a, b, c) + self.thisptr = new defs.Ellipsoidd( a, b, c) property radii: def __get__(self): - return vec3f_to_numpy(( self.thisptr).radii) + return vec3d_to_numpy(( self.thisptr).radii) def __set__(self, values): - ( self.thisptr).radii = numpy_to_vec3f(values) + ( self.thisptr).radii = numpy_to_vec3d(values) cdef class Capsule(CollisionGeometry): def __cinit__(self, radius, lz): - self.thisptr = new defs.Capsule(radius, lz) + self.thisptr = new defs.Capsuled(radius, lz) property radius: def __get__(self): - return ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = value cdef class Cone(CollisionGeometry): def __cinit__(self, radius, lz): - self.thisptr = new defs.Cone(radius, lz) + self.thisptr = new defs.Coned(radius, lz) property radius: def __get__(self): - return ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = value cdef class Cylinder(CollisionGeometry): def __cinit__(self, radius, lz): - self.thisptr = new defs.Cylinder(radius, lz) + self.thisptr = new defs.Cylinderd(radius, lz) property radius: def __get__(self): - return ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = value cdef class Halfspace(CollisionGeometry): def __cinit__(self, n, d): - self.thisptr = new defs.Halfspace(defs.Vec3f( n[0], + self.thisptr = new defs.Halfspaced(defs.Vector3d( n[0], n[1], n[2]), d) property n: def __get__(self): - return vec3f_to_numpy(( self.thisptr).n) + return vec3d_to_numpy(( self.thisptr).n) def __set__(self, value): - ( self.thisptr).n[0] = value[0] - ( self.thisptr).n[1] = value[1] - ( self.thisptr).n[2] = value[2] + ( self.thisptr).n[0] = value[0] + ( self.thisptr).n[1] = value[1] + ( self.thisptr).n[2] = value[2] property d: def __get__(self): - return ( self.thisptr).d + return ( self.thisptr).d def __set__(self, value): - ( self.thisptr).d = value + ( self.thisptr).d = value cdef class Plane(CollisionGeometry): def __cinit__(self, n, d): - self.thisptr = new defs.Plane(defs.Vec3f( n[0], + self.thisptr = new defs.Planed(defs.Vector3d( n[0], n[1], n[2]), d) property n: def __get__(self): - return vec3f_to_numpy(( self.thisptr).n) + return vec3d_to_numpy(( self.thisptr).n) def __set__(self, value): - ( self.thisptr).n[0] = value[0] - ( self.thisptr).n[1] = value[1] - ( self.thisptr).n[2] = value[2] + ( self.thisptr).n[0] = value[0] + ( self.thisptr).n[1] = value[1] + ( self.thisptr).n[2] = value[2] property d: def __get__(self): - return ( self.thisptr).d + return ( self.thisptr).d def __set__(self, value): - ( self.thisptr).d = value + ( self.thisptr).d = value cdef class BVHModel(CollisionGeometry): def __cinit__(self): @@ -355,20 +355,20 @@ cdef class BVHModel(CollisionGeometry): return n def addVertex(self, x, y, z): - n = ( self.thisptr).addVertex(defs.Vec3f( x, y, z)) + n = ( self.thisptr).addVertex(defs.Vector3d( x, y, z)) return self._check_ret_value(n) def addTriangle(self, v1, v2, v3): - n = ( self.thisptr).addTriangle(numpy_to_vec3f(v1), - numpy_to_vec3f(v2), - numpy_to_vec3f(v3)) + n = ( self.thisptr).addTriangle(numpy_to_vec3d(v1), + numpy_to_vec3d(v2), + numpy_to_vec3d(v3)) return self._check_ret_value(n) def addSubModel(self, verts, triangles): - cdef vector[defs.Vec3f] ps + cdef vector[defs.Vector3d] ps cdef vector[defs.Triangle] tris for vert in verts: - ps.push_back(numpy_to_vec3f(vert)) + ps.push_back(numpy_to_vec3d(vert)) for tri in triangles: tris.push_back(defs.Triangle( tri[0], tri[1], tri[2])) n = ( self.thisptr).addSubModel(ps, tris) @@ -406,7 +406,7 @@ cdef class OcTree(CollisionGeometry): self.tree = new octomap.OcTree(r) self.tree.readBinaryData(ss) - self.thisptr = new defs.OcTree(defs.shared_ptr[octomap.OcTree](self.tree)) + self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree)) ############################################################################### @@ -414,11 +414,11 @@ cdef class OcTree(CollisionGeometry): ############################################################################### cdef class DynamicAABBTreeCollisionManager: - cdef defs.DynamicAABBTreeCollisionManager *thisptr + cdef defs.DynamicAABBTreeCollisionManagerd *thisptr cdef list objs def __cinit__(self): - self.thisptr = new defs.DynamicAABBTreeCollisionManager() + self.thisptr = new defs.DynamicAABBTreeCollisionManagerd() self.objs = [] def __dealloc__(self): @@ -426,7 +426,7 @@ cdef class DynamicAABBTreeCollisionManager: del self.thisptr def registerObjects(self, other_objs): - cdef vector[defs.CollisionObject*] pobjs + cdef vector[defs.CollisionObjectd*] pobjs for obj in other_objs: self.objs.append(obj) pobjs.push_back(( obj).thisptr) @@ -445,7 +445,7 @@ cdef class DynamicAABBTreeCollisionManager: self.thisptr.setup() def update(self, arg=None): - cdef vector[defs.CollisionObject*] objs + cdef vector[defs.CollisionObjectd*] objs if hasattr(arg, "__len__"): for a in arg: objs.push_back(( a).thisptr) @@ -471,18 +471,18 @@ cdef class DynamicAABBTreeCollisionManager: else: raise ValueError - def distance(self, *args): - if len(args) == 2 and inspect.isroutine(args[1]): - fn = DistanceFunction(args[1], args[0]) - self.thisptr.distance( fn, DistanceCallBack) - elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): - fn = DistanceFunction(args[2], args[1]) - self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) - elif len(args) == 3 and inspect.isroutine(args[2]): - fn = DistanceFunction(args[2], args[1]) - self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) - else: - raise ValueError +# def distance(self, *args): +# if len(args) == 2 and inspect.isroutine(args[1]): +# fn = DistanceFunction(args[1], args[0]) +# self.thisptr.distance( fn, DistanceCallBack) +# elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): +# fn = DistanceFunction(args[2], args[1]) +# self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) +# elif len(args) == 3 and inspect.isroutine(args[2]): +# fn = DistanceFunction(args[2], args[1]) +# self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) +# else: +# raise ValueError def clear(self): self.thisptr.clear() @@ -547,10 +547,10 @@ def collide(CollisionObject o1, CollisionObject o2, if result is None: result = CollisionResult() - cdef defs.CollisionResult cresult + cdef defs.CollisionResultd cresult cdef size_t ret = defs.collide(o1.thisptr, o2.thisptr, - defs.CollisionRequest( + defs.CollisionRequestd( request.num_max_contacts, request.enable_contact, request.num_max_cost_sources, @@ -562,12 +562,12 @@ def collide(CollisionObject o1, CollisionObject o2, result.is_collision = result.is_collision or cresult.isCollision() - cdef vector[defs.Contact] contacts + cdef vector[defs.Contactd] contacts cresult.getContacts(contacts) for idx in range(contacts.size()): result.contacts.append(c_to_python_contact(contacts[idx], o1, o2)) - cdef vector[defs.CostSource] costs + cdef vector[defs.CostSourced] costs cresult.getCostSources(costs) for idx in range(costs.size()): result.cost_sources.append(c_to_python_costsource(costs[idx])) @@ -583,13 +583,13 @@ def continuousCollide(CollisionObject o1, Transform tf1_end, if result is None: result = ContinuousCollisionResult() - cdef defs.ContinuousCollisionResult cresult + cdef defs.ContinuousCollisionResultd cresult - cdef defs.FCL_REAL ret = defs.continuousCollide(o1.thisptr, deref(tf1_end.thisptr), + cdef double ret = defs.continuousCollide(o1.thisptr, deref(tf1_end.thisptr), o2.thisptr, deref(tf2_end.thisptr), - defs.ContinuousCollisionRequest( + defs.ContinuousCollisionRequestd( request.num_max_iterations, - request.toc_err, + request.toc_err, request.ccd_motion_type, request.gjk_solver_type, request.ccd_solver_type, @@ -601,31 +601,31 @@ def continuousCollide(CollisionObject o1, Transform tf1_end, result.time_of_contact = min(cresult.time_of_contact, result.time_of_contact) return ret -def distance(CollisionObject o1, CollisionObject o2, - request = None, result=None): - - if request is None: - request = DistanceRequest() - if result is None: - result = DistanceResult() - - cdef defs.DistanceResult cresult - - cdef double dis = defs.distance(o1.thisptr, o2.thisptr, - defs.DistanceRequest( - request.enable_nearest_points, - request.gjk_solver_type - ), - cresult) - - result.min_distance = min(cresult.min_distance, result.min_distance) - result.nearest_points = [vec3f_to_numpy(cresult.nearest_points[0]), - vec3f_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 +#def distance(CollisionObject o1, CollisionObject o2, +# request = None, result=None): +# +# if request is None: +# request = DistanceRequest() +# if result is None: +# result = DistanceResult() +# +# cdef defs.DistanceResultd cresult +# +# cdef double dis = defs.distance(o1.thisptr, o2.thisptr, +# defs.DistanceRequestd( +# request.enable_nearest_points, +# 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 +# return dis ############################################################################### # Collision and Distance Callback Functions @@ -651,9 +651,9 @@ def defaultDistanceCallback(CollisionObject o1, CollisionObject o2, cdata): if cdata.done: return True, result.min_distance - - distance(o1, o2, request, result) - +# +# distance(o1, o2, request, result) +# dist = result.min_distance if dist <= 0: @@ -670,7 +670,7 @@ cdef class CollisionFunction: self.py_func = py_func self.py_args = py_args - cdef bool eval_func(self, defs.CollisionObject*o1, defs.CollisionObject*o2): + cdef bool eval_func(self, defs.CollisionObjectd*o1, defs.CollisionObjectd*o2): cdef object py_r = defs.PyObject_CallObject(self.py_func, (copy_ptr_collision_object(o1), copy_ptr_collision_object(o2), @@ -686,18 +686,18 @@ cdef class DistanceFunction: self.py_func = py_func self.py_args = py_args - cdef bool eval_func(self, defs.CollisionObject*o1, defs.CollisionObject*o2, defs.FCL_REAL& dist): + cdef bool eval_func(self, defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, double& dist): cdef object py_r = defs.PyObject_CallObject(self.py_func, (copy_ptr_collision_object(o1), copy_ptr_collision_object(o2), self.py_args)) - (&dist)[0] = py_r[1] + (&dist)[0] = py_r[1] return py_r[0] -cdef inline bool CollisionCallBack(defs.CollisionObject*o1, defs.CollisionObject*o2, void*cdata): +cdef inline bool CollisionCallBack(defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, void*cdata): return ( cdata).eval_func(o1, o2) -cdef inline bool DistanceCallBack(defs.CollisionObject*o1, defs.CollisionObject*o2, void*cdata, defs.FCL_REAL& dist): +cdef inline bool DistanceCallBack(defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, void*cdata, double& dist): return ( cdata).eval_func(o1, o2, dist) @@ -705,56 +705,56 @@ cdef inline bool DistanceCallBack(defs.CollisionObject*o1, defs.CollisionObject* # Helper Functions ############################################################################### -cdef quaternion3f_to_numpy(defs.Quaternion3f q): - return numpy.array([q.getW(), q.getX(), q.getY(), q.getZ()]) +#cdef quaternion3d_to_numpy(defs.Quaterniond q): +# return numpy.array([q.getW(), q.getX(), q.getY(), q.getZ()]) -cdef defs.Quaternion3f numpy_to_quaternion3f(a): - return defs.Quaternion3f( a[0], a[1], a[2], a[3]) +cdef defs.Quaterniond numpy_to_quaternion3d(a): + return defs.Quaterniond( a[0], a[1], a[2], a[3]) -cdef vec3f_to_numpy(defs.Vec3f vec): +cdef vec3d_to_numpy(defs.Vector3d vec): return numpy.array([vec[0], vec[1], vec[2]]) -cdef defs.Vec3f numpy_to_vec3f(a): - return defs.Vec3f( a[0], a[1], a[2]) +cdef defs.Vector3d numpy_to_vec3d(a): + return defs.Vector3d( a[0], a[1], a[2]) -cdef mat3f_to_numpy(defs.Matrix3f m): +cdef mat3d_to_numpy(defs.Matrix3d m): return numpy.array([[m(0,0), m(0,1), m(0,2)], [m(1,0), m(1,1), m(1,2)], [m(2,0), m(2,1), m(2,2)]]) -cdef defs.Matrix3f numpy_to_mat3f(a): - return defs.Matrix3f( a[0][0], a[0][1], a[0][2], - a[1][0], a[1][1], a[1][2], - a[2][0], a[2][1], a[2][2]) +#cdef defs.Matrix3d numpy_to_mat3d(a): +# return defs.Matrix3d( a[0][0], a[0][1], a[0][2], +# a[1][0], a[1][1], a[1][2], +# a[2][0], a[2][1], a[2][2]) -cdef c_to_python_collision_geometry(defs.const_CollisionGeometry*geom, CollisionObject o1, CollisionObject o2): - cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) - cdef CollisionGeometry o2_py_geom = (( o2.thisptr).getUserData()) - if geom == o1_py_geom.thisptr: +cdef c_to_python_collision_geometry(defs.const_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2): + cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) + cdef CollisionGeometry o2_py_geom = (( o2.thisptr).getUserData()) + if geom == o1_py_geom.thisptr: return o1_py_geom else: return o2_py_geom -cdef c_to_python_contact(defs.Contact contact, CollisionObject o1, CollisionObject o2): +cdef c_to_python_contact(defs.Contactd contact, CollisionObject o1, CollisionObject o2): c = Contact() c.o1 = c_to_python_collision_geometry(contact.o1, o1, o2) c.o2 = c_to_python_collision_geometry(contact.o2, o1, o2) c.b1 = contact.b1 c.b2 = contact.b2 - c.normal = vec3f_to_numpy(contact.normal) - c.pos = vec3f_to_numpy(contact.pos) + c.normal = vec3d_to_numpy(contact.normal) + c.pos = vec3d_to_numpy(contact.pos) c.penetration_depth = contact.penetration_depth return c -cdef c_to_python_costsource(defs.CostSource cost_source): +cdef c_to_python_costsource(defs.CostSourced cost_source): c = CostSource() - c.aabb_min = vec3f_to_numpy(cost_source.aabb_min) - c.aabb_max = vec3f_to_numpy(cost_source.aabb_max) + c.aabb_min = vec3d_to_numpy(cost_source.aabb_min) + c.aabb_max = vec3d_to_numpy(cost_source.aabb_max) c.cost_density = cost_source.cost_density c.total_cost = cost_source.total_cost return c -cdef copy_ptr_collision_object(defs.CollisionObject*cobj): +cdef copy_ptr_collision_object(defs.CollisionObjectd*cobj): geom = cobj.getUserData() co = CollisionObject(geom, _no_instance=True) ( co).thisptr = cobj diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd index d5461fd..50dd926 100644 --- a/fcl/fcl_defs.pxd +++ b/fcl/fcl_defs.pxd @@ -18,136 +18,138 @@ cdef extern from "Python.h": # shared_ptr(T*) except + # T* get() -cdef extern from "fcl/data_types.h" namespace "fcl": - ctypedef double FCL_REAL - -cdef extern from "fcl/math/vec_3f.h" namespace "fcl": - cdef cppclass Vec3f: - Vec3f() except + - Vec3f(FCL_REAL x, FCL_REAL y, FCL_REAL z) except + - FCL_REAL& operator[](size_t i) - -cdef extern from "fcl/math/matrix_3f.h" namespace "fcl": - cdef cppclass Matrix3f: - Matrix3f() except + - Matrix3f(FCL_REAL xx, FCL_REAL xy, FCL_REAL xz, - FCL_REAL yx, FCL_REAL yy, FCL_REAL yz, - FCL_REAL zx, FCL_REAL zy, FCL_REAL zz) except + - FCL_REAL operator()(size_t i, size_t j) - -cdef extern from "fcl/math/transform.h" namespace "fcl": - cdef cppclass Quaternion3f: - Quaternion3f() except + - Quaternion3f(FCL_REAL a, FCL_REAL b, - FCL_REAL c, FCL_REAL d) except + - void fromRotation(Matrix3f& R) - void fromAxisAngle(Vec3f& axis, FCL_REAL angle) - FCL_REAL& getW() - FCL_REAL& getX() - FCL_REAL& getY() - FCL_REAL& getZ() - - cdef cppclass Transform3f: - Transform3f() except + - Transform3f(Matrix3f& R_, Vec3f& T_) - Transform3f(Quaternion3f& q_, Vec3f& T_) - Transform3f(Matrix3f& R_) - Transform3f(Quaternion3f& q_) - Transform3f(Vec3f& T_) - Transform3f(Transform3f& tf_) - Matrix3f& getRotation() - Vec3f& getTranslation() - Quaternion3f& getQuatRotation() - void setRotation(Matrix3f& R_) - void setTranslation(Vec3f& T_) - void setQuatRotation(Quaternion3f & q_) - -cdef extern from "fcl/collision_data.h" namespace "fcl": - +cdef extern from "fcl/common/types.h" namespace "fcl": + cdef cppclass Vector3d: + Vector3d() except + + Vector3d(double x, double y, double z) except + + double& operator[](size_t i) + + cdef cppclass Matrix3d: + Matrix3d() except + +# Matrix3d(double xx, double xy, double xz, +# double yx, double yy, double yz, +# double zx, double zy, double zz) except + + double operator()(size_t i, size_t j) + + cdef cppclass Quaterniond: + Quaterniond() except + + Quaterniond(double a, double b, + double c, double d) except + + void fromRotation(Matrix3d& R) + void fromAxisAngle(Vector3d& axis, double angle) +# double& getW() +# double& getX() +# double& getY() +# double& getZ() + + cdef cppclass Transform3d: + Transform3d() except + +# Transform3d(Matrix3d& R_, Vector3d& T_) +# Transform3d(Quaterniond& q_, Vector3d& T_) +# Transform3d(Matrix3d& R_) +# Transform3d(Quaterniond& q_) +# Transform3d(Vector3d& T_) + Transform3d(Transform3d& tf_) +# Matrix3d& getRotation() +# Vector3d& getTranslation() +# Quaterniond& getQuatRotation() +# void setRotation(Matrix3d& R_) +# void setTranslation(Vector3d& T_) +# void setQuatRotation(Quaterniond & q_) + +cdef extern from "fcl/narrowphase/continuous_collision_request.h" namespace "fcl": cdef enum CCDMotionType: CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE cdef enum CCDSolverType: CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER + cdef cppclass ContinuousCollisionRequestd: + size_t num_max_iterations_, + double toc_err_, + CCDMotionType ccd_motion_type_, + GJKSolverType gjk_solver_type_, + GJKSolverType ccd_solver_type_ + + ContinuousCollisionRequestd( + size_t num_max_iterations_, + double toc_err_, + CCDMotionType ccd_motion_type_, + GJKSolverType gjk_solver_type_, + CCDSolverType ccd_solver_type_ ) + +cdef extern from "fcl/narrowphase/gjk_solver_type.h" namespace "fcl": cdef enum GJKSolverType: GST_LIBCCD, GST_INDEP - cdef cppclass Contact: - CollisionGeometry *o1 - CollisionGeometry *o2 +cdef extern from "fcl/narrowphase/contact.h" namespace "fcl": + cdef cppclass Contactd: + CollisionGeometryd *o1 + CollisionGeometryd *o2 int b1 int b2 - Vec3f normal - Vec3f pos - FCL_REAL penetration_depth - Contact() except + - Contact(CollisionGeometry* o1_, - CollisionGeometry* o2_, + Vector3d normal + Vector3d pos + double penetration_depth + Contactd() except + + Contactd(CollisionGeometryd* o1_, + CollisionGeometryd* o2_, int b1_, int b2_) except + - cdef cppclass CostSource: - Vec3f aabb_min - Vec3f aabb_max - FCL_REAL cost_density - FCL_REAL total_cost +cdef extern from "fcl/narrowphase/cost_source.h" namespace "fcl": + cdef cppclass CostSourced: + Vector3d aabb_min + Vector3d aabb_max + double cost_density + double total_cost - cdef cppclass CollisionResult: - CollisionResult() except + +cdef extern from "fcl/narrowphase/collision_result.h" namespace "fcl": + cdef cppclass CollisionResultd: + CollisionResultd() except + bool isCollision() - void getContacts(vector[Contact]& contacts_) - void getCostSources(vector[CostSource]& cost_sources_) + void getContacts(vector[Contactd]& contacts_) + void getCostSources(vector[CostSourced]& cost_sources_) - cdef cppclass ContinuousCollisionResult: - ContinuousCollisionResult() except + +cdef extern from "fcl/narrowphase/continuous_collision_result.h" namespace "fcl": + cdef cppclass ContinuousCollisionResultd: + ContinuousCollisionResultd() except + bool is_collide - FCL_REAL time_of_contact - Transform3f contact_tf1, contact_tf2 + double time_of_contact + Transform3d contact_tf1, contact_tf2 - cdef cppclass CollisionRequest: +cdef extern from "fcl/narrowphase/collision_request.h" namespace "fcl": + cdef cppclass CollisionRequestd: size_t num_max_contacts bool enable_contact size_t num_max_cost_sources bool enable_cost bool use_approximate_cost GJKSolverType gjk_solver_type - CollisionRequest(size_t num_max_contacts_, + CollisionRequestd(size_t num_max_contacts_, bool enable_contact_, size_t num_max_cost_sources_, bool enable_cost_, bool use_approximate_cost_, GJKSolverType gjk_solver_type_) - cdef cppclass ContinuousCollisionRequest: - size_t num_max_iterations_, - FCL_REAL toc_err_, - CCDMotionType ccd_motion_type_, - GJKSolverType gjk_solver_type_, - GJKSolverType ccd_solver_type_ - - ContinuousCollisionRequest( - size_t num_max_iterations_, - FCL_REAL toc_err_, - CCDMotionType ccd_motion_type_, - GJKSolverType gjk_solver_type_, - CCDSolverType ccd_solver_type_ ) - - cdef cppclass DistanceResult: - FCL_REAL min_distance - Vec3f* nearest_points - CollisionGeometry* o1 - CollisionGeometry* o2 +cdef extern from "fcl/narrowphase/distance_result.h" namespace "fcl": + cdef cppclass DistanceResultd: + double min_distance + Vector3d* nearest_points + CollisionGeometryd* o1 + CollisionGeometryd* o2 int b1 int b2 - DistanceResult(FCL_REAL min_distance_) except + - DistanceResult() except + + DistanceResultd(double min_distance_) except + + DistanceResultd() except + - cdef cppclass DistanceRequest: +cdef extern from "fcl/narrowphase/distance_request.h" namespace "fcl": + cdef cppclass DistanceRequestd: bool enable_nearest_points GJKSolverType gjk_solver_type - DistanceRequest(bool enable_nearest_points_, GJKSolverType gjk_solver_type_) except + + DistanceRequestd(bool enable_nearest_points_, GJKSolverType gjk_solver_type_) except + -cdef extern from "fcl/collision_object.h" namespace "fcl": +cdef extern from "fcl/geometry/collision_geometry.h" namespace "fcl": cdef enum OBJECT_TYPE: OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT cdef enum NODE_TYPE: @@ -155,34 +157,37 @@ cdef extern from "fcl/collision_object.h" namespace "fcl": GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT - cdef cppclass CollisionGeometry: - CollisionGeometry() except + + cdef cppclass CollisionGeometryd: + CollisionGeometryd() except + OBJECT_TYPE getObjectType() NODE_TYPE getNodeType() void computeLocalAABB() - Vec3f aabb_center - FCL_REAL aabb_radius - FCL_REAL cost_density - FCL_REAL threshold_occupied - FCL_REAL threshold_free - - cdef cppclass CollisionObject: - CollisionObject() except + - CollisionObject(shared_ptr[CollisionGeometry]& cgeom_) except + - CollisionObject(shared_ptr[CollisionGeometry]& cgeom_, Transform3f& tf) except + + Vector3d aabb_center + double aabb_radius + double cost_density + double threshold_occupied + double threshold_free + + ctypedef CollisionGeometryd const_CollisionGeometryd "const fcl::CollisionGeometryd" + +cdef extern from "fcl/narrowphase/collision_object.h" namespace "fcl": + cdef cppclass CollisionObjectd: + CollisionObjectd() except + + CollisionObjectd(shared_ptr[CollisionGeometryd]& cgeom_) except + + CollisionObjectd(shared_ptr[CollisionGeometryd]& cgeom_, Transform3d& tf) except + OBJECT_TYPE getObjectType() NODE_TYPE getNodeType() - Vec3f& getTranslation() - Matrix3f& getRotation() - Quaternion3f& getQuatRotation() - Transform3f& getTransform() - CollisionGeometry* getCollisionGeometry() - void setTranslation(Vec3f& T) - void setRotation(Matrix3f& M) - void setQuatRotation(Quaternion3f& q) - void setTransform(Quaternion3f& q, Vec3f& T) - void setTransform(Matrix3f& q, Vec3f& T) - void setTransform(Transform3f& tf) + Vector3d& getTranslation() + Matrix3d& getRotation() + Quaterniond& getQuatRotation() + Transform3d& getTransform() + CollisionGeometryd* getCollisionGeometry() + void setTranslation(Vector3d& T) + void setRotation(Matrix3d& M) + void setQuatRotation(Quaterniond& q) + void setTransform(Quaterniond& q, Vector3d& T) + void setTransform(Matrix3d& q, Vector3d& T) + void setTransform(Transform3d& tf) void setUserData(void *data) void computeAABB() void *getUserData() @@ -190,82 +195,91 @@ cdef extern from "fcl/collision_object.h" namespace "fcl": bool isFree() bool isUncertain() - ctypedef CollisionGeometry const_CollisionGeometry "const fcl::CollisionGeometry" - ctypedef CollisionObject const_CollisionObject "const fcl::CollisionObject" - -cdef extern from "fcl/shape/geometric_shapes.h" namespace "fcl": - cdef cppclass ShapeBase(CollisionGeometry): - ShapeBase() except + - - cdef cppclass TriangleP(ShapeBase): - TriangleP(Vec3f& a_, Vec3f& b_, Vec3f& c_) except + - Vec3f a, b, c - - cdef cppclass Box(ShapeBase): - Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) except + - Vec3f side - - cdef cppclass Sphere(ShapeBase): - Sphere(FCL_REAL radius_) except + - FCL_REAL radius - - cdef cppclass Ellipsoid(ShapeBase): - Ellipsoid(FCL_REAL a_, FCL_REAL b_, FCL_REAL c_) except + - Vec3f radii - - cdef cppclass Capsule(ShapeBase): - Capsule(FCL_REAL radius_, FCL_REAL lz_) except + - FCL_REAL radius - FCL_REAL lz - - cdef cppclass Cone(ShapeBase): - Cone(FCL_REAL radius_, FCL_REAL lz_) except + - FCL_REAL radius - FCL_REAL lz - - cdef cppclass Cylinder(ShapeBase): - Cylinder(FCL_REAL radius_, FCL_REAL lz_) except + - FCL_REAL radius - FCL_REAL lz - - cdef cppclass Convex(ShapeBase): - Convex(Vec3f* plane_nomals_, - FCL_REAL* plane_dis_, + ctypedef CollisionObjectd const_CollisionObjectd "const fcl::CollisionObjectd" + +cdef extern from "fcl/geometry/shape/shape_base.h" namespace "fcl": + cdef cppclass ShapeBased(CollisionGeometryd): + ShapeBased() except + + +cdef extern from "fcl/geometry/shape/triangle_p.h" namespace "fcl": + cdef cppclass TrianglePd(ShapeBased): + TrianglePd(Vector3d& a_, Vector3d& b_, Vector3d& c_) except + + Vector3d a, b, c + +cdef extern from "fcl/geometry/shape/box.h" namespace "fcl": + cdef cppclass Boxd(ShapeBased): + Boxd(double x, double y, double z) except + + Vector3d side + +cdef extern from "fcl/geometry/shape/sphere.h" namespace "fcl": + cdef cppclass Sphered(ShapeBased): + Sphered(double radius_) except + + double radius + +cdef extern from "fcl/geometry/shape/ellipsoid.h" namespace "fcl": + cdef cppclass Ellipsoidd(ShapeBased): + Ellipsoidd(double a_, double b_, double c_) except + + Vector3d radii + +cdef extern from "fcl/geometry/shape/capsule.h" namespace "fcl": + cdef cppclass Capsuled(ShapeBased): + Capsuled(double radius_, double lz_) except + + double radius + double lz + +cdef extern from "fcl/geometry/shape/cone.h" namespace "fcl": + cdef cppclass Coned(ShapeBased): + Coned(double radius_, double lz_) except + + double radius + double lz + +cdef extern from "fcl/geometry/shape/cylinder.h" namespace "fcl": + cdef cppclass Cylinderd(ShapeBased): + Cylinderd(double radius_, double lz_) except + + double radius + double lz + +cdef extern from "fcl/geometry/shape/convex.h" namespace "fcl": + cdef cppclass Convexd(ShapeBased): + Convexd(Vector3d* plane_nomals_, + double* plane_dis_, int num_planes, - Vec3f* points_, + Vector3d* points_, int num_points_, int* polygons_) except + - cdef cppclass Halfspace(ShapeBase): - Halfspace(Vec3f& n_, FCL_REAL d_) except + - Vec3f n - FCL_REAL d +cdef extern from "fcl/geometry/shape/halfspace.h" namespace "fcl": + cdef cppclass Halfspaced(ShapeBased): + Halfspaced(Vector3d& n_, double d_) except + + Vector3d n + double d - cdef cppclass Plane(ShapeBase): - Plane(Vec3f& n_, FCL_REAL d_) except + - Vec3f n - FCL_REAL d +cdef extern from "fcl/geometry/shape/plane.h" namespace "fcl": + cdef cppclass Planed(ShapeBased): + Planed(Vector3d& n_, double d_) except + + Vector3d n + double d -cdef extern from "fcl/broadphase/broadphase.h" namespace "fcl": - ctypedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata) - ctypedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist) +cdef extern from "fcl/broadphase/broadphase_collision_manager.h" namespace "fcl": + ctypedef bool (*CollisionCallBack)(CollisionObjectd* o1, CollisionObjectd* o2, void* cdata) + ctypedef bool (*DistanceCallBack)(CollisionObjectd* o1, CollisionObjectd* o2, void* cdata, double& dist) cdef extern from "fcl/broadphase/broadphase_dynamic_AABB_tree.h" namespace "fcl": - cdef cppclass DynamicAABBTreeCollisionManager: - DynamicAABBTreeCollisionManager() except + - void registerObjects(vector[CollisionObject*]& other_objs) - void registerObject(CollisionObject* obj) - void unregisterObject(CollisionObject* obj) - void collide(DynamicAABBTreeCollisionManager* mgr, void* cdata, CollisionCallBack callback) - void distance(DynamicAABBTreeCollisionManager* mgr, void* cdata, DistanceCallBack callback) - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) + cdef cppclass DynamicAABBTreeCollisionManagerd: + DynamicAABBTreeCollisionManagerd() except + + void registerObjects(vector[CollisionObjectd*]& other_objs) + void registerObject(CollisionObjectd* obj) + void unregisterObject(CollisionObjectd* obj) + void collide(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, CollisionCallBack callback) +# void distance(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, DistanceCallBack callback) + void collide(CollisionObjectd* obj, void* cdata, CollisionCallBack callback) +# void distance(CollisionObjectd* obj, void* cdata, DistanceCallBack callback) void collide(void* cdata, CollisionCallBack callback) - void distance(void* cdata, DistanceCallBack callback) +# void distance(void* cdata, DistanceCallBack callback) void setup() void update() - void update(CollisionObject* updated_obj) - void update(vector[CollisionObject*] updated_objs) + void update(CollisionObjectd* updated_obj) + void update(vector[CollisionObjectd*] updated_objs) void clear() bool empty() size_t size() @@ -277,42 +291,41 @@ cdef extern from "fcl/broadphase/broadphase_dynamic_AABB_tree.h" namespace "fcl" bool octree_as_geometry_collide bool octree_as_geometry_distance -cdef extern from "fcl/collision.h" namespace "fcl": - size_t collide(CollisionObject* o1, CollisionObject* o2, - CollisionRequest& request, - CollisionResult& result) +cdef extern from "fcl/narrowphase/collision.h" namespace "fcl": + size_t collide(CollisionObjectd* o1, CollisionObjectd* o2, + CollisionRequestd& request, + CollisionResultd& result) - size_t collide(CollisionGeometry* o1, Transform3f& tf1, - CollisionGeometry* o2, Transform3f& tf2, - CollisionRequest& request, - CollisionResult& result) + size_t collide(CollisionGeometryd* o1, Transform3d& tf1, + CollisionGeometryd* o2, Transform3d& tf2, + CollisionRequestd& request, + CollisionResultd& result) -cdef extern from "fcl/continuous_collision.h" namespace "fcl": - FCL_REAL continuousCollide(CollisionGeometry* o1, Transform3f& tf1_beg, Transform3f& tf1_end, - CollisionGeometry* o2, Transform3f& tf2_beg, Transform3f& tf2_end, - ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) +cdef extern from "fcl/narrowphase/continuous_collision.h" namespace "fcl": + double continuousCollide(CollisionGeometryd* o1, Transform3d& tf1_beg, Transform3d& tf1_end, + CollisionGeometryd* o2, Transform3d& tf2_beg, Transform3d& tf2_end, + ContinuousCollisionRequestd& request, + ContinuousCollisionResultd& result) - FCL_REAL continuousCollide(CollisionObject* o1, Transform3f& tf1_end, - CollisionObject* o2, Transform3f& tf2_end, - ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) + double continuousCollide(CollisionObjectd* o1, Transform3d& tf1_end, + CollisionObjectd* o2, Transform3d& tf2_end, + ContinuousCollisionRequestd& request, + ContinuousCollisionResultd& result) -cdef extern from "fcl/distance.h" namespace "fcl": - FCL_REAL distance(CollisionObject* o1, CollisionObject* o2, - DistanceRequest& request, DistanceResult& result) - FCL_REAL distance(CollisionGeometry* o1, Transform3f& tf1, - CollisionGeometry* o2, Transform3f& tf2, - DistanceRequest& request, DistanceResult& result) +cdef extern from "fcl/narrowphase/distance.h" namespace "fcl": + double distance(CollisionObjectd* o1, CollisionObjectd* o2, + DistanceRequestd& request, DistanceResultd& result) + double distance(CollisionGeometryd* o1, Transform3d& tf1, + CollisionGeometryd* o2, Transform3d& tf2, + DistanceRequestd& request, DistanceResultd& result) -cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl": +cdef extern from "fcl/geometry/BVH/BVH_internal.h" namespace "fcl": cdef enum BVHModelType: BVH_MODEL_UNKNOWN, # unknown model type BVH_MODEL_TRIANGLES, # triangle model BVH_MODEL_POINTCLOUD # point cloud model -cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl": cdef enum BVHReturnCode: BVH_OK = 0, # BVH is valid BVH_ERR_MODEL_OUT_OF_MEMORY = -1, # Cannot allocate memory for vertices and triangles @@ -325,7 +338,6 @@ cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl": BVH_ERR_UNKNOWN = -8 # Unknown failure -cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl": cdef enum BVHBuildState: BVH_BUILD_STATE_EMPTY, # empty state, immediately after constructor BVH_BUILD_STATE_BEGUN, # after beginModel(), state for adding geometry primitives @@ -334,36 +346,37 @@ cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl": BVH_BUILD_STATE_UPDATED, # after tree has been build for updated geometry, ready for ccd use BVH_BUILD_STATE_REPLACE_BEGUN, # after beginReplaceModel(), state for replacing geometry primitives -cdef extern from "fcl/data_types.h" namespace "fcl": +cdef extern from "fcl/math/triangle.h" namespace "fcl": cdef cppclass Triangle: Triangle() except + Triangle(size_t p1, size_t p2, size_t p3) except + size_t vids[3] -cdef extern from "fcl/BVH/BV_splitter.h" namespace "fcl": +# TODO what about these guys? +cdef extern from "fcl/geometry/BVH/detail/BV_splitter_base.h" namespace "fcl": cdef cppclass BVSplitterBase: pass -cdef extern from "fcl/BVH/BV_fitter.h" namespace "fcl": +cdef extern from "fcl/geometry/BVH/detail/BV_fitter_base.h" namespace "fcl": cdef cppclass BVFitterBase: pass -cdef extern from "fcl/BVH/BVH_model.h" namespace "fcl": +cdef extern from "fcl/geometry/BVH/BVH_model.h" namespace "fcl": # Cython only accepts type template parameters. # see https://groups.google.com/forum/#!topic/cython-users/xAZxdCFw6Xs - cdef cppclass BVHModel "fcl::BVHModel" ( CollisionGeometry ): + cdef cppclass BVHModel "fcl::BVHModel" ( CollisionGeometryd ): # Constructing an empty BVH BVHModel() except + BVHModel(BVHModel& other) except + # #Geometry point data - Vec3f* vertices + Vector3d* vertices # #Geometry triangle index data, will be NULL for point clouds Triangle* tri_indices # #Geometry point data in previous frame - Vec3f* prev_vertices + Vector3d* prev_vertices # #Number of triangles int num_tris @@ -383,16 +396,16 @@ cdef extern from "fcl/BVH/BVH_model.h" namespace "fcl": int beginModel(int num_tris_, int num_vertices_) - int addVertex(const Vec3f& p) + int addVertex(const Vector3d& p) - int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) + int addTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3) - #int addSubModel(const std::vector& ps) - # void getCostSources(vector[CostSource]& cost_sources_) + #int addSubModel(const std::vector& ps) + # void getCostSources(vector[CostSourced]& cost_sources_) - #int addSubModel(const vector[Vec3f]& ps) + #int addSubModel(const vector[Vector3d]& ps) # - int addSubModel(const vector[Vec3f]& ps, const vector[Triangle]& ts) + int addSubModel(const vector[Vector3d]& ps, const vector[Triangle]& ts) int endModel() @@ -401,9 +414,9 @@ cdef extern from "fcl/BVH/BVH_model.h" namespace "fcl": # void computeLocalAABB() -cdef extern from "fcl/octree.h" namespace "fcl": - cdef cppclass OcTree(CollisionGeometry): +cdef extern from "fcl/geometry/octree/octree.h" namespace "fcl": + cdef cppclass OcTreed(CollisionGeometryd): # Constructing - OcTree(FCL_REAL resolution) except + - OcTree(shared_ptr[octomap.OcTree]& tree_) except + + OcTreed(double resolution) except + + OcTreed(shared_ptr[octomap.OcTree]& tree_) except + diff --git a/setup.py b/setup.py index 4dedd51..c00ce0b 100644 --- a/setup.py +++ b/setup.py @@ -20,7 +20,7 @@ platform_supported = True include_dirs = ['/usr/include', '/usr/local/include', - '/usr/include/eigen3'] + '/usr/local/include/eigen3'] lib_dirs = ['/usr/lib', '/usr/local/lib'] From bc62e9ceac81d36daa9116d601ffd43c18faf9dc Mon Sep 17 00:00:00 2001 From: Dmitry Neverov Date: Mon, 27 Apr 2020 19:22:40 +0900 Subject: [PATCH 02/54] Fixed Transform extension type: Vector3, Matrix3, Quaternion and Transform are now native Eigen objects in FCL, so helper functions were changed accordingly. --- fcl/fcl.pyx | 134 +++++++++++++++++++++++++++++------------------ fcl/fcl_defs.pxd | 35 +++++-------- 2 files changed, 96 insertions(+), 73 deletions(-) diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx index c867aa7..a2d0a3f 100644 --- a/fcl/fcl.pyx +++ b/fcl/fcl.pyx @@ -16,6 +16,21 @@ cimport octomap_defs as octomap cimport std_defs as std from collision_data import Contact, CostSource, CollisionRequest, ContinuousCollisionRequest, CollisionResult, ContinuousCollisionResult, DistanceRequest, DistanceResult +""" +Eigen::Transform linear and translation parts are returned as Eigen::Block +It can be an rvalue and an lvalue, so in C++ you could assign something to translation() like: + `tf.translation() = (Vector3d (0., 0., 50));` +In python and cython however, a function call is never an lvalue, so we workaround with the following macro +""" +cdef extern from *: + """ + /* Verbatim C as a workaround for assingment to lvalue-returning functions*/ + #define ASSIGN(a, b) a = b + """ + void ASSIGN(defs.Vector3d&, defs.Vector3d) + void ASSIGN(defs.Matrix3d&, defs.Matrix3d) + #void ASSIGN[T](T&, T) # This doesn't work somehow + ############################################################################### # Transforms ############################################################################### @@ -25,31 +40,51 @@ cdef class Transform: def __cinit__(self, *args): if len(args) == 0: self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() elif len(args) == 1: if isinstance(args[0], Transform): self.thisptr = new defs.Transform3d(deref(( args[0]).thisptr)) -# else: -# data = numpy.array(args[0]) -# if data.shape == (3,3): -# self.thisptr = new defs.Transform3d(numpy_to_mat3d(data)) -# elif data.shape == (4,): -# self.thisptr = new defs.Transform3d(numpy_to_quaternion3d(data)) -# elif data.shape == (3,): -# self.thisptr = new defs.Transform3d(numpy_to_vec3d(data)) -# else: -# raise ValueError('Invalid input to Transform().') + else: + data = numpy.array(args[0]) + if data.shape == (3,3): + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(data)) + elif data.shape == (4,): + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_quaternion3d(data).toRotationMatrix()) + elif data.shape == (3,): + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(data)) + else: + raise ValueError('Invalid input to Transform().') elif len(args) == 2: rot = numpy.array(args[0]) trans = numpy.array(args[1]).squeeze() if not trans.shape == (3,): raise ValueError('Translation must be (3,).') -# if rot.shape == (3,3): -# self.thisptr = new defs.Transform3d(numpy_to_mat3d(rot), numpy_to_vec3d(trans)) -# elif rot.shape == (4,): -# self.thisptr = new defs.Transform3d(numpy_to_quaternion3d(rot), numpy_to_vec3d(trans)) -# else: -# raise ValueError('Invalid input to Transform().') + if rot.shape == (3,3): + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(rot)) + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(trans)) + elif rot.shape == (4,): + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_quaternion3d(rot).toRotationMatrix()) + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(trans)) + else: + raise ValueError('Invalid input to Transform().') else: raise ValueError('Too many arguments to Transform().') @@ -57,23 +92,27 @@ cdef class Transform: if self.thisptr: free(self.thisptr) -# def getRotation(self): -# return mat3d_to_numpy(self.thisptr.getRotation()) + def getRotation(self): + return mat3d_to_numpy(self.thisptr.linear()) -# def getTranslation(self): -# return vec3d_to_numpy(self.thisptr.getTranslation()) + def getTranslation(self): + return vec3d_to_numpy(self.thisptr.translation()) -# def getQuatRotation(self): -# return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) + def getQuatRotation(self): + cdef defs.Quaterniond quaternion = defs.Quaterniond(self.thisptr.linear()) + return quaternion3d_to_numpy(quaternion) -# def setRotation(self, R): -# self.thisptr.setRotation(numpy_to_mat3d(R)) + def setRotation(self, R): + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(R)) -# def setTranslation(self, T): -# self.thisptr.setTranslation(numpy_to_vec3d(T)) + def setTranslation(self, T): + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(T)) -# def setQuatRotation(self, q): -# self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) + def setQuatRotation(self, q): + ASSIGN(self.thisptr.linear(), + numpy_to_quaternion3d(q).toRotationMatrix()) ############################################################################### # Collision objects and geometries @@ -121,12 +160,12 @@ cdef class CollisionObject: def getRotation(self): return mat3d_to_numpy(self.thisptr.getRotation()) -# def setRotation(self, mat): -# self.thisptr.setRotation(numpy_to_mat3d(mat)) -# self.thisptr.computeAABB() + def setRotation(self, mat): + self.thisptr.setRotation(numpy_to_mat3d(mat)) + self.thisptr.computeAABB() -# def getQuatRotation(self): -# return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) + def getQuatRotation(self): + return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) def setQuatRotation(self, q): self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) @@ -295,10 +334,8 @@ cdef class Cylinder(CollisionGeometry): ( self.thisptr).lz = value cdef class Halfspace(CollisionGeometry): - def __cinit__(self, n, d): - self.thisptr = new defs.Halfspaced(defs.Vector3d( n[0], - n[1], - n[2]), + def __cinit__(self, np.ndarray[double, ndim=1, mode="c"] n, d): + self.thisptr = new defs.Halfspaced(defs.Vector3d(&n[0]), d) property n: @@ -316,10 +353,8 @@ cdef class Halfspace(CollisionGeometry): ( self.thisptr).d = value cdef class Plane(CollisionGeometry): - def __cinit__(self, n, d): - self.thisptr = new defs.Planed(defs.Vector3d( n[0], - n[1], - n[2]), + def __cinit__(self, np.ndarray[double, ndim=1, mode="c"] n, d): + self.thisptr = new defs.Planed(defs.Vector3d(&n[0]), d) property n: @@ -355,7 +390,8 @@ cdef class BVHModel(CollisionGeometry): return n def addVertex(self, x, y, z): - n = ( self.thisptr).addVertex(defs.Vector3d( x, y, z)) + cdef np.ndarray[double, ndim=1, mode="c"] n = numpy.array([x, y, z]) + n = ( self.thisptr).addVertex(defs.Vector3d(&n[0])) return self._check_ret_value(n) def addTriangle(self, v1, v2, v3): @@ -705,8 +741,8 @@ cdef inline bool DistanceCallBack(defs.CollisionObjectd*o1, defs.CollisionObject # Helper Functions ############################################################################### -#cdef quaternion3d_to_numpy(defs.Quaterniond q): -# return numpy.array([q.getW(), q.getX(), q.getY(), q.getZ()]) +cdef quaternion3d_to_numpy(defs.Quaterniond q): + return numpy.array([q.w(), q.x(), q.y(), q.z()]) cdef defs.Quaterniond numpy_to_quaternion3d(a): return defs.Quaterniond( a[0], a[1], a[2], a[3]) @@ -714,18 +750,16 @@ cdef defs.Quaterniond numpy_to_quaternion3d(a): cdef vec3d_to_numpy(defs.Vector3d vec): return numpy.array([vec[0], vec[1], vec[2]]) -cdef defs.Vector3d numpy_to_vec3d(a): - return defs.Vector3d( a[0], a[1], a[2]) +cdef defs.Vector3d numpy_to_vec3d(np.ndarray[double, ndim=1, mode="c"] a): + return defs.Vector3d(&a[0]) cdef mat3d_to_numpy(defs.Matrix3d m): return numpy.array([[m(0,0), m(0,1), m(0,2)], [m(1,0), m(1,1), m(1,2)], [m(2,0), m(2,1), m(2,2)]]) -#cdef defs.Matrix3d numpy_to_mat3d(a): -# return defs.Matrix3d( a[0][0], a[0][1], a[0][2], -# a[1][0], a[1][1], a[1][2], -# a[2][0], a[2][1], a[2][2]) +cdef defs.Matrix3d numpy_to_mat3d(np.ndarray[double, ndim=2, mode="c"] a): + return defs.Matrix3d(&a[0, 0]) cdef c_to_python_collision_geometry(defs.const_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2): cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd index 50dd926..f392565 100644 --- a/fcl/fcl_defs.pxd +++ b/fcl/fcl_defs.pxd @@ -21,41 +21,30 @@ cdef extern from "Python.h": cdef extern from "fcl/common/types.h" namespace "fcl": cdef cppclass Vector3d: Vector3d() except + - Vector3d(double x, double y, double z) except + + Vector3d(double *data) except + double& operator[](size_t i) cdef cppclass Matrix3d: Matrix3d() except + -# Matrix3d(double xx, double xy, double xz, -# double yx, double yy, double yz, -# double zx, double zy, double zz) except + + Matrix3d(double *data) double operator()(size_t i, size_t j) cdef cppclass Quaterniond: Quaterniond() except + - Quaterniond(double a, double b, - double c, double d) except + - void fromRotation(Matrix3d& R) - void fromAxisAngle(Vector3d& axis, double angle) -# double& getW() -# double& getX() -# double& getY() -# double& getZ() + Quaterniond(double a, double b, double c, double d) except + + Quaterniond(Matrix3d& R) except + + double& w() + double& x() + double& y() + double& z() + Matrix3d& toRotationMatrix() cdef cppclass Transform3d: Transform3d() except + -# Transform3d(Matrix3d& R_, Vector3d& T_) -# Transform3d(Quaterniond& q_, Vector3d& T_) -# Transform3d(Matrix3d& R_) -# Transform3d(Quaterniond& q_) -# Transform3d(Vector3d& T_) Transform3d(Transform3d& tf_) -# Matrix3d& getRotation() -# Vector3d& getTranslation() -# Quaterniond& getQuatRotation() -# void setRotation(Matrix3d& R_) -# void setTranslation(Vector3d& T_) -# void setQuatRotation(Quaterniond & q_) + void setIdentity() + Matrix3d& linear() + Vector3d& translation() cdef extern from "fcl/narrowphase/continuous_collision_request.h" namespace "fcl": cdef enum CCDMotionType: From 23672d91c4debe33d79926b96a1364da3cfe4357 Mon Sep 17 00:00:00 2001 From: Dmitry Neverov Date: Mon, 27 Apr 2020 19:38:01 +0900 Subject: [PATCH 03/54] Returned all previous functionallity: distance function and its dependants. Now the old test/* is passing. --- fcl/__init__.py | 4 +-- fcl/fcl.pyx | 80 ++++++++++++++++++++++++------------------------ fcl/fcl_defs.pxd | 6 ++-- 3 files changed, 45 insertions(+), 45 deletions(-) diff --git a/fcl/__init__.py b/fcl/__init__.py index fae3eb8..e852be0 100644 --- a/fcl/__init__.py +++ b/fcl/__init__.py @@ -1,5 +1,5 @@ -from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, defaultCollisionCallback, defaultDistanceCallback -#distance +from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, distance, defaultCollisionCallback, defaultDistanceCallback + from .collision_data import OBJECT_TYPE, NODE_TYPE, CCDMotionType, CCDSolverType, GJKSolverType, Contact, CostSource, CollisionRequest, CollisionResult, ContinuousCollisionRequest, ContinuousCollisionResult, DistanceRequest, DistanceResult, CollisionData, DistanceData from .version import __version__ diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx index a2d0a3f..b2fb29c 100644 --- a/fcl/fcl.pyx +++ b/fcl/fcl.pyx @@ -507,18 +507,18 @@ cdef class DynamicAABBTreeCollisionManager: else: raise ValueError -# def distance(self, *args): -# if len(args) == 2 and inspect.isroutine(args[1]): -# fn = DistanceFunction(args[1], args[0]) -# self.thisptr.distance( fn, DistanceCallBack) -# elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): -# fn = DistanceFunction(args[2], args[1]) -# self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) -# elif len(args) == 3 and inspect.isroutine(args[2]): -# fn = DistanceFunction(args[2], args[1]) -# self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) -# else: -# raise ValueError + def distance(self, *args): + if len(args) == 2 and inspect.isroutine(args[1]): + fn = DistanceFunction(args[1], args[0]) + self.thisptr.distance( fn, DistanceCallBack) + elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): + fn = DistanceFunction(args[2], args[1]) + self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) + elif len(args) == 3 and inspect.isroutine(args[2]): + fn = DistanceFunction(args[2], args[1]) + self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) + else: + raise ValueError def clear(self): self.thisptr.clear() @@ -637,31 +637,31 @@ def continuousCollide(CollisionObject o1, Transform tf1_end, result.time_of_contact = min(cresult.time_of_contact, result.time_of_contact) return ret -#def distance(CollisionObject o1, CollisionObject o2, -# request = None, result=None): -# -# if request is None: -# request = DistanceRequest() -# if result is None: -# result = DistanceResult() -# -# cdef defs.DistanceResultd cresult -# -# cdef double dis = defs.distance(o1.thisptr, o2.thisptr, -# defs.DistanceRequestd( -# request.enable_nearest_points, -# 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 -# return dis +def distance(CollisionObject o1, CollisionObject o2, + request = None, result=None): + + if request is None: + request = DistanceRequest() + if result is None: + result = DistanceResult() + + cdef defs.DistanceResultd cresult + + cdef double dis = defs.distance(o1.thisptr, o2.thisptr, + defs.DistanceRequestd( + request.enable_nearest_points, + 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 + return dis ############################################################################### # Collision and Distance Callback Functions @@ -687,9 +687,9 @@ def defaultDistanceCallback(CollisionObject o1, CollisionObject o2, cdata): if cdata.done: return True, result.min_distance -# -# distance(o1, o2, request, result) -# + + distance(o1, o2, request, result) + dist = result.min_distance if dist <= 0: diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd index f392565..649f2f0 100644 --- a/fcl/fcl_defs.pxd +++ b/fcl/fcl_defs.pxd @@ -260,11 +260,11 @@ cdef extern from "fcl/broadphase/broadphase_dynamic_AABB_tree.h" namespace "fcl" void registerObject(CollisionObjectd* obj) void unregisterObject(CollisionObjectd* obj) void collide(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, CollisionCallBack callback) -# void distance(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, DistanceCallBack callback) + void distance(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, DistanceCallBack callback) void collide(CollisionObjectd* obj, void* cdata, CollisionCallBack callback) -# void distance(CollisionObjectd* obj, void* cdata, DistanceCallBack callback) + void distance(CollisionObjectd* obj, void* cdata, DistanceCallBack callback) void collide(void* cdata, CollisionCallBack callback) -# void distance(void* cdata, DistanceCallBack callback) + void distance(void* cdata, DistanceCallBack callback) void setup() void update() void update(CollisionObjectd* updated_obj) From 2dee00364c65eb5021efc4a0695a0371ff431dec Mon Sep 17 00:00:00 2001 From: Dmitry Neverov Date: Mon, 27 Apr 2020 20:51:19 +0900 Subject: [PATCH 04/54] Fixed ordering mismatch bug --- fcl/fcl.pyx | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx index b2fb29c..f15e777 100644 --- a/fcl/fcl.pyx +++ b/fcl/fcl.pyx @@ -334,7 +334,7 @@ cdef class Cylinder(CollisionGeometry): ( self.thisptr).lz = value cdef class Halfspace(CollisionGeometry): - def __cinit__(self, np.ndarray[double, ndim=1, mode="c"] n, d): + def __cinit__(self, np.ndarray[double, ndim=1] n, d): self.thisptr = new defs.Halfspaced(defs.Vector3d(&n[0]), d) @@ -353,7 +353,7 @@ cdef class Halfspace(CollisionGeometry): ( self.thisptr).d = value cdef class Plane(CollisionGeometry): - def __cinit__(self, np.ndarray[double, ndim=1, mode="c"] n, d): + def __cinit__(self, np.ndarray[double, ndim=1] n, d): self.thisptr = new defs.Planed(defs.Vector3d(&n[0]), d) @@ -390,7 +390,7 @@ cdef class BVHModel(CollisionGeometry): return n def addVertex(self, x, y, z): - cdef np.ndarray[double, ndim=1, mode="c"] n = numpy.array([x, y, z]) + cdef np.ndarray[double, ndim=1] n = numpy.array([x, y, z]) n = ( self.thisptr).addVertex(defs.Vector3d(&n[0])) return self._check_ret_value(n) @@ -750,7 +750,7 @@ cdef defs.Quaterniond numpy_to_quaternion3d(a): cdef vec3d_to_numpy(defs.Vector3d vec): return numpy.array([vec[0], vec[1], vec[2]]) -cdef defs.Vector3d numpy_to_vec3d(np.ndarray[double, ndim=1, mode="c"] a): +cdef defs.Vector3d numpy_to_vec3d(np.ndarray[double, ndim=1] a): return defs.Vector3d(&a[0]) cdef mat3d_to_numpy(defs.Matrix3d m): @@ -758,8 +758,11 @@ cdef mat3d_to_numpy(defs.Matrix3d m): [m(1,0), m(1,1), m(1,2)], [m(2,0), m(2,1), m(2,2)]]) -cdef defs.Matrix3d numpy_to_mat3d(np.ndarray[double, ndim=2, mode="c"] a): - return defs.Matrix3d(&a[0, 0]) +cdef defs.Matrix3d numpy_to_mat3d(np.ndarray[double, ndim=2] a): + # NOTE Eigen defaults to column-major storage, + # which corresponds to non-default Fortran mode of ordering in numpy + cdef np.ndarray[double, ndim=2, mode='fortran'] f = np.ndarray.copy(a, order='F') + return defs.Matrix3d(&f[0, 0]) cdef c_to_python_collision_geometry(defs.const_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2): cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) From af54f5908fdc531b8ede2f293a92c50af8b0873c Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Thu, 23 Jul 2020 21:33:18 +0200 Subject: [PATCH 05/54] Fix bvh path --- fcl/fcl_defs.pxd | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd index 649f2f0..2bbe78a 100644 --- a/fcl/fcl_defs.pxd +++ b/fcl/fcl_defs.pxd @@ -309,7 +309,7 @@ cdef extern from "fcl/narrowphase/distance.h" namespace "fcl": CollisionGeometryd* o2, Transform3d& tf2, DistanceRequestd& request, DistanceResultd& result) -cdef extern from "fcl/geometry/BVH/BVH_internal.h" namespace "fcl": +cdef extern from "fcl/geometry/bvh/BVH_internal.h" namespace "fcl": cdef enum BVHModelType: BVH_MODEL_UNKNOWN, # unknown model type BVH_MODEL_TRIANGLES, # triangle model @@ -342,15 +342,15 @@ cdef extern from "fcl/math/triangle.h" namespace "fcl": size_t vids[3] # TODO what about these guys? -cdef extern from "fcl/geometry/BVH/detail/BV_splitter_base.h" namespace "fcl": +cdef extern from "fcl/geometry/bvh/detail/BV_splitter_base.h" namespace "fcl": cdef cppclass BVSplitterBase: pass -cdef extern from "fcl/geometry/BVH/detail/BV_fitter_base.h" namespace "fcl": +cdef extern from "fcl/geometry/bvh/detail/BV_fitter_base.h" namespace "fcl": cdef cppclass BVFitterBase: pass -cdef extern from "fcl/geometry/BVH/BVH_model.h" namespace "fcl": +cdef extern from "fcl/geometry/bvh/BVH_model.h" namespace "fcl": # Cython only accepts type template parameters. # see https://groups.google.com/forum/#!topic/cython-users/xAZxdCFw6Xs cdef cppclass BVHModel "fcl::BVHModel" ( CollisionGeometryd ): From 6ecd4d221f1a0f1bd993d26f8fa9e87c5e12e7bc Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Wed, 21 Jul 2021 11:36:01 +0200 Subject: [PATCH 06/54] update metadata to fcl 0.6.1 Use fcl version as version to make targeted version more obvious. --- README.md | 7 ++++--- fcl/version.py | 2 +- setup.py | 10 +++++----- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 9297ebe..6fc558f 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,12 @@ +# Disclaimer +This is fork of https://github.com/BerkeleyAutomation/python-fcl intented to be used in [ifcopenshell](https://github.com/IfcOpenShell/IfcOpenShell) / [BlenderBIM Add-on](https://blenderbim.org/) as orgininal repo is currently unactive and is not merging any PR. + # python-fcl ### Python Interface for the Flexible Collision Library -[![Build Status](https://travis-ci.org/BerkeleyAutomation/python-fcl.svg?branch=master)](https://travis-ci.org/BerkeleyAutomation/python-fcl) - Python-FCL is an (unofficial) Python interface for the [Flexible Collision Library (FCL)](https://github.com/flexible-collision-library/fcl), an excellent C++ library for performing proximity and collision queries on pairs of geometric models. -Currently, this package is targeted for FCL 0.5.0. +Currently, this package is targeted for FCL 0.6.1. This package supports three types of proximity queries for pairs of geometric models: * __Collision Detection__: Detecting whether two models overlap (and optionally where). diff --git a/fcl/version.py b/fcl/version.py index b459ff2..8411e55 100644 --- a/fcl/version.py +++ b/fcl/version.py @@ -1 +1 @@ -__version__ = '0.0.12' +__version__ = '0.6.1' diff --git a/setup.py b/setup.py index c00ce0b..019c6c3 100644 --- a/setup.py +++ b/setup.py @@ -19,8 +19,8 @@ if prefix in sys.platform: platform_supported = True include_dirs = ['/usr/include', - '/usr/local/include', - '/usr/local/include/eigen3'] + '/usr/include', + '/usr/include/eigen3'] lib_dirs = ['/usr/lib', '/usr/local/lib'] @@ -49,9 +49,9 @@ version=__version__, description='Python bindings for the Flexible Collision Library', long_description='Python bindings for the Flexible Collision Library', - url='https://github.com/BerkeleyAutomation/python-fcl', - author='Matthew Matl', - author_email='mmatl@eecs.berkeley.edu', + url='https://github.com/CyrilWaechter/python-fcl', + author='Cyril Waechter', + author_email='cyrwae@hotmail.com', license = "BSD", classifiers=[ 'Development Status :: 3 - Alpha', From 92502dcfcd2a127e2c71596d5a6f6990c918d140 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Fri, 23 Jul 2021 07:02:30 +0200 Subject: [PATCH 07/54] update docker file for manylinux build --- requirements/Dockerfile | 16 ++++++---------- requirements/build.bash | 7 +++++++ requirements/clone.bash | 22 +++++----------------- requirements/install_cmake.bash | 5 ----- setup.py | 5 +++-- 5 files changed, 21 insertions(+), 34 deletions(-) delete mode 100644 requirements/install_cmake.bash diff --git a/requirements/Dockerfile b/requirements/Dockerfile index eaccf63..52965aa 100644 --- a/requirements/Dockerfile +++ b/requirements/Dockerfile @@ -6,14 +6,7 @@ # ln -sf requirements/Dockerfile . # docker build . -t pythonfcl -FROM quay.io/pypa/manylinux1_x86_64:latest - - -RUN yum install -y gcc - -# install cmake 2.8.12 -COPY requirements/install_cmake.bash . -RUN bash install_cmake.bash +FROM quay.io/pypa/manylinux_2_24_x86_64:latest # clone FCL and libccd # the exact checkouts are in clone.bash @@ -27,7 +20,7 @@ RUN bash build.bash # manylinux includes a bunch of pythons # to test with others change this env variable #ENV PATH=/opt/python/cp27-cp27m/bin:$PATH -ENV PATH=/opt/python/cp36-cp36m/bin:$PATH +ENV PATH=/opt/python/cp39-cp39/bin:$PATH # we need numpy to build python-fcl # since we set our path we'll be using the right pip @@ -35,4 +28,7 @@ RUN pip install numpy cython # build the python-fcl module COPY . /python_fcl -RUN cd /python_fcl && python setup.py build_ext +RUN pip wheel /python_fcl --no-deps -w wheelhouse/ +RUN pip install /python_fcl --no-index -f /wheelhouse +RUN ls /wheelhouse +RUN auditwheel repair wheelhouse/python_fcl-0.6.1-cp39-cp39-linux_x86_64.whl --plat manylinux_2_24_x86_64 -w /wheelhouse diff --git a/requirements/build.bash b/requirements/build.bash index 16b7091..0d1653b 100644 --- a/requirements/build.bash +++ b/requirements/build.bash @@ -1,15 +1,22 @@ +echo "Install eigen" +cmake -B build -S eigen-3.3.9 +cmake --install build + +echo "Build and install libccd" cd libccd cmake . make -j4 make install cd .. +echo "Build and install octomap" cd octomap cmake . make -j4 make install cd .. +echo "Build and install fcl" cd fcl cmake . make -j4 diff --git a/requirements/clone.bash b/requirements/clone.bash index fc7d56a..e200678 100644 --- a/requirements/clone.bash +++ b/requirements/clone.bash @@ -1,24 +1,12 @@ rm -rf libccd -git clone https://github.com/danfis/libccd.git -cd libccd -git pull -git checkout 64f02f741ac94fccd0fb660a5bffcbe6d01d9939 -cd .. +git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd.git rm -rf octomap -git clone https://github.com/OctoMap/octomap.git -cd octomap -git pull -git checkout b8c1d62a7a64ce0a5df278503f31d73acafa97e4 -cd .. +git clone --depth 1 --branch v1.8.0 https://github.com/OctoMap/octomap.git rm -rf fcl -git clone https://github.com/flexible-collision-library/fcl.git -cd fcl -git pull -git checkout 22f375f333beccc10c527974cef96784f0841649 -cd .. +git clone --depth 1 --branch v0.6.1 https://github.com/flexible-collision-library/fcl.git # get eigen -#curl -OL https://github.com/RLovelett/eigen/archive/3.3.4.tar.gz -#tar -zxvf 3.3.4.tar.gz +curl -OL https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz +tar -zxf eigen-3.3.9.tar.gz \ No newline at end of file diff --git a/requirements/install_cmake.bash b/requirements/install_cmake.bash deleted file mode 100644 index 06072cd..0000000 --- a/requirements/install_cmake.bash +++ /dev/null @@ -1,5 +0,0 @@ -# install cmake using their shell script -# we do this because fcl needs cmake >= 2.8.12, but centos 5 -# from the manylinux image has only cmake 2.8.11 -curl -OL https://cmake.org/files/v2.8/cmake-2.8.12.2-Linux-i386.sh -bash cmake-2.8.12.2-Linux-i386.sh --skip-license --prefix=/usr diff --git a/setup.py b/setup.py index 019c6c3..1f9b490 100644 --- a/setup.py +++ b/setup.py @@ -19,8 +19,9 @@ if prefix in sys.platform: platform_supported = True include_dirs = ['/usr/include', - '/usr/include', - '/usr/include/eigen3'] + '/usr/local/include', + '/usr/include/eigen3', + '/usr/local/include/eigen3'] lib_dirs = ['/usr/lib', '/usr/local/lib'] From 158e3c9e79e22e3a096db7f28753e8e6acf3431a Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Fri, 23 Jul 2021 11:38:35 +0200 Subject: [PATCH 08/54] Add github actions to build python wheels --- .github/workflows/wheels.yml | 19 +++++++ .gitignore | 3 + pyproject.toml | 10 ++++ setup.cfg | 33 +++++++++++ setup.py | 103 ++++++++++++----------------------- 5 files changed, 101 insertions(+), 67 deletions(-) create mode 100644 .github/workflows/wheels.yml create mode 100644 pyproject.toml create mode 100644 setup.cfg diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml new file mode 100644 index 0000000..5c396b6 --- /dev/null +++ b/.github/workflows/wheels.yml @@ -0,0 +1,19 @@ +name: Build + +on: [push, pull_request] + +jobs: + build_wheels: + name: Build wheel py${{matrix.python}} on ${{matrix.platform}} + runs-on: ${{matrix.platform}} + strategy: + matrix: + platform: [ubuntu-latest] # [ubuntu-latest, macos-latest, windows-latest] + python: [39] + steps: + - uses: actions/checkout@v2 + - name: Build wheels + uses: pypa/cibuildwheel@v2.0.0 + - uses: actions/upload-artifact@v2 + with: + path: ./wheelhouse/*.whl \ No newline at end of file diff --git a/.gitignore b/.gitignore index db59b2b..8f238da 100644 --- a/.gitignore +++ b/.gitignore @@ -103,3 +103,6 @@ venv.bak/ # mypy .mypy_cache/ + +# vscode settings +.vscode/ diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..249cd8b --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,10 @@ +[build-system] +requires = ["setuptools", "wheel", "numpy", "Cython"] +build-backend = "setuptools.build_meta" + +[tool.cibuildwheel] +before-all = "bash requirements/clone.bash && bash requirements/build.bash" +before-build = "pip install numpy cython" +manylinux-x86_64-image = "manylinux_2_24" +test-requires = "pytest" +test-command = "pytest {package}/test" \ No newline at end of file diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..3b133e3 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,33 @@ +[metadata] +name = python-fcl +version = attr: fcl.__version__ +description = Python bindings for the Flexible Collision Library +long_description = file: README.md +long_description_content_type = text/markdown +url = https://github.com/CyrilWaechter/python-fcl +maintainer = Cyril Waechter +maintainer_email = cyrwae@hotmail.com +license = BSD +license_files = LICENSE +classifiers = + Development Status :: 3 - Alpha + License :: OSI Approved :: BSD License + Operating System :: POSIX :: Linux + Programming Language :: Python :: 2 + Programming Language :: Python :: 2.7 + Programming Language :: Python :: 3 + Programming Language :: Python :: 3.0 + Programming Language :: Python :: 3.1 + Programming Language :: Python :: 3.2 + Programming Language :: Python :: 3.3 + Programming Language :: Python :: 3.4 + Programming Language :: Python :: 3.5 + Programming Language :: Python :: 3.6 +keywords = fcl collision distance +packages = fcl +setup_requires = + numpy + Cython +install_requires = + numpy + Cython diff --git a/setup.py b/setup.py index 1f9b490..4df633f 100644 --- a/setup.py +++ b/setup.py @@ -1,83 +1,52 @@ import os import sys -import inspect from setuptools import Extension, setup -# get current directory of file in case someone -# called setup.py from elsewhere -cwd = os.path.dirname(os.path.abspath( - inspect.getfile(inspect.currentframe()))) - -# load __version__ -exec(open(os.path.join(cwd, - 'fcl/version.py'), 'r').read()) - -platform_supported = False -for prefix in ['darwin', 'linux', 'bsd']: - if prefix in sys.platform: - platform_supported = True - include_dirs = ['/usr/include', - '/usr/local/include', - '/usr/include/eigen3', - '/usr/local/include/eigen3'] - lib_dirs = ['/usr/lib', - '/usr/local/lib'] - - if 'CPATH' in os.environ: - include_dirs += os.environ['CPATH'].split(':') - if 'LD_LIBRARY_PATH' in os.environ: - lib_dirs += os.environ['LD_LIBRARY_PATH'].split(':') - - try: - # get the numpy include path from numpy - import numpy - include_dirs.append(numpy.get_include()) - except: - pass - - break - -if sys.platform == "win32": +def get_include_dirs(): platform_supported = False - -if not platform_supported: + for prefix in ['darwin', 'linux', 'bsd']: + if prefix in sys.platform: + platform_supported = True + include_dirs = ['/usr/include', + '/usr/local/include', + '/usr/include/eigen3', + '/usr/local/include/eigen3'] + + if 'CPATH' in os.environ: + include_dirs += os.environ['CPATH'].split(':') + + break + if sys.platform == "win32": + platform_supported = False + if not platform_supported: + raise NotImplementedError(sys.platform) + + # get the numpy include path from numpy + import numpy + include_dirs.append(numpy.get_include()) + return include_dirs + +def get_libraries_dir(): + for prefix in ['darwin', 'linux', 'bsd']: + if prefix in sys.platform: + platform_supported = True + lib_dirs = ['/usr/lib', + '/usr/local/lib'] + + if 'LD_LIBRARY_PATH' in os.environ: + lib_dirs += os.environ['LD_LIBRARY_PATH'].split(':') + return lib_dirs raise NotImplementedError(sys.platform) + setup( - name='python-fcl', - version=__version__, - description='Python bindings for the Flexible Collision Library', - long_description='Python bindings for the Flexible Collision Library', - url='https://github.com/CyrilWaechter/python-fcl', - author='Cyril Waechter', - author_email='cyrwae@hotmail.com', - license = "BSD", - classifiers=[ - 'Development Status :: 3 - Alpha', - 'License :: OSI Approved :: BSD License', - 'Operating System :: POSIX :: Linux', - 'Programming Language :: Python :: 2', - 'Programming Language :: Python :: 2.7', - 'Programming Language :: Python :: 3', - 'Programming Language :: Python :: 3.0', - 'Programming Language :: Python :: 3.1', - 'Programming Language :: Python :: 3.2', - 'Programming Language :: Python :: 3.3', - 'Programming Language :: Python :: 3.4', - 'Programming Language :: Python :: 3.5', - 'Programming Language :: Python :: 3.6', - ], - keywords='fcl collision distance', - packages=['fcl'], - setup_requires=['cython'], - install_requires=['numpy', 'cython'], ext_modules=[Extension( "fcl.fcl", ["fcl/fcl.pyx"], - include_dirs = include_dirs, - library_dirs = lib_dirs, + include_dirs = get_include_dirs(), + library_dirs = get_libraries_dir(), libraries=[ "fcl","octomap" ], From 8f606d5cb9a971811dc63e8df297fd103d5bcb90 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Sun, 25 Jul 2021 04:46:57 +0200 Subject: [PATCH 09/54] Fix many issues in deployment process * Fix conflicting folder name between python package and fcl dependency * Move to `src/module` tree schema * Fix `setup.cfg` options * Merge clone and build bash scripts for clarity (future mac/win builds) --- .gitignore | 1 + MANIFEST.in | 5 -- .../Dockerfile | 0 build_dependencies/install_linux.sh | 41 ++++++++++++++ fcl/__init__.py | 5 -- pyproject.toml | 2 +- requirements/build.bash | 24 -------- requirements/clone.bash | 12 ---- setup.cfg | 13 ++++- setup.py | 56 ++++++++++--------- src/fcl/__init__.py | 48 ++++++++++++++++ {fcl => src/fcl}/collision_data.py | 0 {fcl => src/fcl}/fcl.pyx | 0 {fcl => src/fcl}/fcl_defs.pxd | 0 {fcl => src/fcl}/octomap_defs.pxd | 0 {fcl => src/fcl}/std_defs.pxd | 0 {fcl => src/fcl}/version.py | 0 17 files changed, 134 insertions(+), 73 deletions(-) delete mode 100644 MANIFEST.in rename {requirements => build_dependencies}/Dockerfile (100%) create mode 100644 build_dependencies/install_linux.sh delete mode 100644 fcl/__init__.py delete mode 100644 requirements/build.bash delete mode 100644 requirements/clone.bash create mode 100644 src/fcl/__init__.py rename {fcl => src/fcl}/collision_data.py (100%) rename {fcl => src/fcl}/fcl.pyx (100%) rename {fcl => src/fcl}/fcl_defs.pxd (100%) rename {fcl => src/fcl}/octomap_defs.pxd (100%) rename {fcl => src/fcl}/std_defs.pxd (100%) rename {fcl => src/fcl}/version.py (100%) diff --git a/.gitignore b/.gitignore index 8f238da..2cd1861 100644 --- a/.gitignore +++ b/.gitignore @@ -25,6 +25,7 @@ wheels/ .installed.cfg *.egg MANIFEST +deps/ # PyInstaller # Usually these files are written by a python script from a template diff --git a/MANIFEST.in b/MANIFEST.in deleted file mode 100644 index d616172..0000000 --- a/MANIFEST.in +++ /dev/null @@ -1,5 +0,0 @@ -# Include the license -include LICENSE -include fcl/fcl_defs.pxd -include fcl/octomap_defs.pxd -include fcl/std_defs.pxd diff --git a/requirements/Dockerfile b/build_dependencies/Dockerfile similarity index 100% rename from requirements/Dockerfile rename to build_dependencies/Dockerfile diff --git a/build_dependencies/install_linux.sh b/build_dependencies/install_linux.sh new file mode 100644 index 0000000..022f08c --- /dev/null +++ b/build_dependencies/install_linux.sh @@ -0,0 +1,41 @@ +mkdir -p deps +cd deps +get eigen +curl -OL https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz +tar -zxf eigen-3.3.9.tar.gz + +rm -rf libccd +git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd.git + +rm -rf octomap +git clone --depth 1 --branch v1.8.0 https://github.com/OctoMap/octomap.git + +rm -rf fcl +git clone --depth 1 --branch v0.6.1 https://github.com/flexible-collision-library/fcl.git + +echo "Install eigen" +cmake -B build -S eigen-3.3.9 +cmake --install build + +echo "Build and install libccd" +cd libccd +cmake . +make -j4 +make install +cd .. + +echo "Build and install octomap" +cd octomap +cmake . +make -j4 +make install +cd .. + +echo "Build and install fcl" +cd fcl +cmake . +make -j4 +make install +cd .. + +cd .. \ No newline at end of file diff --git a/fcl/__init__.py b/fcl/__init__.py deleted file mode 100644 index e852be0..0000000 --- a/fcl/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, distance, defaultCollisionCallback, defaultDistanceCallback - -from .collision_data import OBJECT_TYPE, NODE_TYPE, CCDMotionType, CCDSolverType, GJKSolverType, Contact, CostSource, CollisionRequest, CollisionResult, ContinuousCollisionRequest, ContinuousCollisionResult, DistanceRequest, DistanceResult, CollisionData, DistanceData - -from .version import __version__ diff --git a/pyproject.toml b/pyproject.toml index 249cd8b..bb25658 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,7 +3,7 @@ requires = ["setuptools", "wheel", "numpy", "Cython"] build-backend = "setuptools.build_meta" [tool.cibuildwheel] -before-all = "bash requirements/clone.bash && bash requirements/build.bash" +before-all = "bash build_dependencies/install_linux.sh" before-build = "pip install numpy cython" manylinux-x86_64-image = "manylinux_2_24" test-requires = "pytest" diff --git a/requirements/build.bash b/requirements/build.bash deleted file mode 100644 index 0d1653b..0000000 --- a/requirements/build.bash +++ /dev/null @@ -1,24 +0,0 @@ -echo "Install eigen" -cmake -B build -S eigen-3.3.9 -cmake --install build - -echo "Build and install libccd" -cd libccd -cmake . -make -j4 -make install -cd .. - -echo "Build and install octomap" -cd octomap -cmake . -make -j4 -make install -cd .. - -echo "Build and install fcl" -cd fcl -cmake . -make -j4 -make install -cd .. diff --git a/requirements/clone.bash b/requirements/clone.bash deleted file mode 100644 index e200678..0000000 --- a/requirements/clone.bash +++ /dev/null @@ -1,12 +0,0 @@ -rm -rf libccd -git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd.git - -rm -rf octomap -git clone --depth 1 --branch v1.8.0 https://github.com/OctoMap/octomap.git - -rm -rf fcl -git clone --depth 1 --branch v0.6.1 https://github.com/flexible-collision-library/fcl.git - -# get eigen -curl -OL https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -tar -zxf eigen-3.3.9.tar.gz \ No newline at end of file diff --git a/setup.cfg b/setup.cfg index 3b133e3..d0af2c8 100644 --- a/setup.cfg +++ b/setup.cfg @@ -24,10 +24,21 @@ classifiers = Programming Language :: Python :: 3.5 Programming Language :: Python :: 3.6 keywords = fcl collision distance -packages = fcl + +[options] +include_package_data = True +package_dir = + = src +packages = find: setup_requires = numpy Cython install_requires = numpy Cython + +[options.package_data] +* = *.pyx, *.pxd + +[options.packages.find] +where = src \ No newline at end of file diff --git a/setup.py b/setup.py index 4df633f..ab96748 100644 --- a/setup.py +++ b/setup.py @@ -2,20 +2,23 @@ import sys from setuptools import Extension, setup +from Cython.Build import cythonize def get_include_dirs(): platform_supported = False - for prefix in ['darwin', 'linux', 'bsd']: + for prefix in ["darwin", "linux", "bsd"]: if prefix in sys.platform: platform_supported = True - include_dirs = ['/usr/include', - '/usr/local/include', - '/usr/include/eigen3', - '/usr/local/include/eigen3'] - - if 'CPATH' in os.environ: - include_dirs += os.environ['CPATH'].split(':') + include_dirs = [ + "/usr/include", + "/usr/local/include", + "/usr/include/eigen3", + "/usr/local/include/eigen3", + ] + + if "CPATH" in os.environ: + include_dirs += os.environ["CPATH"].split(":") break if sys.platform == "win32": @@ -25,32 +28,35 @@ def get_include_dirs(): # get the numpy include path from numpy import numpy + include_dirs.append(numpy.get_include()) return include_dirs + def get_libraries_dir(): - for prefix in ['darwin', 'linux', 'bsd']: + for prefix in ["darwin", "linux", "bsd"]: if prefix in sys.platform: platform_supported = True - lib_dirs = ['/usr/lib', - '/usr/local/lib'] - - if 'LD_LIBRARY_PATH' in os.environ: - lib_dirs += os.environ['LD_LIBRARY_PATH'].split(':') + lib_dirs = ["/usr/lib", "/usr/local/lib"] + + if "LD_LIBRARY_PATH" in os.environ: + lib_dirs += os.environ["LD_LIBRARY_PATH"].split(":") return lib_dirs raise NotImplementedError(sys.platform) setup( - ext_modules=[Extension( - "fcl.fcl", - ["fcl/fcl.pyx"], - include_dirs = get_include_dirs(), - library_dirs = get_libraries_dir(), - libraries=[ - "fcl","octomap" - ], - language="c++", - extra_compile_args = ["-std=c++11"] - )] + ext_modules=cythonize( + [ + Extension( + "fcl.fcl", + ["src/fcl/fcl.pyx"], + include_dirs=get_include_dirs(), + library_dirs=get_libraries_dir(), + libraries=["fcl", "octomap"], + language="c++", + extra_compile_args=["-std=c++11"], + ) + ], + ) ) diff --git a/src/fcl/__init__.py b/src/fcl/__init__.py new file mode 100644 index 0000000..5a6e4af --- /dev/null +++ b/src/fcl/__init__.py @@ -0,0 +1,48 @@ +try: + from .fcl import ( + CollisionObject, + CollisionGeometry, + Transform, + TriangleP, + Box, + Sphere, + Ellipsoid, + Capsule, + Cone, + Cylinder, + Halfspace, + Plane, + BVHModel, + OcTree, + DynamicAABBTreeCollisionManager, + collide, + continuousCollide, + distance, + defaultCollisionCallback, + defaultDistanceCallback, + ) +except ModuleNotFoundError: + import traceback + + traceback.print_exc() + print("Failed to import fcl.fcl. It is probably not correctly compiled.") + +from .collision_data import ( + OBJECT_TYPE, + NODE_TYPE, + CCDMotionType, + CCDSolverType, + GJKSolverType, + Contact, + CostSource, + CollisionRequest, + CollisionResult, + ContinuousCollisionRequest, + ContinuousCollisionResult, + DistanceRequest, + DistanceResult, + CollisionData, + DistanceData, +) + +from .version import __version__ diff --git a/fcl/collision_data.py b/src/fcl/collision_data.py similarity index 100% rename from fcl/collision_data.py rename to src/fcl/collision_data.py diff --git a/fcl/fcl.pyx b/src/fcl/fcl.pyx similarity index 100% rename from fcl/fcl.pyx rename to src/fcl/fcl.pyx diff --git a/fcl/fcl_defs.pxd b/src/fcl/fcl_defs.pxd similarity index 100% rename from fcl/fcl_defs.pxd rename to src/fcl/fcl_defs.pxd diff --git a/fcl/octomap_defs.pxd b/src/fcl/octomap_defs.pxd similarity index 100% rename from fcl/octomap_defs.pxd rename to src/fcl/octomap_defs.pxd diff --git a/fcl/std_defs.pxd b/src/fcl/std_defs.pxd similarity index 100% rename from fcl/std_defs.pxd rename to src/fcl/std_defs.pxd diff --git a/fcl/version.py b/src/fcl/version.py similarity index 100% rename from fcl/version.py rename to src/fcl/version.py From bf53e5509e71e3d8bfd0d6ac71c617cb8df7dee4 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Mon, 26 Jul 2021 12:08:43 +0200 Subject: [PATCH 10/54] Add macos build --- .github/workflows/wheels.yml | 4 ++-- build_dependencies/install_macos.sh | 23 +++++++++++++++++++++++ pyproject.toml | 18 ++++++++++++++---- setup.cfg | 2 ++ 4 files changed, 41 insertions(+), 6 deletions(-) create mode 100644 build_dependencies/install_macos.sh diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml index 5c396b6..fa427d6 100644 --- a/.github/workflows/wheels.yml +++ b/.github/workflows/wheels.yml @@ -4,11 +4,11 @@ on: [push, pull_request] jobs: build_wheels: - name: Build wheel py${{matrix.python}} on ${{matrix.platform}} + name: Build wheel on ${{matrix.platform}} runs-on: ${{matrix.platform}} strategy: matrix: - platform: [ubuntu-latest] # [ubuntu-latest, macos-latest, windows-latest] + platform: [ubuntu-latest, macos-latest] # [ubuntu-latest, macos-latest, windows-latest] python: [39] steps: - uses: actions/checkout@v2 diff --git a/build_dependencies/install_macos.sh b/build_dependencies/install_macos.sh new file mode 100644 index 0000000..9d89609 --- /dev/null +++ b/build_dependencies/install_macos.sh @@ -0,0 +1,23 @@ +brew update > /dev/null + +# brew install git +# brew install cmake +# brew install eigen +# brew install libccd +brew install fcl + +# mkdir -p deps +# cd deps +# # Octomap +# git clone https://github.com/OctoMap/octomap +# cd octomap +# git checkout tags/v1.8.0 +# mkdir build +# cd build +# cmake .. +# make +# sudo make install + +# cd .. +# cd .. +# cd .. \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index bb25658..ac99b93 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,8 +3,18 @@ requires = ["setuptools", "wheel", "numpy", "Cython"] build-backend = "setuptools.build_meta" [tool.cibuildwheel] -before-all = "bash build_dependencies/install_linux.sh" -before-build = "pip install numpy cython" +skip = "pp*" +before-build = "pip install numpy Cython" manylinux-x86_64-image = "manylinux_2_24" -test-requires = "pytest" -test-command = "pytest {package}/test" \ No newline at end of file +# test-requires = "pytest" +# test-command = "pytest {package}/test" + +[tool.cibuildwheel.linux] +before-all = "bash build_dependencies/install_linux.sh" +archs = ["x86_64"] + +[tool.cibuildwheel.macos] +before-all = "bash build_dependencies/install_macos.sh" + +[tool.cibuildwheel.windows] +archs = ["AMD64"] \ No newline at end of file diff --git a/setup.cfg b/setup.cfg index d0af2c8..58e63b5 100644 --- a/setup.cfg +++ b/setup.cfg @@ -5,6 +5,8 @@ description = Python bindings for the Flexible Collision Library long_description = file: README.md long_description_content_type = text/markdown url = https://github.com/CyrilWaechter/python-fcl +author = Jelle Feringa, Matthew Matl, Shirokuma, Michael Dawson-Haggerty, See contributor list +author_email = jelleferinga@gmail.com, mmatl@eecs.berkeley.edu, rootstock_acg@yahoo.co.jp maintainer = Cyril Waechter maintainer_email = cyrwae@hotmail.com license = BSD From 0a046fb20b8cef10f74a81a9b5f0edff510c1623 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Tue, 27 Jul 2021 17:35:25 +0200 Subject: [PATCH 11/54] Add windows build --- .github/workflows/wheels.yml | 3 +- build_dependencies/install_windows.ps1 | 101 +++++++++++++++++++++++++ pyproject.toml | 9 ++- setup.cfg | 2 +- setup.py | 66 ++++++++++------ 5 files changed, 150 insertions(+), 31 deletions(-) create mode 100644 build_dependencies/install_windows.ps1 diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml index fa427d6..7858377 100644 --- a/.github/workflows/wheels.yml +++ b/.github/workflows/wheels.yml @@ -8,8 +8,7 @@ jobs: runs-on: ${{matrix.platform}} strategy: matrix: - platform: [ubuntu-latest, macos-latest] # [ubuntu-latest, macos-latest, windows-latest] - python: [39] + platform: [ubuntu-latest, macos-latest, windows-latest] steps: - uses: actions/checkout@v2 - name: Build wheels diff --git a/build_dependencies/install_windows.ps1 b/build_dependencies/install_windows.ps1 new file mode 100644 index 0000000..c0dbdc0 --- /dev/null +++ b/build_dependencies/install_windows.ps1 @@ -0,0 +1,101 @@ +<# +Originally based on script written by Pebaz (https://github.com/Pebaz/python-fcl/blob/master/requirements/build_win32.ps1) +but with many modification in order to use fcl 0.6.1 and install dependencies without admin rights. + +This script builds fcl and it's dependencies for python-fcl on Windows. + +It downloads, builds, installs, and then deletes: + * fcl + * libccd + * eigen + * octomap +#> + +$base_dir = Get-Location + +# Create a directory that encapsulates all dependencies +mkdir -p deps; Set-Location deps + +# All compiled depencies will be install in following folder +$install_dir = "$base_dir\deps\install" + + +#------------------------------------------------------------------------------ +# Eigen +Write-Host "Building Eigen" +$eigen_ver = "3.3.9" +Invoke-WebRequest -Uri https://gitlab.com/libeigen/eigen/-/archive/$eigen_ver/eigen-$eigen_ver.tar.gz -Outfile eigen-$eigen_ver.tar.gz +tar -zxf "eigen-$eigen_ver.tar.gz" +Set-Location "eigen-$eigen_ver" + +cmake -B build ` + -D CMAKE_BUILD_TYPE=Release ` + -G "Visual Studio 16 2019" ` + -D BUILD_SHARED_LIBS=ON ` + -D CMAKE_INSTALL_PREFIX=$install_dir +cmake --install build + +Set-Location .. + + +# ------------------------------------------------------------------------------ +# LibCCD +Write-Host "Building LibCCD" +git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd +Set-Location libccd + +cmake -B build ` + -D CMAKE_BUILD_TYPE=Release ` + -G "Visual Studio 16 2019" ` + -D BUILD_SHARED_LIBS=ON ` + -D CMAKE_INSTALL_PREFIX=$install_dir +cmake --build build --config Release --target install + +Set-Location .. + + +# ------------------------------------------------------------------------------ +# Octomap +Write-Host "Building Octomap" +git clone --depth 1 --branch v1.8.0 https://github.com/OctoMap/octomap +Set-Location octomap + +cmake -B build ` + -D CMAKE_PREFIX_PATH=$install_dir ` + -D CMAKE_BUILD_TYPE=Release ` + -G "Visual Studio 16 2019" ` + -D BUILD_SHARED_LIBS=ON ` + -D CMAKE_INSTALL_PREFIX=$install_dir ` + -D BUILD_OCTOVIS_SUBPROJECT=OFF ` + -D BUILD_DYNAMICETD3D_SUBPROJECT=OFF +cmake --build build --config Release +cmake --build build --config Release --target install + +Set-Location .. + +# ------------------------------------------------------------------------------ +# FCL +Write-Host "Building FCL" +git clone --depth 1 --branch v0.6.1 https://github.com/flexible-collision-library/fcl +Set-Location fcl + +cmake -B build ` + -D CMAKE_PREFIX_PATH=$install_dir ` + -D CMAKE_BUILD_TYPE=Release ` + -G "Visual Studio 16 2019" ` + -D CMAKE_INSTALL_PREFIX=$install_dir + +cmake --build build --config Release --target install +Write-Host "Done" +Set-Location .. + +# ------------------------------------------------------------------------------ +# Python-FCL + +Write-Host "Copying dependent DLLs" +Copy-Item $install_dir\bin\octomap.dll $base_dir\src\fcl +Copy-Item $install_dir\bin\octomath.dll $base_dir\src\fcl +Copy-Item $install_dir\bin\ccd.dll $base_dir\src\fcl + +Set-Location $base_dir +Write-Host "All done!" diff --git a/pyproject.toml b/pyproject.toml index ac99b93..aba56a1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -6,8 +6,6 @@ build-backend = "setuptools.build_meta" skip = "pp*" before-build = "pip install numpy Cython" manylinux-x86_64-image = "manylinux_2_24" -# test-requires = "pytest" -# test-command = "pytest {package}/test" [tool.cibuildwheel.linux] before-all = "bash build_dependencies/install_linux.sh" @@ -15,6 +13,11 @@ archs = ["x86_64"] [tool.cibuildwheel.macos] before-all = "bash build_dependencies/install_macos.sh" +test-requires = "pytest" +test-command = "pytest {package}/test" [tool.cibuildwheel.windows] -archs = ["AMD64"] \ No newline at end of file +before-all = "powershell build_dependencies\\install_windows.ps1" +archs = ["AMD64"] +test-requires = "pytest" +test-command = "pytest {package}\\test" \ No newline at end of file diff --git a/setup.cfg b/setup.cfg index 58e63b5..aad6d41 100644 --- a/setup.cfg +++ b/setup.cfg @@ -40,7 +40,7 @@ install_requires = Cython [options.package_data] -* = *.pyx, *.pxd +* = *.pyx, *.pxd, *.dll [options.packages.find] where = src \ No newline at end of file diff --git a/setup.py b/setup.py index ab96748..a7fb666 100644 --- a/setup.py +++ b/setup.py @@ -4,26 +4,34 @@ from setuptools import Extension, setup from Cython.Build import cythonize +INSTALL_PREFIX_WIN = "deps\\install" -def get_include_dirs(): - platform_supported = False + +def is_nix_platform(platform): for prefix in ["darwin", "linux", "bsd"]: if prefix in sys.platform: - platform_supported = True - include_dirs = [ - "/usr/include", - "/usr/local/include", - "/usr/include/eigen3", - "/usr/local/include/eigen3", - ] - - if "CPATH" in os.environ: - include_dirs += os.environ["CPATH"].split(":") - - break - if sys.platform == "win32": - platform_supported = False - if not platform_supported: + return True + return False + + +def get_include_dirs(): + if is_nix_platform(sys.platform): + include_dirs = [ + "/usr/include", + "/usr/local/include", + "/usr/include/eigen3", + "/usr/local/include/eigen3", + ] + + if "CPATH" in os.environ: + include_dirs += os.environ["CPATH"].split(":") + + elif sys.platform == "win32": + include_dirs = [ + f"{INSTALL_PREFIX_WIN}\\include", + f"{INSTALL_PREFIX_WIN}\\include\\eigen3", + ] + else: raise NotImplementedError(sys.platform) # get the numpy include path from numpy @@ -34,17 +42,25 @@ def get_include_dirs(): def get_libraries_dir(): - for prefix in ["darwin", "linux", "bsd"]: - if prefix in sys.platform: - platform_supported = True - lib_dirs = ["/usr/lib", "/usr/local/lib"] + if is_nix_platform(sys.platform): + lib_dirs = ["/usr/lib", "/usr/local/lib"] + + if "LD_LIBRARY_PATH" in os.environ: + lib_dirs += os.environ["LD_LIBRARY_PATH"].split(":") + return lib_dirs + if sys.platform == "win32": + return [f"{INSTALL_PREFIX_WIN}\\lib"] - if "LD_LIBRARY_PATH" in os.environ: - lib_dirs += os.environ["LD_LIBRARY_PATH"].split(":") - return lib_dirs raise NotImplementedError(sys.platform) +def get_libraries(): + libraries = ["fcl", "octomap"] + if sys.platform == "win32": + libraries.extend(["octomath", "ccd", "vcruntime"]) + return libraries + + setup( ext_modules=cythonize( [ @@ -53,7 +69,7 @@ def get_libraries_dir(): ["src/fcl/fcl.pyx"], include_dirs=get_include_dirs(), library_dirs=get_libraries_dir(), - libraries=["fcl", "octomap"], + libraries=get_libraries(), language="c++", extra_compile_args=["-std=c++11"], ) From 369e48c8d1848de3ccec7772e7b02e35803c24d1 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Tue, 27 Jul 2021 21:02:50 +0200 Subject: [PATCH 12/54] Update setup.cfg + refactor ps script --- build_dependencies/install_windows.ps1 | 13 ++++++++----- setup.cfg | 14 ++++++-------- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/build_dependencies/install_windows.ps1 b/build_dependencies/install_windows.ps1 index c0dbdc0..aeae411 100644 --- a/build_dependencies/install_windows.ps1 +++ b/build_dependencies/install_windows.ps1 @@ -11,11 +11,15 @@ It downloads, builds, installs, and then deletes: * octomap #> +# Remember starting location for future usage $base_dir = Get-Location # Create a directory that encapsulates all dependencies mkdir -p deps; Set-Location deps +# Build options +$generator = "Visual Studio 16 2019" + # All compiled depencies will be install in following folder $install_dir = "$base_dir\deps\install" @@ -30,7 +34,7 @@ Set-Location "eigen-$eigen_ver" cmake -B build ` -D CMAKE_BUILD_TYPE=Release ` - -G "Visual Studio 16 2019" ` + -G $generator ` -D BUILD_SHARED_LIBS=ON ` -D CMAKE_INSTALL_PREFIX=$install_dir cmake --install build @@ -46,7 +50,7 @@ Set-Location libccd cmake -B build ` -D CMAKE_BUILD_TYPE=Release ` - -G "Visual Studio 16 2019" ` + -G $generator ` -D BUILD_SHARED_LIBS=ON ` -D CMAKE_INSTALL_PREFIX=$install_dir cmake --build build --config Release --target install @@ -63,7 +67,7 @@ Set-Location octomap cmake -B build ` -D CMAKE_PREFIX_PATH=$install_dir ` -D CMAKE_BUILD_TYPE=Release ` - -G "Visual Studio 16 2019" ` + -G $generator ` -D BUILD_SHARED_LIBS=ON ` -D CMAKE_INSTALL_PREFIX=$install_dir ` -D BUILD_OCTOVIS_SUBPROJECT=OFF ` @@ -82,11 +86,10 @@ Set-Location fcl cmake -B build ` -D CMAKE_PREFIX_PATH=$install_dir ` -D CMAKE_BUILD_TYPE=Release ` - -G "Visual Studio 16 2019" ` + -G $generator ` -D CMAKE_INSTALL_PREFIX=$install_dir cmake --build build --config Release --target install -Write-Host "Done" Set-Location .. # ------------------------------------------------------------------------------ diff --git a/setup.cfg b/setup.cfg index aad6d41..8b75367 100644 --- a/setup.cfg +++ b/setup.cfg @@ -15,16 +15,14 @@ classifiers = Development Status :: 3 - Alpha License :: OSI Approved :: BSD License Operating System :: POSIX :: Linux - Programming Language :: Python :: 2 - Programming Language :: Python :: 2.7 + Operating System :: MacOS + Operating System :: Microsoft :: Windows Programming Language :: Python :: 3 - Programming Language :: Python :: 3.0 - Programming Language :: Python :: 3.1 - Programming Language :: Python :: 3.2 - Programming Language :: Python :: 3.3 - Programming Language :: Python :: 3.4 - Programming Language :: Python :: 3.5 Programming Language :: Python :: 3.6 + Programming Language :: Python :: 3.7 + Programming Language :: Python :: 3.8 + Programming Language :: Python :: 3.9 + Programming Language :: Python :: Implementation :: CPython keywords = fcl collision distance [options] From f64a153457368db0822b793e24b73ef599b03008 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Thu, 29 Jul 2021 12:41:55 +0200 Subject: [PATCH 13/54] Fix build for blender and win ci * Blender use a specific version of numpy. * ci failed to compiled fcl while it was correctyl compiled locally with the same build script. Use precompiled binaries until we found out why. * upload wheels to release --- .github/workflows/wheels.yml | 9 ++++++- blender_requirements.txt | 2 ++ build_dependencies/install_windows.ps1 | 2 +- build_dependencies/install_windows_ci.ps1 | 29 +++++++++++++++++++++++ pyproject.toml | 4 ++-- 5 files changed, 42 insertions(+), 4 deletions(-) create mode 100644 blender_requirements.txt create mode 100644 build_dependencies/install_windows_ci.ps1 diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml index 7858377..b0108c9 100644 --- a/.github/workflows/wheels.yml +++ b/.github/workflows/wheels.yml @@ -15,4 +15,11 @@ jobs: uses: pypa/cibuildwheel@v2.0.0 - uses: actions/upload-artifact@v2 with: - path: ./wheelhouse/*.whl \ No newline at end of file + path: ./wheelhouse/*.whl + - uses: xresloader/upload-to-github-release@v1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + file: ./wheelhouse/*.whl + overwrite: true + update_latest_release: true \ No newline at end of file diff --git a/blender_requirements.txt b/blender_requirements.txt new file mode 100644 index 0000000..f6319cd --- /dev/null +++ b/blender_requirements.txt @@ -0,0 +1,2 @@ +numpy==1.19.5 +Cython \ No newline at end of file diff --git a/build_dependencies/install_windows.ps1 b/build_dependencies/install_windows.ps1 index aeae411..d2822cd 100644 --- a/build_dependencies/install_windows.ps1 +++ b/build_dependencies/install_windows.ps1 @@ -4,7 +4,7 @@ but with many modification in order to use fcl 0.6.1 and install dependencies wi This script builds fcl and it's dependencies for python-fcl on Windows. -It downloads, builds, installs, and then deletes: +It downloads, builds, installs: * fcl * libccd * eigen diff --git a/build_dependencies/install_windows_ci.ps1 b/build_dependencies/install_windows_ci.ps1 new file mode 100644 index 0000000..bec5ebb --- /dev/null +++ b/build_dependencies/install_windows_ci.ps1 @@ -0,0 +1,29 @@ +<# +This script install precompiled dependencies to build python-fcl on Windows: + * fcl + * libccd + * eigen + * octomap +#> + +# Remember starting location for future usage +$base_dir = Get-Location +# Binaries folder +$install_dir = "$base_dir\deps\install" + + +$file_name = "PrecompiledDependenciesWindows.zip" +Invoke-WebRequest -Uri "https://github.com/CyrilWaechter/python-fcl/releases/download/v0.6.1/$file_name" -Outfile $file_name +Expand-Archive $file_name -DestinationPath deps + + +# ------------------------------------------------------------------------------ +# Python-FCL + +Write-Host "Copying dependent DLLs" +Copy-Item $install_dir\bin\octomap.dll $base_dir\src\fcl +Copy-Item $install_dir\bin\octomath.dll $base_dir\src\fcl +Copy-Item $install_dir\bin\ccd.dll $base_dir\src\fcl + +Set-Location $base_dir +Write-Host "All done!" diff --git a/pyproject.toml b/pyproject.toml index aba56a1..e7f48d4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [tool.cibuildwheel] skip = "pp*" -before-build = "pip install numpy Cython" +before-build = "pip install -r blender_requirements.txt" manylinux-x86_64-image = "manylinux_2_24" [tool.cibuildwheel.linux] @@ -17,7 +17,7 @@ test-requires = "pytest" test-command = "pytest {package}/test" [tool.cibuildwheel.windows] -before-all = "powershell build_dependencies\\install_windows.ps1" +before-all = "powershell build_dependencies\\install_windows_ci.ps1" archs = ["AMD64"] test-requires = "pytest" test-command = "pytest {package}\\test" \ No newline at end of file From 4d13c917ba3f5208d6257d8c7e4633da4f941ae0 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Tue, 3 Aug 2021 10:42:53 +0200 Subject: [PATCH 14/54] Fix release tag and remove useless before build cibuildwheel already install dependencies during the process. --- .github/workflows/wheels.yml | 4 +++- pyproject.toml | 11 ++++------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml index b0108c9..e0f1e3f 100644 --- a/.github/workflows/wheels.yml +++ b/.github/workflows/wheels.yml @@ -22,4 +22,6 @@ jobs: with: file: ./wheelhouse/*.whl overwrite: true - update_latest_release: true \ No newline at end of file + draft: false + update_latest_release: true + tag_name: v0.6.1 \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index e7f48d4..cd9a2d3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,11 +1,12 @@ [build-system] -requires = ["setuptools", "wheel", "numpy", "Cython"] +requires = ["setuptools", "wheel", "oldest-supported-numpy", "Cython"] build-backend = "setuptools.build_meta" [tool.cibuildwheel] skip = "pp*" -before-build = "pip install -r blender_requirements.txt" manylinux-x86_64-image = "manylinux_2_24" +test-requires = "pytest" +test-command = "pytest {package}/test" [tool.cibuildwheel.linux] before-all = "bash build_dependencies/install_linux.sh" @@ -13,11 +14,7 @@ archs = ["x86_64"] [tool.cibuildwheel.macos] before-all = "bash build_dependencies/install_macos.sh" -test-requires = "pytest" -test-command = "pytest {package}/test" [tool.cibuildwheel.windows] before-all = "powershell build_dependencies\\install_windows_ci.ps1" -archs = ["AMD64"] -test-requires = "pytest" -test-command = "pytest {package}\\test" \ No newline at end of file +archs = ["AMD64"] \ No newline at end of file From 840fd6491e59e6f32f02428121dd997ec53f6cf8 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Tue, 3 Aug 2021 11:20:49 +0200 Subject: [PATCH 15/54] Fix Linux build : build on manylinux_2010 instead --- pyproject.toml | 1 - 1 file changed, 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index cd9a2d3..ef88f68 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,6 @@ build-backend = "setuptools.build_meta" [tool.cibuildwheel] skip = "pp*" -manylinux-x86_64-image = "manylinux_2_24" test-requires = "pytest" test-command = "pytest {package}/test" From 9c1811619122dd5887d8ce2cb6827eab5cf90f84 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Wed, 4 Aug 2021 11:31:25 +0200 Subject: [PATCH 16/54] Modify metadata/dockerfile/travis to prepare merge --- .travis.yml | 6 ------ README.md | 3 --- build_dependencies/Dockerfile | 13 ++++--------- setup.cfg | 4 ++-- 4 files changed, 6 insertions(+), 20 deletions(-) diff --git a/.travis.yml b/.travis.yml index 76785fe..5172712 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,11 +1,5 @@ env: global: - - CIBW_BEFORE_BUILD="yum install -y gcc && yum install -y glibc.i686 && pip install - numpy cython && cd requirements && bash install_cmake.bash && bash clone.bash - && bash build.bash" - - CIBW_TEST_REQUIRES="nose2" - - CIBW_TEST_COMMAND="cd {project}/test && nose2" - - CIBW_SKIP="cp33-* cp34-*" - TWINE_USERNAME=mmatl matrix: include: diff --git a/README.md b/README.md index 6fc558f..bfe9822 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,3 @@ -# Disclaimer -This is fork of https://github.com/BerkeleyAutomation/python-fcl intented to be used in [ifcopenshell](https://github.com/IfcOpenShell/IfcOpenShell) / [BlenderBIM Add-on](https://blenderbim.org/) as orgininal repo is currently unactive and is not merging any PR. - # python-fcl ### Python Interface for the Flexible Collision Library diff --git a/build_dependencies/Dockerfile b/build_dependencies/Dockerfile index 52965aa..da9d08f 100644 --- a/build_dependencies/Dockerfile +++ b/build_dependencies/Dockerfile @@ -6,16 +6,11 @@ # ln -sf requirements/Dockerfile . # docker build . -t pythonfcl -FROM quay.io/pypa/manylinux_2_24_x86_64:latest - -# clone FCL and libccd -# the exact checkouts are in clone.bash -COPY requirements/clone.bash . -RUN bash clone.bash +FROM quay.io/pypa/manylinux2010_x86_64:latest # build and install libccd and fcl using cmake -COPY requirements/build.bash . -RUN bash build.bash +COPY build_dependencies/install_linux.sh . +RUN bash install_linux.sh # manylinux includes a bunch of pythons # to test with others change this env variable @@ -31,4 +26,4 @@ COPY . /python_fcl RUN pip wheel /python_fcl --no-deps -w wheelhouse/ RUN pip install /python_fcl --no-index -f /wheelhouse RUN ls /wheelhouse -RUN auditwheel repair wheelhouse/python_fcl-0.6.1-cp39-cp39-linux_x86_64.whl --plat manylinux_2_24_x86_64 -w /wheelhouse +RUN auditwheel repair wheelhouse/python_fcl-0.6.1-cp39-cp39-linux_x86_64.whl --plat manylinux2010_x86_64 -w /wheelhouse diff --git a/setup.cfg b/setup.cfg index 8b75367..4b431f7 100644 --- a/setup.cfg +++ b/setup.cfg @@ -7,8 +7,8 @@ long_description_content_type = text/markdown url = https://github.com/CyrilWaechter/python-fcl author = Jelle Feringa, Matthew Matl, Shirokuma, Michael Dawson-Haggerty, See contributor list author_email = jelleferinga@gmail.com, mmatl@eecs.berkeley.edu, rootstock_acg@yahoo.co.jp -maintainer = Cyril Waechter -maintainer_email = cyrwae@hotmail.com +maintainer = Matthew Matl +maintainer_email = mmatl@eecs.berkeley.edu license = BSD license_files = LICENSE classifiers = From 12edfe2deef752171b59f33dc6e7541bc50c8779 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Wed, 4 Aug 2021 12:30:11 +0200 Subject: [PATCH 17/54] Split push/release github actions workflows --- .github/workflows/push.yml | 18 ++++++++++++++++++ .github/workflows/{wheels.yml => release.yml} | 4 ++-- 2 files changed, 20 insertions(+), 2 deletions(-) create mode 100644 .github/workflows/push.yml rename .github/workflows/{wheels.yml => release.yml} (94%) diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml new file mode 100644 index 0000000..7858377 --- /dev/null +++ b/.github/workflows/push.yml @@ -0,0 +1,18 @@ +name: Build + +on: [push, pull_request] + +jobs: + build_wheels: + name: Build wheel on ${{matrix.platform}} + runs-on: ${{matrix.platform}} + strategy: + matrix: + platform: [ubuntu-latest, macos-latest, windows-latest] + steps: + - uses: actions/checkout@v2 + - name: Build wheels + uses: pypa/cibuildwheel@v2.0.0 + - uses: actions/upload-artifact@v2 + with: + path: ./wheelhouse/*.whl \ No newline at end of file diff --git a/.github/workflows/wheels.yml b/.github/workflows/release.yml similarity index 94% rename from .github/workflows/wheels.yml rename to .github/workflows/release.yml index e0f1e3f..c2a68c1 100644 --- a/.github/workflows/wheels.yml +++ b/.github/workflows/release.yml @@ -1,6 +1,6 @@ -name: Build +name: Release -on: [push, pull_request] +on: release jobs: build_wheels: From 2173ff923aba94fb6f64370d8844064413df0758 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Thu, 5 Aug 2021 09:01:28 +0200 Subject: [PATCH 18/54] Remove useless blender_requirement.txt --- blender_requirements.txt | 2 -- 1 file changed, 2 deletions(-) delete mode 100644 blender_requirements.txt diff --git a/blender_requirements.txt b/blender_requirements.txt deleted file mode 100644 index f6319cd..0000000 --- a/blender_requirements.txt +++ /dev/null @@ -1,2 +0,0 @@ -numpy==1.19.5 -Cython \ No newline at end of file From c5d4f1ff73b634cf6ca093122eb91c77d0feefd1 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Fri, 6 Aug 2021 13:00:17 -0400 Subject: [PATCH 19/54] Update src/fcl/fcl.pyx Co-authored-by: Dmitry Neverov --- src/fcl/fcl.pyx | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fcl/fcl.pyx b/src/fcl/fcl.pyx index f15e777..fc4c52c 100644 --- a/src/fcl/fcl.pyx +++ b/src/fcl/fcl.pyx @@ -750,8 +750,9 @@ cdef defs.Quaterniond numpy_to_quaternion3d(a): cdef vec3d_to_numpy(defs.Vector3d vec): return numpy.array([vec[0], vec[1], vec[2]]) -cdef defs.Vector3d numpy_to_vec3d(np.ndarray[double, ndim=1] a): - return defs.Vector3d(&a[0]) +cdef defs.Vector3d numpy_to_vec3d(arr): + cdef double[:] memview = arr.astype(numpy.float64) + return defs.Vector3d(&memview[0]) cdef mat3d_to_numpy(defs.Matrix3d m): return numpy.array([[m(0,0), m(0,1), m(0,2)], @@ -796,4 +797,3 @@ cdef copy_ptr_collision_object(defs.CollisionObjectd*cobj): co = CollisionObject(geom, _no_instance=True) ( co).thisptr = cobj return co - From 9c19f8a371c3fd912493ef3776e8beb8ac89f255 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Fri, 6 Aug 2021 13:00:24 -0400 Subject: [PATCH 20/54] Update src/fcl/fcl.pyx Co-authored-by: Dmitry Neverov --- src/fcl/fcl.pyx | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fcl/fcl.pyx b/src/fcl/fcl.pyx index fc4c52c..ff8209e 100644 --- a/src/fcl/fcl.pyx +++ b/src/fcl/fcl.pyx @@ -759,11 +759,11 @@ cdef mat3d_to_numpy(defs.Matrix3d m): [m(1,0), m(1,1), m(1,2)], [m(2,0), m(2,1), m(2,2)]]) -cdef defs.Matrix3d numpy_to_mat3d(np.ndarray[double, ndim=2] a): +cdef defs.Matrix3d numpy_to_mat3d(arr): # NOTE Eigen defaults to column-major storage, # which corresponds to non-default Fortran mode of ordering in numpy - cdef np.ndarray[double, ndim=2, mode='fortran'] f = np.ndarray.copy(a, order='F') - return defs.Matrix3d(&f[0, 0]) + cdef double[:, :] memview = arr.astype(numpy.float64, order='F') + return defs.Matrix3d(&memview[0, 0]) cdef c_to_python_collision_geometry(defs.const_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2): cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) From 0b4a1cf86e08df57179998d020d5c958d49239a3 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Thu, 9 Sep 2021 12:50:38 -0400 Subject: [PATCH 21/54] add pypi upload step --- .github/workflows/release.yml | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index c2a68c1..50f932b 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -24,4 +24,16 @@ jobs: overwrite: true draft: false update_latest_release: true - tag_name: v0.6.1 \ No newline at end of file + tag_name: v0.6.1 + upload_pypi: + needs: [build_wheels] + runs-on: ubuntu-latest + steps: + - uses: actions/download-artifact@v2 + with: + name: artifact + path: dist + - uses: pypa/gh-action-pypi-publish@v1.4.2 + with: + user: mmatl + password: ${{ secrets.PYPI_API_TOKEN }} From 61b67c8a0518779879d79f422d743657b1a73951 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Thu, 9 Sep 2021 13:20:25 -0400 Subject: [PATCH 22/54] run release on push to master --- .github/workflows/release.yml | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 50f932b..085a94b 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -1,6 +1,9 @@ -name: Release +name: Release To Pypi -on: release +on: + push: + branches: + - master jobs: build_wheels: @@ -13,9 +16,15 @@ jobs: - uses: actions/checkout@v2 - name: Build wheels uses: pypa/cibuildwheel@v2.0.0 + - uses: actions/upload-artifact@v2 with: path: ./wheelhouse/*.whl + - name: Tag Version + id: set_tag + run: | + export VER=$(python -c "exec(open('src/fcl/version.py','r').read());print(__version__)") + echo "::set-output name=tag_name::${VER}" - uses: xresloader/upload-to-github-release@v1 env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} @@ -24,7 +33,7 @@ jobs: overwrite: true draft: false update_latest_release: true - tag_name: v0.6.1 + tag_name: ${{ steps.set_tag.outputs.tag_name }} upload_pypi: needs: [build_wheels] runs-on: ubuntu-latest From 46deaf3277450e9b9a5d8de904ddc18dda8c45e8 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Thu, 9 Sep 2021 13:40:14 -0400 Subject: [PATCH 23/54] github actions new env-variable setting method --- .github/workflows/release.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 085a94b..5acb956 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -22,9 +22,7 @@ jobs: path: ./wheelhouse/*.whl - name: Tag Version id: set_tag - run: | - export VER=$(python -c "exec(open('src/fcl/version.py','r').read());print(__version__)") - echo "::set-output name=tag_name::${VER}" + run: echo "FCL_VERSION=$(python -c \"exec(open('src/fcl/version.py','r').read());print(__version__)\")" >> $GITHUB_ENV - uses: xresloader/upload-to-github-release@v1 env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} @@ -33,7 +31,7 @@ jobs: overwrite: true draft: false update_latest_release: true - tag_name: ${{ steps.set_tag.outputs.tag_name }} + tag_name: ${{ env.FCL_VERSION }} upload_pypi: needs: [build_wheels] runs-on: ubuntu-latest From 3ed68c228d615d4d16a2ef58a391ec92607eab56 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Thu, 9 Sep 2021 14:15:58 -0400 Subject: [PATCH 24/54] get version differently on windows --- .github/workflows/release.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 5acb956..8c8a3fc 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -22,7 +22,12 @@ jobs: path: ./wheelhouse/*.whl - name: Tag Version id: set_tag + if: matrix.os != 'windows-latest' run: echo "FCL_VERSION=$(python -c \"exec(open('src/fcl/version.py','r').read());print(__version__)\")" >> $GITHUB_ENV + - name: Tag Version Windows + id: set_tag + if: matrix.os == 'windows-latest' + run: echo "FCL_VERSION=0.6.1" >> $GITHUB_ENV - uses: xresloader/upload-to-github-release@v1 env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} From 0378a2c7706ad0b5f3dfeeb7f91337c59f3408df Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Thu, 9 Sep 2021 14:59:11 -0400 Subject: [PATCH 25/54] GA tag --- .github/workflows/release.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 8c8a3fc..349f4e1 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -25,7 +25,6 @@ jobs: if: matrix.os != 'windows-latest' run: echo "FCL_VERSION=$(python -c \"exec(open('src/fcl/version.py','r').read());print(__version__)\")" >> $GITHUB_ENV - name: Tag Version Windows - id: set_tag if: matrix.os == 'windows-latest' run: echo "FCL_VERSION=0.6.1" >> $GITHUB_ENV - uses: xresloader/upload-to-github-release@v1 From df3e3473f5dde1b7950ee90398c81566b263da9d Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Thu, 9 Sep 2021 15:10:57 -0400 Subject: [PATCH 26/54] matrix key --- .github/workflows/release.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 349f4e1..38f329b 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -22,10 +22,10 @@ jobs: path: ./wheelhouse/*.whl - name: Tag Version id: set_tag - if: matrix.os != 'windows-latest' + if: matrix.platform != 'windows-latest' run: echo "FCL_VERSION=$(python -c \"exec(open('src/fcl/version.py','r').read());print(__version__)\")" >> $GITHUB_ENV - name: Tag Version Windows - if: matrix.os == 'windows-latest' + if: matrix.platform == 'windows-latest' run: echo "FCL_VERSION=0.6.1" >> $GITHUB_ENV - uses: xresloader/upload-to-github-release@v1 env: From f0836e2fa3b0527a1df8cdd62a6247ba2c159310 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Thu, 9 Sep 2021 15:37:54 -0400 Subject: [PATCH 27/54] skip auto tag for now --- .github/workflows/release.yml | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 38f329b..91bf726 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -16,17 +16,9 @@ jobs: - uses: actions/checkout@v2 - name: Build wheels uses: pypa/cibuildwheel@v2.0.0 - - uses: actions/upload-artifact@v2 with: path: ./wheelhouse/*.whl - - name: Tag Version - id: set_tag - if: matrix.platform != 'windows-latest' - run: echo "FCL_VERSION=$(python -c \"exec(open('src/fcl/version.py','r').read());print(__version__)\")" >> $GITHUB_ENV - - name: Tag Version Windows - if: matrix.platform == 'windows-latest' - run: echo "FCL_VERSION=0.6.1" >> $GITHUB_ENV - uses: xresloader/upload-to-github-release@v1 env: GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} @@ -35,7 +27,7 @@ jobs: overwrite: true draft: false update_latest_release: true - tag_name: ${{ env.FCL_VERSION }} + tag_name: 0.6.1 upload_pypi: needs: [build_wheels] runs-on: ubuntu-latest From 5a8d3598ac5b87a3d38c3e4a7a911e1dd698559b Mon Sep 17 00:00:00 2001 From: Mike Danielczuk Date: Tue, 4 Jan 2022 15:00:12 -0800 Subject: [PATCH 28/54] fix user for pypi --- .github/workflows/release.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 91bf726..44bec03 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -38,5 +38,5 @@ jobs: path: dist - uses: pypa/gh-action-pypi-publish@v1.4.2 with: - user: mmatl + user: __token__ password: ${{ secrets.PYPI_API_TOKEN }} From 96144afb2e56073bf313b656be4748dd9b05b3fd Mon Sep 17 00:00:00 2001 From: Mike Danielczuk Date: Fri, 1 Jul 2022 13:28:57 -0700 Subject: [PATCH 29/54] ci: remove travis, add formatting --- .github/workflows/push.yml | 19 +++++++++- .travis.yml | 31 ---------------- pyproject.toml | 2 +- setup.cfg | 15 +++++++- setup.py | 2 +- src/fcl/collision_data.py | 75 +++++++++++++++++++++++++++++--------- src/fcl/fcl.pyx | 29 +++++++++++---- src/fcl/octomap_defs.pxd | 6 +-- src/fcl/std_defs.pxd | 2 - src/fcl/version.py | 2 +- 10 files changed, 117 insertions(+), 66 deletions(-) delete mode 100644 .travis.yml 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/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/collision_data.py b/src/fcl/collision_data.py index bf01996..5364e2d 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,30 +86,36 @@ 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, @@ -86,6 +123,7 @@ def __init__(self, self.enable_nearest_points = enable_nearest_points 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 +133,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 +144,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 +154,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..4fef172 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 @@ -440,7 +455,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)) 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 "" 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 @@ -__version__ = '0.6.1' +__version__ = "0.6.1" From 2cb71fa33a8d4341d89094ac3ccfff87074e23d6 Mon Sep 17 00:00:00 2001 From: Mike Danielczuk Date: Fri, 1 Jul 2022 13:34:51 -0700 Subject: [PATCH 30/54] fix: py2to3 example --- example/example.py | 190 --------------------------------------- examples/example.py | 212 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 212 insertions(+), 190 deletions(-) delete mode 100644 example/example.py create mode 100644 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("") From 780f4b0e8e2c3f83d025ddd637df3630f849f9ae Mon Sep 17 00:00:00 2001 From: Mike Danielczuk Date: Fri, 1 Jul 2022 13:36:32 -0700 Subject: [PATCH 31/54] feat: expose signed distance flag --- src/fcl/collision_data.py | 10 +++++++--- src/fcl/fcl.pyx | 1 + src/fcl/fcl_defs.pxd | 4 ++-- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/fcl/collision_data.py b/src/fcl/collision_data.py index 5364e2d..0297173 100644 --- a/src/fcl/collision_data.py +++ b/src/fcl/collision_data.py @@ -117,10 +117,14 @@ def __init__(self, is_collide=False, time_of_contact=1.0): 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 diff --git a/src/fcl/fcl.pyx b/src/fcl/fcl.pyx index 4fef172..53e6d85 100644 --- a/src/fcl/fcl.pyx +++ b/src/fcl/fcl.pyx @@ -665,6 +665,7 @@ def distance(CollisionObject o1, CollisionObject o2, cdef double dis = defs.distance(o1.thisptr, o2.thisptr, defs.DistanceRequestd( request.enable_nearest_points, + request.enable_signed_distance, request.gjk_solver_type ), cresult) diff --git a/src/fcl/fcl_defs.pxd b/src/fcl/fcl_defs.pxd index 2bbe78a..d480573 100644 --- a/src/fcl/fcl_defs.pxd +++ b/src/fcl/fcl_defs.pxd @@ -135,8 +135,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: @@ -408,4 +409,3 @@ cdef extern from "fcl/geometry/octree/octree.h" namespace "fcl": # Constructing OcTreed(double resolution) except + OcTreed(shared_ptr[octomap.OcTree]& tree_) except + - From 8ba088220fd4ee464ece60bdbd77c2cfc4d08be6 Mon Sep 17 00:00:00 2001 From: Mike Danielczuk Date: Fri, 1 Jul 2022 13:37:23 -0700 Subject: [PATCH 32/54] feat: bind fcl convex type --- README.md | 21 +++++++++++-- src/fcl/__init__.py | 74 ++++++++++++++++++++++++++++++++++---------- src/fcl/fcl.pyx | 14 +++++++++ src/fcl/fcl_defs.pxd | 15 ++++----- 4 files changed, 95 insertions(+), 29 deletions(-) diff --git a/README.md b/README.md index bfe9822..6c55791 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). 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/fcl.pyx b/src/fcl/fcl.pyx index 53e6d85..e44d800 100644 --- a/src/fcl/fcl.pyx +++ b/src/fcl/fcl.pyx @@ -332,6 +332,20 @@ cdef class Cone(CollisionGeometry): def __set__(self, value): ( self.thisptr).lz = 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 ( self.thisptr).getFaceCount() + cdef class Cylinder(CollisionGeometry): def __cinit__(self, radius, lz): self.thisptr = new defs.Cylinderd(radius, lz) diff --git a/src/fcl/fcl_defs.pxd b/src/fcl/fcl_defs.pxd index d480573..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 @@ -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): From e9f3037b0ebf35eb521bccd127aed9c00da900a3 Mon Sep 17 00:00:00 2001 From: Mike Danielczuk Date: Fri, 1 Jul 2022 13:37:48 -0700 Subject: [PATCH 33/54] fix: min distance nearest points fix, add to tests --- README.md | 2 +- src/fcl/fcl.pyx | 15 +- {test => tests}/test_fcl.py | 301 +++++++++++++++++++++++++++--------- 3 files changed, 234 insertions(+), 84 deletions(-) rename {test => tests}/test_fcl.py (53%) diff --git a/README.md b/README.md index 6c55791..5eff2a6 100644 --- a/README.md +++ b/README.md @@ -362,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/src/fcl/fcl.pyx b/src/fcl/fcl.pyx index e44d800..7738bad 100644 --- a/src/fcl/fcl.pyx +++ b/src/fcl/fcl.pyx @@ -684,13 +684,14 @@ def distance(CollisionObject o1, CollisionObject o2, ), 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/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() From cc7536926ce8cfa42943b77d9f4edd24dd64b03b Mon Sep 17 00:00:00 2001 From: Mike Danielczuk Date: Wed, 6 Jul 2022 16:04:22 -0700 Subject: [PATCH 34/54] bump version for pypi, fix CI versioning --- .github/workflows/release.yml | 35 ++++++++++++++++++++++++++++++++++- src/fcl/version.py | 2 +- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 44bec03..00a37da 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -6,9 +6,42 @@ on: - master jobs: + create_release: + name: Create GitHub Release + runs-on: ubuntu-latest + steps: + - name: Checkout code + uses: actions/checkout@master + with: + fetch-depth: 2 + - name: Get changed files + id: changed-files + uses: tj-actions/changed-files@v5.1 + - name: Tag Version + if: contains(steps.changed-files.outputs.modified_files, 'src/fcl/version.py') + id: set_tag + run: | + export VER=v$(python -c "exec(open('src/fcl/version.py','r').read());print(__version__)") + echo "::set-output name=tag_name::${VER}" + - name: Create Release + if: contains(steps.changed-files.outputs.modified_files, 'src/fcl/version.py') + id: create_release + uses: actions/create-release@v1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + tag_name: ${{ steps.set_tag.outputs.tag_name }} + release_name: ${{ steps.set_tag.outputs.tag_name }} + draft: false + prerelease: false + outputs: + mod_files: ${{ steps.changed-files.outputs.modified_files }} + build_wheels: + if: contains(needs.create_release.outputs.mod_files, 'src/fcl/version.py') name: Build wheel on ${{matrix.platform}} runs-on: ${{matrix.platform}} + needs: create_release strategy: matrix: platform: [ubuntu-latest, macos-latest, windows-latest] @@ -27,7 +60,7 @@ jobs: overwrite: true draft: false update_latest_release: true - tag_name: 0.6.1 + upload_pypi: needs: [build_wheels] runs-on: ubuntu-latest diff --git a/src/fcl/version.py b/src/fcl/version.py index 43c4ab0..33c23c3 100644 --- a/src/fcl/version.py +++ b/src/fcl/version.py @@ -1 +1 @@ -__version__ = "0.6.1" +__version__ = "0.6.11" From 8ffedeb01b21f405cc9ee51cf74800ce100dec50 Mon Sep 17 00:00:00 2001 From: mjd3 Date: Wed, 6 Jul 2022 19:53:37 -0400 Subject: [PATCH 35/54] upgrade to fcl 0.7.0, octomap 1.9.8 (#51) --- .github/workflows/push.yml | 7 +++--- .github/workflows/release.yml | 13 +++++----- README.md | 2 +- build_dependencies/Dockerfile | 10 +++----- build_dependencies/install_linux.sh | 21 +++++++++------- build_dependencies/install_windows.ps1 | 10 ++++---- build_dependencies/install_windows_ci.ps1 | 29 ----------------------- pyproject.toml | 4 ++-- src/fcl/version.py | 2 +- 9 files changed, 35 insertions(+), 63 deletions(-) delete mode 100644 build_dependencies/install_windows_ci.ps1 diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml index d01cb83..2636bb7 100644 --- a/.github/workflows/push.yml +++ b/.github/workflows/push.yml @@ -10,7 +10,7 @@ jobs: - name: Check out source repository uses: actions/checkout@v2 - name: Set up Python environment - uses: actions/setup-python@v2 + uses: actions/setup-python@v3 with: python-version: 3.9 - name: flake8 @@ -24,12 +24,13 @@ jobs: name: Build wheel on ${{matrix.platform}} runs-on: ${{matrix.platform}} strategy: + fail-fast: false matrix: platform: [ubuntu-latest, macos-latest, windows-latest] steps: - uses: actions/checkout@v2 - name: Build wheels - uses: pypa/cibuildwheel@v2.0.0 - - uses: actions/upload-artifact@v2 + uses: pypa/cibuildwheel@v2.7.0 + - uses: actions/upload-artifact@v3 with: path: ./wheelhouse/*.whl \ No newline at end of file diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 00a37da..cc481af 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -48,8 +48,8 @@ jobs: steps: - uses: actions/checkout@v2 - name: Build wheels - uses: pypa/cibuildwheel@v2.0.0 - - uses: actions/upload-artifact@v2 + uses: pypa/cibuildwheel@v2.7.0 + - uses: actions/upload-artifact@v3 with: path: ./wheelhouse/*.whl - uses: xresloader/upload-to-github-release@v1 @@ -60,16 +60,17 @@ jobs: overwrite: true draft: false update_latest_release: true - + upload_pypi: - needs: [build_wheels] + if: contains(needs.create_release.outputs.mod_files, 'src/fcl/version.py') + needs: [create_release, build_wheels] runs-on: ubuntu-latest steps: - - uses: actions/download-artifact@v2 + - uses: actions/download-artifact@v3 with: name: artifact path: dist - - uses: pypa/gh-action-pypi-publish@v1.4.2 + - uses: pypa/gh-action-pypi-publish@v1.5.0 with: user: __token__ password: ${{ secrets.PYPI_API_TOKEN }} diff --git a/README.md b/README.md index 5eff2a6..5948444 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ Python-FCL is an (unofficial) Python interface for the [Flexible Collision Library (FCL)](https://github.com/flexible-collision-library/fcl), an excellent C++ library for performing proximity and collision queries on pairs of geometric models. -Currently, this package is targeted for FCL 0.6.1. +Currently, this package is targeted for FCL 0.7.0. This package supports three types of proximity queries for pairs of geometric models: * __Collision Detection__: Detecting whether two models overlap (and optionally where). diff --git a/build_dependencies/Dockerfile b/build_dependencies/Dockerfile index da9d08f..f1f19d2 100644 --- a/build_dependencies/Dockerfile +++ b/build_dependencies/Dockerfile @@ -1,10 +1,6 @@ # NOTE: this docker file is only for local testing of the build -# it should be copied or symlinked up one directory as docker -# can't go into parent directories # example of testing a build locally: - -# ln -sf requirements/Dockerfile . -# docker build . -t pythonfcl +# docker build . -t pythonfcl -f build_dependencies/Dockerfile FROM quay.io/pypa/manylinux2010_x86_64:latest @@ -24,6 +20,6 @@ RUN pip install numpy cython # build the python-fcl module COPY . /python_fcl RUN pip wheel /python_fcl --no-deps -w wheelhouse/ -RUN pip install /python_fcl --no-index -f /wheelhouse +RUN pip install python_fcl --no-index -f /wheelhouse RUN ls /wheelhouse -RUN auditwheel repair wheelhouse/python_fcl-0.6.1-cp39-cp39-linux_x86_64.whl --plat manylinux2010_x86_64 -w /wheelhouse +RUN auditwheel repair wheelhouse/python_fcl-0.7.0-cp39-cp39-linux_x86_64.whl -w /wheelhouse diff --git a/build_dependencies/install_linux.sh b/build_dependencies/install_linux.sh index 022f08c..1c798c9 100644 --- a/build_dependencies/install_linux.sh +++ b/build_dependencies/install_linux.sh @@ -1,6 +1,9 @@ +# exit immediately on any failed step +set -xe + mkdir -p deps cd deps -get eigen + curl -OL https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz tar -zxf eigen-3.3.9.tar.gz @@ -8,34 +11,34 @@ rm -rf libccd git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd.git rm -rf octomap -git clone --depth 1 --branch v1.8.0 https://github.com/OctoMap/octomap.git +git clone --depth 1 --branch v1.9.8 https://github.com/OctoMap/octomap.git rm -rf fcl -git clone --depth 1 --branch v0.6.1 https://github.com/flexible-collision-library/fcl.git +git clone --depth 1 --branch v0.7.0 https://github.com/ambi-robotics/fcl.git -echo "Install eigen" +# Install eigen cmake -B build -S eigen-3.3.9 cmake --install build -echo "Build and install libccd" +# Build and install libccd cd libccd cmake . make -j4 make install cd .. -echo "Build and install octomap" +# Build and install octomap cd octomap -cmake . +cmake . -D CMAKE_BUILD_TYPE=Release -D BUILD_OCTOVIS_SUBPROJECT=OFF -D BUILD_DYNAMICETD3D_SUBPROJECT=OFF make -j4 make install cd .. -echo "Build and install fcl" +# Build and install fcl cd fcl cmake . make -j4 make install cd .. -cd .. \ No newline at end of file +cd .. diff --git a/build_dependencies/install_windows.ps1 b/build_dependencies/install_windows.ps1 index d2822cd..a0d88c1 100644 --- a/build_dependencies/install_windows.ps1 +++ b/build_dependencies/install_windows.ps1 @@ -1,6 +1,6 @@ <# Originally based on script written by Pebaz (https://github.com/Pebaz/python-fcl/blob/master/requirements/build_win32.ps1) -but with many modification in order to use fcl 0.6.1 and install dependencies without admin rights. +but with many modifications in order to use fcl 0.7.0 and install dependencies without admin rights. This script builds fcl and it's dependencies for python-fcl on Windows. @@ -18,9 +18,9 @@ $base_dir = Get-Location mkdir -p deps; Set-Location deps # Build options -$generator = "Visual Studio 16 2019" +$generator = "Visual Studio 17 2022" -# All compiled depencies will be install in following folder +# All compiled dependencies will be installed in following folder $install_dir = "$base_dir\deps\install" @@ -61,7 +61,7 @@ Set-Location .. # ------------------------------------------------------------------------------ # Octomap Write-Host "Building Octomap" -git clone --depth 1 --branch v1.8.0 https://github.com/OctoMap/octomap +git clone --depth 1 --branch v1.9.8 https://github.com/OctoMap/octomap Set-Location octomap cmake -B build ` @@ -80,7 +80,7 @@ Set-Location .. # ------------------------------------------------------------------------------ # FCL Write-Host "Building FCL" -git clone --depth 1 --branch v0.6.1 https://github.com/flexible-collision-library/fcl +git clone --depth 1 --branch v0.7.0 https://github.com/ambi-robotics/fcl.git Set-Location fcl cmake -B build ` diff --git a/build_dependencies/install_windows_ci.ps1 b/build_dependencies/install_windows_ci.ps1 deleted file mode 100644 index bec5ebb..0000000 --- a/build_dependencies/install_windows_ci.ps1 +++ /dev/null @@ -1,29 +0,0 @@ -<# -This script install precompiled dependencies to build python-fcl on Windows: - * fcl - * libccd - * eigen - * octomap -#> - -# Remember starting location for future usage -$base_dir = Get-Location -# Binaries folder -$install_dir = "$base_dir\deps\install" - - -$file_name = "PrecompiledDependenciesWindows.zip" -Invoke-WebRequest -Uri "https://github.com/CyrilWaechter/python-fcl/releases/download/v0.6.1/$file_name" -Outfile $file_name -Expand-Archive $file_name -DestinationPath deps - - -# ------------------------------------------------------------------------------ -# Python-FCL - -Write-Host "Copying dependent DLLs" -Copy-Item $install_dir\bin\octomap.dll $base_dir\src\fcl -Copy-Item $install_dir\bin\octomath.dll $base_dir\src\fcl -Copy-Item $install_dir\bin\ccd.dll $base_dir\src\fcl - -Set-Location $base_dir -Write-Host "All done!" diff --git a/pyproject.toml b/pyproject.toml index d0847cf..07a350c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,7 +3,7 @@ requires = ["setuptools", "wheel", "oldest-supported-numpy", "Cython"] build-backend = "setuptools.build_meta" [tool.cibuildwheel] -skip = "pp*" +skip = ["pp*", "*musllinux*"] test-requires = "pytest" test-command = "pytest {package}/tests" @@ -15,5 +15,5 @@ archs = ["x86_64"] before-all = "bash build_dependencies/install_macos.sh" [tool.cibuildwheel.windows] -before-all = "powershell build_dependencies\\install_windows_ci.ps1" +before-all = "powershell build_dependencies\\install_windows.ps1" archs = ["AMD64"] \ No newline at end of file diff --git a/src/fcl/version.py b/src/fcl/version.py index 33c23c3..49e0fc1 100644 --- a/src/fcl/version.py +++ b/src/fcl/version.py @@ -1 +1 @@ -__version__ = "0.6.11" +__version__ = "0.7.0" From b2bd50e6aacdc755db6095d87b2bb3b4743636b5 Mon Sep 17 00:00:00 2001 From: mjd3 Date: Fri, 8 Jul 2022 21:11:31 -0700 Subject: [PATCH 36/54] Enable double precision libccd builds for windows/linux (#52) * add debug failure case * fix: use double precision ccd, add tests * bump version for new wheel --- build_dependencies/install_linux.sh | 6 +- build_dependencies/install_windows.ps1 | 1 + src/fcl/version.py | 2 +- tests/test_precision.py | 205 +++++++++++++++++++++++++ 4 files changed, 210 insertions(+), 4 deletions(-) create mode 100644 tests/test_precision.py diff --git a/build_dependencies/install_linux.sh b/build_dependencies/install_linux.sh index 1c798c9..202c386 100644 --- a/build_dependencies/install_linux.sh +++ b/build_dependencies/install_linux.sh @@ -10,7 +10,7 @@ tar -zxf eigen-3.3.9.tar.gz rm -rf libccd git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd.git -rm -rf octomap +rm -rf octomap git clone --depth 1 --branch v1.9.8 https://github.com/OctoMap/octomap.git rm -rf fcl @@ -22,13 +22,13 @@ cmake --install build # Build and install libccd cd libccd -cmake . +cmake . -D ENABLE_DOUBLE_PRECISION=ON make -j4 make install cd .. # Build and install octomap -cd octomap +cd octomap cmake . -D CMAKE_BUILD_TYPE=Release -D BUILD_OCTOVIS_SUBPROJECT=OFF -D BUILD_DYNAMICETD3D_SUBPROJECT=OFF make -j4 make install diff --git a/build_dependencies/install_windows.ps1 b/build_dependencies/install_windows.ps1 index a0d88c1..e0d9f77 100644 --- a/build_dependencies/install_windows.ps1 +++ b/build_dependencies/install_windows.ps1 @@ -52,6 +52,7 @@ cmake -B build ` -D CMAKE_BUILD_TYPE=Release ` -G $generator ` -D BUILD_SHARED_LIBS=ON ` + -D ENABLE_DOUBLE_PRECISION=ON ` -D CMAKE_INSTALL_PREFIX=$install_dir cmake --build build --config Release --target install diff --git a/src/fcl/version.py b/src/fcl/version.py index 49e0fc1..ca046e9 100644 --- a/src/fcl/version.py +++ b/src/fcl/version.py @@ -1 +1 @@ -__version__ = "0.7.0" +__version__ = "0.7.0.1" diff --git a/tests/test_precision.py b/tests/test_precision.py new file mode 100644 index 0000000..7e2d546 --- /dev/null +++ b/tests/test_precision.py @@ -0,0 +1,205 @@ +import unittest + +import numpy as np + +import fcl + + +# These test cases were added because there was a very sneaky crash that would occur +# in the signed distance functions if all libs were not compiled with double precision +class TestPrecision(unittest.TestCase): + def setUp(self): + self.act_dist = -0.00735518 + + # Set up first mesh + f1 = np.array( + [ + [0, 1, 2], + [0, 2, 3], + [3, 4, 5], + [3, 5, 0], + [2, 6, 4], + [2, 4, 3], + [1, 7, 6], + [1, 6, 2], + [7, 5, 4], + [7, 4, 6], + [0, 5, 7], + [0, 7, 1], + ] + ) + tris1 = np.concatenate( + (3 * np.ones((len(f1), 1), dtype=np.int64), f1), axis=1 + ).flatten() + v1 = np.array( + [ + [-1.5596524477005005, -1.4861732721328735, 0.0], + [-1.55965256690979, -1.3408161401748657, 0.0], + [0.8114118576049805, -1.3408160209655762, 0.0], + [0.8114122152328491, -1.4861732721328735, 0.0], + [0.8114122152328491, -1.4861732721328735, 2.566922187805176], + [-1.559652328491211, -1.4861699342727661, 2.566922187805176], + [0.8114122152328491, -1.3408160209655762, 2.566922187805176], + [-1.5596498250961304, -1.3408160209655762, 2.566922187805176], + ] + ) + t1 = np.zeros(3) + r1 = np.eye(3) + + # Set up second mesh + f2 = np.array( + [ + [0, 1, 2], + [0, 2, 3], + [3, 2, 4], + [3, 4, 5], + [5, 4, 6], + [5, 6, 7], + [7, 6, 1], + [7, 1, 0], + [3, 5, 7], + [3, 7, 0], + [4, 2, 1], + [4, 1, 6], + ] + ) + tris2 = np.concatenate( + (3 * np.ones((len(f2), 1), dtype=np.int64), f2), axis=1 + ).flatten() + v2 = np.array( + [ + [-0.035486817359924316, -0.15411892533302307, 0.08323988318443298], + [-0.035486817359924316, -0.034327179193496704, 0.13931824266910553], + [-0.035486817359924316, -0.004236131906509399, 0.07503926753997803], + [-0.035486817359924316, -0.12402787804603577, 0.018960915505886078], + [0.035486817359924316, -0.004236131906509399, 0.07503926753997803], + [0.035486817359924316, -0.12402787804603577, 0.018960915505886078], + [0.035486817359924316, -0.034327179193496704, 0.13931824266910553], + [0.035486817359924316, -0.15411892533302307, 0.08323988318443298], + ] + ) + t2 = np.array([-0.17816561357553456, -1.1899346069644454, 1.1061233975645952]) + r2 = np.array( + [ + [-0.7731251991739131, 0.3586043541062879, -0.5231446679631834], + [0.4540350514442685, 0.8888443213778688, -0.06170854409493361], + [0.4428652147801832, -0.28523444667558645, -0.8500068893646534], + ] + ) + + # Wrap meshes in FCL Convex objects + c1 = fcl.Convex(v1, len(f1), tris1) + c2 = fcl.Convex(v2, len(f2), tris2) + + # Wrap in CollisionObjects + self.o1 = fcl.CollisionObject(c1, fcl.Transform(r1, t1)) + self.o2 = fcl.CollisionObject(c2, fcl.Transform(r2, t2)) + + # Create managers + self.mgr1 = fcl.DynamicAABBTreeCollisionManager() + self.mgr2 = fcl.DynamicAABBTreeCollisionManager() + + self.mgr1.registerObjects([self.o1]) + self.mgr2.registerObjects([self.o2]) + + self.mgr1.setup() + self.mgr2.setup() + + def test_obj_obj_coll(self): + request = fcl.CollisionRequest() + result = fcl.CollisionResult() + ret = fcl.collide(self.o1, self.o2, request, result) + assert ret == 1 # Objects are in collision + + request = fcl.CollisionRequest() + result = fcl.CollisionResult() + ret = fcl.collide(self.o2, self.o1, request, result) + assert ret == 1 # Objects are in collision + + def test_obj_obj_simple_distance(self): + request = fcl.DistanceRequest() + result = fcl.DistanceResult() + ret = fcl.distance(self.o1, self.o2, request, result) + assert ret == -1 + + request = fcl.DistanceRequest() + result = fcl.DistanceResult() + ret = fcl.distance(self.o2, self.o1, request, result) + assert ret == -1 + + def test_obj_obj_signed_distance(self): + + request = fcl.DistanceRequest( + enable_nearest_points=True, enable_signed_distance=True + ) + result = fcl.DistanceResult() + ret = fcl.distance(self.o1, self.o2, request, result) + assert np.isclose(ret, self.act_dist) + assert np.isclose(np.linalg.norm(np.subtract(*result.nearest_points)), abs(ret)) + + request = fcl.DistanceRequest( + enable_nearest_points=True, enable_signed_distance=True + ) + result = fcl.DistanceResult() + ret = fcl.distance(self.o2, self.o1, request, result) + assert np.isclose(ret, self.act_dist) + assert np.isclose(np.linalg.norm(np.subtract(*result.nearest_points)), abs(ret)) + + def test_mgr_obj_coll(self): + req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) + rdata = fcl.CollisionData(request=req) + self.mgr1.collide(self.o2, rdata, fcl.defaultCollisionCallback) + assert rdata.result.is_collision + + rdata = fcl.CollisionData(request=req) + self.mgr2.collide(self.o1, rdata, fcl.defaultCollisionCallback) + assert rdata.result.is_collision + + def test_mgr_mgr_coll(self): + req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) + rdata = fcl.CollisionData(request=req) + self.mgr1.collide(self.mgr2, rdata, fcl.defaultCollisionCallback) + assert rdata.result.is_collision + + rdata = fcl.CollisionData(request=req) + self.mgr2.collide(self.mgr1, rdata, fcl.defaultCollisionCallback) + assert rdata.result.is_collision + + def test_mgr_obj_signed_distance(self): + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + ddata = fcl.DistanceData(req) + self.mgr1.distance(self.o2, ddata, fcl.defaultDistanceCallback) + assert np.isclose(ddata.result.min_distance, self.act_dist) + assert np.isclose( + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + abs(ddata.result.min_distance), + ) + + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + ddata = fcl.DistanceData(req) + self.mgr2.distance(self.o1, ddata, fcl.defaultDistanceCallback) + assert np.isclose(ddata.result.min_distance, self.act_dist) + assert np.isclose( + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + abs(ddata.result.min_distance), + ) + + def test_mgr_mgr_signed_distance(self): + + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + ddata = fcl.DistanceData(req) + self.mgr1.distance(self.mgr2, ddata, fcl.defaultDistanceCallback) + assert np.isclose(ddata.result.min_distance, self.act_dist) + assert np.isclose( + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + abs(ddata.result.min_distance), + ) + + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + ddata = fcl.DistanceData(req) + self.mgr2.distance(self.mgr1, ddata, fcl.defaultDistanceCallback) + assert np.isclose(ddata.result.min_distance, self.act_dist) + assert np.isclose( + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + abs(ddata.result.min_distance), + ) From 501945475e07b208c7075c9a741cf0631f323532 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Tue, 10 Jan 2023 19:01:25 -0500 Subject: [PATCH 37/54] try newer cibuildwheel --- .github/workflows/release.yml | 2 +- src/fcl/version.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index cc481af..1ac2bf7 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -48,7 +48,7 @@ jobs: steps: - uses: actions/checkout@v2 - name: Build wheels - uses: pypa/cibuildwheel@v2.7.0 + uses: pypa/cibuildwheel@v2.11.4 - uses: actions/upload-artifact@v3 with: path: ./wheelhouse/*.whl diff --git a/src/fcl/version.py b/src/fcl/version.py index ca046e9..fe78b13 100644 --- a/src/fcl/version.py +++ b/src/fcl/version.py @@ -1 +1 @@ -__version__ = "0.7.0.1" +__version__ = "0.7.0.2" From 4c26f808c0713c5876cb49e7893af4a3f7debbdf Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Tue, 10 Jan 2023 19:18:42 -0500 Subject: [PATCH 38/54] try avoiding auto cleanup --- build_dependencies/install_macos.sh | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/build_dependencies/install_macos.sh b/build_dependencies/install_macos.sh index 9d89609..3c6f5bb 100644 --- a/build_dependencies/install_macos.sh +++ b/build_dependencies/install_macos.sh @@ -1,9 +1,6 @@ -brew update > /dev/null +# prevent brew from auto cleanup +export HOMEBREW_NO_INSTALL_CLEANUP=1 -# brew install git -# brew install cmake -# brew install eigen -# brew install libccd brew install fcl # mkdir -p deps @@ -20,4 +17,4 @@ brew install fcl # cd .. # cd .. -# cd .. \ No newline at end of file +# cd .. From 70684795f04a0d1dd3d349fe118dd479647730ee Mon Sep 17 00:00:00 2001 From: Jack Bowman Date: Tue, 17 Jan 2023 15:57:11 -0500 Subject: [PATCH 39/54] feat: macos arm wheels --- .github/workflows/push.yml | 4 +++- .github/workflows/release.yml | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml index 2636bb7..46867a7 100644 --- a/.github/workflows/push.yml +++ b/.github/workflows/push.yml @@ -30,7 +30,9 @@ jobs: steps: - uses: actions/checkout@v2 - name: Build wheels - uses: pypa/cibuildwheel@v2.7.0 + uses: pypa/cibuildwheel@v2.12.0 + env: + CIBW_ARCHS_MACOS: 'x86_64 arm64' - uses: actions/upload-artifact@v3 with: path: ./wheelhouse/*.whl \ No newline at end of file diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 1ac2bf7..0b8870f 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -48,7 +48,9 @@ jobs: steps: - uses: actions/checkout@v2 - name: Build wheels - uses: pypa/cibuildwheel@v2.11.4 + uses: pypa/cibuildwheel@v2.12.0 + env: + CIBW_ARCHS_MACOS: 'x86_64 arm64' - uses: actions/upload-artifact@v3 with: path: ./wheelhouse/*.whl From cb03f823819a2ba9e479355361d4bd012834c2cb Mon Sep 17 00:00:00 2001 From: Jack Bowman Date: Tue, 17 Jan 2023 16:07:49 -0500 Subject: [PATCH 40/54] feat: format and upgrade github actions --- .github/workflows/push.yml | 36 ++++++++++++++++----------------- .github/workflows/release.yml | 38 +++++++++++++++++------------------ 2 files changed, 37 insertions(+), 37 deletions(-) diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml index 2636bb7..0edbeb9 100644 --- a/.github/workflows/push.yml +++ b/.github/workflows/push.yml @@ -7,18 +7,18 @@ jobs: 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@v3 - with: - python-version: 3.9 - - name: flake8 - uses: py-actions/flake8@v2 - - name: black - uses: psf/black@stable - with: - options: "--check --diff -l 90" + - name: Check out source repository + uses: actions/checkout@v3 + - name: Set up Python environment + uses: actions/setup-python@v4 + 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}} @@ -28,9 +28,9 @@ jobs: matrix: platform: [ubuntu-latest, macos-latest, windows-latest] steps: - - uses: actions/checkout@v2 - - name: Build wheels - uses: pypa/cibuildwheel@v2.7.0 - - uses: actions/upload-artifact@v3 - with: - path: ./wheelhouse/*.whl \ No newline at end of file + - uses: actions/checkout@v3 + - name: Build wheels + uses: pypa/cibuildwheel@v2.12.0 + - uses: actions/upload-artifact@v3 + with: + path: ./wheelhouse/*.whl diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 1ac2bf7..0171f6c 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -11,18 +11,18 @@ jobs: runs-on: ubuntu-latest steps: - name: Checkout code - uses: actions/checkout@master + uses: actions/checkout@v3 with: fetch-depth: 2 - name: Get changed files id: changed-files - uses: tj-actions/changed-files@v5.1 + uses: tj-actions/changed-files@v35.4.1 - name: Tag Version if: contains(steps.changed-files.outputs.modified_files, 'src/fcl/version.py') id: set_tag run: | export VER=v$(python -c "exec(open('src/fcl/version.py','r').read());print(__version__)") - echo "::set-output name=tag_name::${VER}" + echo echo "tag_name=${VER}" >> $GITHUB_OUTPUT - name: Create Release if: contains(steps.changed-files.outputs.modified_files, 'src/fcl/version.py') id: create_release @@ -46,21 +46,21 @@ jobs: matrix: platform: [ubuntu-latest, macos-latest, windows-latest] steps: - - uses: actions/checkout@v2 - - name: Build wheels - uses: pypa/cibuildwheel@v2.11.4 - - uses: actions/upload-artifact@v3 - with: - path: ./wheelhouse/*.whl - - uses: xresloader/upload-to-github-release@v1 - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - with: - file: ./wheelhouse/*.whl - overwrite: true - draft: false - update_latest_release: true - + - uses: actions/checkout@v3 + - name: Build wheels + uses: pypa/cibuildwheel@v2.12.0 + - uses: actions/upload-artifact@v3 + with: + path: ./wheelhouse/*.whl + - uses: xresloader/upload-to-github-release@v1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + file: ./wheelhouse/*.whl + overwrite: true + draft: false + update_latest_release: true + upload_pypi: if: contains(needs.create_release.outputs.mod_files, 'src/fcl/version.py') needs: [create_release, build_wheels] @@ -70,7 +70,7 @@ jobs: with: name: artifact path: dist - - uses: pypa/gh-action-pypi-publish@v1.5.0 + - uses: pypa/gh-action-pypi-publish@v1.6.4 with: user: __token__ password: ${{ secrets.PYPI_API_TOKEN }} From 8747d18c8469b2c48f058b7d8e5eab77d854c05c Mon Sep 17 00:00:00 2001 From: Jack Bowman Date: Wed, 18 Jan 2023 14:33:53 -0500 Subject: [PATCH 41/54] fix: bump version to 0.7.0.3 --- src/fcl/version.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fcl/version.py b/src/fcl/version.py index fe78b13..3ebcf68 100644 --- a/src/fcl/version.py +++ b/src/fcl/version.py @@ -1 +1 @@ -__version__ = "0.7.0.2" +__version__ = "0.7.0.3" From 6318381bcaa438f599a5cdef1d7fa02ca1ed67a7 Mon Sep 17 00:00:00 2001 From: Jack Bowman Date: Thu, 19 Jan 2023 12:37:10 -0500 Subject: [PATCH 42/54] fix: release.yml echo echo --- .github/workflows/release.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 062078f..d486c39 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -22,7 +22,7 @@ jobs: id: set_tag run: | export VER=v$(python -c "exec(open('src/fcl/version.py','r').read());print(__version__)") - echo echo "tag_name=${VER}" >> $GITHUB_OUTPUT + echo "tag_name=${VER}" >> $GITHUB_OUTPUT - name: Create Release if: contains(steps.changed-files.outputs.modified_files, 'src/fcl/version.py') id: create_release From dc999536756a9f43c2edd85a53332cd06bdde3a0 Mon Sep 17 00:00:00 2001 From: Michael Dawson-Haggerty Date: Thu, 19 Jan 2023 13:53:51 -0500 Subject: [PATCH 43/54] Update version.py Bump version to trigger CI. --- src/fcl/version.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fcl/version.py b/src/fcl/version.py index 3ebcf68..52ef09c 100644 --- a/src/fcl/version.py +++ b/src/fcl/version.py @@ -1 +1 @@ -__version__ = "0.7.0.3" +__version__ = "0.7.0.4" From b41e4409ac9e0ab45f0040b77600a44d6fb82c36 Mon Sep 17 00:00:00 2001 From: Eric Vin <8935814+Eric-Vin@users.noreply.github.com> Date: Tue, 15 Aug 2023 11:18:30 -0700 Subject: [PATCH 44/54] Update push.yml --- .github/workflows/push.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml index 75a0c54..a111c45 100644 --- a/.github/workflows/push.yml +++ b/.github/workflows/push.yml @@ -30,7 +30,7 @@ jobs: steps: - uses: actions/checkout@v3 - name: Build wheels - uses: pypa/cibuildwheel@v2.12.0 + uses: pypa/cibuildwheel@v2.15.0 env: CIBW_ARCHS_MACOS: 'x86_64 arm64' - uses: actions/upload-artifact@v3 From 942657e86184a73ada439ef5b8279ec581655c82 Mon Sep 17 00:00:00 2001 From: Eric Vin <8935814+Eric-Vin@users.noreply.github.com> Date: Tue, 15 Aug 2023 11:20:00 -0700 Subject: [PATCH 45/54] Update release.yml --- .github/workflows/release.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index d486c39..f3d2bfe 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -48,7 +48,7 @@ jobs: steps: - uses: actions/checkout@v3 - name: Build wheels - uses: pypa/cibuildwheel@v2.12.0 + uses: pypa/cibuildwheel@v2.15.0 env: CIBW_ARCHS_MACOS: 'x86_64 arm64' - uses: actions/upload-artifact@v3 From e8720115c51646497f2eef5a9a80065d0d76d7d6 Mon Sep 17 00:00:00 2001 From: Eric Vin <8935814+Eric-Vin@users.noreply.github.com> Date: Tue, 15 Aug 2023 11:56:00 -0700 Subject: [PATCH 46/54] Bump Version from 0.7.0.4 -> 0.7.0.5 --- src/fcl/version.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fcl/version.py b/src/fcl/version.py index 52ef09c..c5f016e 100644 --- a/src/fcl/version.py +++ b/src/fcl/version.py @@ -1 +1 @@ -__version__ = "0.7.0.4" +__version__ = "0.7.0.5" From 9bdcfd9946e8ac84f2415cfe5dd5c71ba906515d Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 15 Aug 2023 12:24:25 -0700 Subject: [PATCH 47/54] Ran black on tests. --- tests/test_fcl.py | 8 ++++++-- tests/test_precision.py | 18 ++++++++++++------ 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/tests/test_fcl.py b/tests/test_fcl.py index 11e5932..c94a939 100644 --- a/tests/test_fcl.py +++ b/tests/test_fcl.py @@ -479,7 +479,9 @@ def test_nearest_points(self): manager2.setup() manager2r.setup() - ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True)) + ddata = fcl.DistanceData( + request=fcl.DistanceRequest(enable_nearest_points=True) + ) manager1.distance(manager2, ddata, fcl.defaultDistanceCallback) self.assertAlmostEqual( ddata.result.min_distance, @@ -487,7 +489,9 @@ def test_nearest_points(self): places=6, ) - ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True)) + ddata = fcl.DistanceData( + request=fcl.DistanceRequest(enable_nearest_points=True) + ) manager1.distance(manager2r, ddata, fcl.defaultDistanceCallback) self.assertAlmostEqual( ddata.result.min_distance, diff --git a/tests/test_precision.py b/tests/test_precision.py index 7e2d546..60f647a 100644 --- a/tests/test_precision.py +++ b/tests/test_precision.py @@ -128,7 +128,6 @@ def test_obj_obj_simple_distance(self): assert ret == -1 def test_obj_obj_signed_distance(self): - request = fcl.DistanceRequest( enable_nearest_points=True, enable_signed_distance=True ) @@ -166,7 +165,9 @@ def test_mgr_mgr_coll(self): assert rdata.result.is_collision def test_mgr_obj_signed_distance(self): - req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + req = fcl.DistanceRequest( + enable_signed_distance=True, enable_nearest_points=True + ) ddata = fcl.DistanceData(req) self.mgr1.distance(self.o2, ddata, fcl.defaultDistanceCallback) assert np.isclose(ddata.result.min_distance, self.act_dist) @@ -175,7 +176,9 @@ def test_mgr_obj_signed_distance(self): abs(ddata.result.min_distance), ) - req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + req = fcl.DistanceRequest( + enable_signed_distance=True, enable_nearest_points=True + ) ddata = fcl.DistanceData(req) self.mgr2.distance(self.o1, ddata, fcl.defaultDistanceCallback) assert np.isclose(ddata.result.min_distance, self.act_dist) @@ -185,8 +188,9 @@ def test_mgr_obj_signed_distance(self): ) def test_mgr_mgr_signed_distance(self): - - req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + req = fcl.DistanceRequest( + enable_signed_distance=True, enable_nearest_points=True + ) ddata = fcl.DistanceData(req) self.mgr1.distance(self.mgr2, ddata, fcl.defaultDistanceCallback) assert np.isclose(ddata.result.min_distance, self.act_dist) @@ -195,7 +199,9 @@ def test_mgr_mgr_signed_distance(self): abs(ddata.result.min_distance), ) - req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + req = fcl.DistanceRequest( + enable_signed_distance=True, enable_nearest_points=True + ) ddata = fcl.DistanceData(req) self.mgr2.distance(self.mgr1, ddata, fcl.defaultDistanceCallback) assert np.isclose(ddata.result.min_distance, self.act_dist) From ba2c811980d5dcb9b5f6ec0d300a73c10b3a8dda Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 15 Aug 2023 12:29:10 -0700 Subject: [PATCH 48/54] Added max Cython version. --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 07a350c..0323d58 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,5 +1,5 @@ [build-system] -requires = ["setuptools", "wheel", "oldest-supported-numpy", "Cython"] +requires = ["setuptools", "wheel", "oldest-supported-numpy", "Cython<3.0"] build-backend = "setuptools.build_meta" [tool.cibuildwheel] From 5dd75b4ce852fc8df0e7d1bc1485234a31b91391 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 15 Aug 2023 12:47:49 -0700 Subject: [PATCH 49/54] Rerun black with '-l 90' --- tests/test_fcl.py | 8 ++------ tests/test_precision.py | 16 ++++------------ 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/tests/test_fcl.py b/tests/test_fcl.py index c94a939..11e5932 100644 --- a/tests/test_fcl.py +++ b/tests/test_fcl.py @@ -479,9 +479,7 @@ def test_nearest_points(self): manager2.setup() manager2r.setup() - ddata = fcl.DistanceData( - request=fcl.DistanceRequest(enable_nearest_points=True) - ) + ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True)) manager1.distance(manager2, ddata, fcl.defaultDistanceCallback) self.assertAlmostEqual( ddata.result.min_distance, @@ -489,9 +487,7 @@ def test_nearest_points(self): places=6, ) - ddata = fcl.DistanceData( - request=fcl.DistanceRequest(enable_nearest_points=True) - ) + ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True)) manager1.distance(manager2r, ddata, fcl.defaultDistanceCallback) self.assertAlmostEqual( ddata.result.min_distance, diff --git a/tests/test_precision.py b/tests/test_precision.py index 60f647a..a695cb2 100644 --- a/tests/test_precision.py +++ b/tests/test_precision.py @@ -165,9 +165,7 @@ def test_mgr_mgr_coll(self): assert rdata.result.is_collision def test_mgr_obj_signed_distance(self): - req = fcl.DistanceRequest( - enable_signed_distance=True, enable_nearest_points=True - ) + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) ddata = fcl.DistanceData(req) self.mgr1.distance(self.o2, ddata, fcl.defaultDistanceCallback) assert np.isclose(ddata.result.min_distance, self.act_dist) @@ -176,9 +174,7 @@ def test_mgr_obj_signed_distance(self): abs(ddata.result.min_distance), ) - req = fcl.DistanceRequest( - enable_signed_distance=True, enable_nearest_points=True - ) + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) ddata = fcl.DistanceData(req) self.mgr2.distance(self.o1, ddata, fcl.defaultDistanceCallback) assert np.isclose(ddata.result.min_distance, self.act_dist) @@ -188,9 +184,7 @@ def test_mgr_obj_signed_distance(self): ) def test_mgr_mgr_signed_distance(self): - req = fcl.DistanceRequest( - enable_signed_distance=True, enable_nearest_points=True - ) + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) ddata = fcl.DistanceData(req) self.mgr1.distance(self.mgr2, ddata, fcl.defaultDistanceCallback) assert np.isclose(ddata.result.min_distance, self.act_dist) @@ -199,9 +193,7 @@ def test_mgr_mgr_signed_distance(self): abs(ddata.result.min_distance), ) - req = fcl.DistanceRequest( - enable_signed_distance=True, enable_nearest_points=True - ) + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) ddata = fcl.DistanceData(req) self.mgr2.distance(self.mgr1, ddata, fcl.defaultDistanceCallback) assert np.isclose(ddata.result.min_distance, self.act_dist) From 2f67f5b547c6d6ab6844fdb1bba7e13730f8069c Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 15 Aug 2023 14:17:20 -0700 Subject: [PATCH 50/54] Changed numpy versioning. --- pyproject.toml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 0323d58..6e6a223 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,5 +1,10 @@ [build-system] -requires = ["setuptools", "wheel", "oldest-supported-numpy", "Cython<3.0"] +requires = [ + "setuptools", + "wheel", + "Cython<3.0", + "oldest-supported-numpy; python_version<'3.12'", + "numpy>=1.26.0b1; python_version>='3.12'"] build-backend = "setuptools.build_meta" [tool.cibuildwheel] From 74215b3eab0673b7bbf1a265ecbfef4220a485b2 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 15 Aug 2023 15:37:30 -0700 Subject: [PATCH 51/54] Changed installed numpy version to fixed versions. --- setup.cfg | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/setup.cfg b/setup.cfg index c7a244e..ab8a321 100644 --- a/setup.cfg +++ b/setup.cfg @@ -31,10 +31,12 @@ package_dir = = src packages = find: setup_requires = - numpy + oldest-supported-numpy; python_version<'3.12' + numpy>=1.26.0b1; python_version>='3.12' Cython install_requires = - numpy + oldest-supported-numpy; python_version<'3.12' + numpy>=1.26.0b1; python_version>='3.12' Cython [options.package_data] From 17d1b5697ce51ac1d4cdff8b71f05d2bc24e1270 Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Tue, 15 Aug 2023 19:39:31 -0700 Subject: [PATCH 52/54] Try changing back from oldest-supported-numpy to numpy --- setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/setup.cfg b/setup.cfg index ab8a321..7cc2386 100644 --- a/setup.cfg +++ b/setup.cfg @@ -31,11 +31,11 @@ package_dir = = src packages = find: setup_requires = - oldest-supported-numpy; python_version<'3.12' + numpy; python_version<'3.12' numpy>=1.26.0b1; python_version>='3.12' Cython install_requires = - oldest-supported-numpy; python_version<'3.12' + numpy; python_version<'3.12' numpy>=1.26.0b1; python_version>='3.12' Cython From 43ebd59808ca9227977b26db97f120cc0cf552cc Mon Sep 17 00:00:00 2001 From: Eric Vin Date: Wed, 16 Aug 2023 09:58:26 -0700 Subject: [PATCH 53/54] Tweak to avoid double specifying numpy? --- setup.cfg | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/setup.cfg b/setup.cfg index 7cc2386..1283084 100644 --- a/setup.cfg +++ b/setup.cfg @@ -31,8 +31,7 @@ package_dir = = src packages = find: setup_requires = - numpy; python_version<'3.12' - numpy>=1.26.0b1; python_version>='3.12' + numpy Cython install_requires = numpy; python_version<'3.12' From 418819d9a77870037f8a36ef4827820b48244cd4 Mon Sep 17 00:00:00 2001 From: CyrilWaechter Date: Mon, 1 Jan 2024 10:16:12 +0100 Subject: [PATCH 54/54] Update url and misc in setup.cfg --- setup.cfg | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/setup.cfg b/setup.cfg index 1283084..028d9d0 100644 --- a/setup.cfg +++ b/setup.cfg @@ -4,7 +4,7 @@ version = attr: fcl.__version__ description = Python bindings for the Flexible Collision Library long_description = file: README.md long_description_content_type = text/markdown -url = https://github.com/CyrilWaechter/python-fcl +url = https://github.com/BerkeleyAutomation/python-fcl.git author = Jelle Feringa, Matthew Matl, Shirokuma, Michael Dawson-Haggerty, See contributor list author_email = jelleferinga@gmail.com, mmatl@eecs.berkeley.edu, rootstock_acg@yahoo.co.jp maintainer = Matthew Matl @@ -22,6 +22,8 @@ classifiers = Programming Language :: Python :: 3.7 Programming Language :: Python :: 3.8 Programming Language :: Python :: 3.9 + Programming Language :: Python :: 3.10 + Programming Language :: Python :: 3.11 Programming Language :: Python :: Implementation :: CPython keywords = fcl collision distance