Fix bug to force CollisionObjects to update AABBs

Author: mmatl

fcl/fcl.pyx

@@ -113,18 +113,21 @@ cdef class CollisionObject:
 
     def setTranslation(self, vec):
         self.thisptr.setTranslation(numpy_to_vec3f(vec))
+        self.thisptr.computeAABB()
 
     def getRotation(self):
         return mat3f_to_numpy(self.thisptr.getRotation())
 
     def setRotation(self, mat):
         self.thisptr.setRotation(numpy_to_mat3f(mat))
+        self.thisptr.computeAABB()
 
     def getQuatRotation(self):
         return quaternion3f_to_numpy(self.thisptr.getQuatRotation())
 
     def setQuatRotation(self, q):
         self.thisptr.setQuatRotation(numpy_to_quaternion3f(q))
+        self.thisptr.computeAABB()
 
     def getTransform(self):
         rot = self.getRotation()
@@ -133,6 +136,7 @@ cdef class CollisionObject:
 
     def setTransform(self, tf):
         self.thisptr.setTransform(deref((<Transform> tf).thisptr))
+        self.thisptr.computeAABB()
 
     def isOccupied(self):
         return self.thisptr.isOccupied()

fcl/fcl_defs.pxd

@@ -183,6 +183,7 @@ cdef extern from "fcl/collision_object.h" namespace "fcl":
         void setTransform(Matrix3f& q, Vec3f& T)
         void setTransform(Transform3f& tf)
         void setUserData(void *data)
+        void computeAABB()
         void *getUserData()
         bool isOccupied()
         bool isFree()

fcl/version.py

@@ -1 +1 @@
-__version__ = '0.0.4'
+__version__ = '0.0.5'

test/test_fcl.py

@@ -203,6 +203,28 @@ def test_managed_collisions(self):
         manager1.collide(manager3, cdata, fcl.defaultCollisionCallback)
         self.assertTrue(cdata.result.is_collision)
 
+    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])))]
+
+        manager.registerObjects(objs)
+        manager.setup()
+
+        self.assertTrue(len(manager.getObjects()) == 2)
+
+        # Many-to-many, internal
+        cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
+        manager.collide(cdata, fcl.defaultCollisionCallback)
+        self.assertFalse(cdata.result.is_collision)
+
+        objs[1].setTranslation(np.array([0.0, 0.0, -0.3]))
+        cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
+        manager.update(objs[1])
+        manager.collide(cdata, fcl.defaultCollisionCallback)
+        self.assertTrue(cdata.result.is_collision)
+
     def test_managed_distances(self):
         manager1 = fcl.DynamicAABBTreeCollisionManager()
         manager2 = fcl.DynamicAABBTreeCollisionManager()