def test_collision_and_distance_box_sphere(self): dis, result = fcl.distance( fcl.CollisionObject(self.box, self.trans1), fcl.CollisionObject(self.sphere, self.trans2), fcl.DistanceRequest(True)) self.assertEqual(dis, self._ref_dist) self.assertEqual(result.min_distance, self._ref_dist) self.assertEqual(result.nearest_points, self._nearest_points) self.assertEqual(result.b1, -1) self.assertEqual(result.b2, -1) dis, result = fcl.distance( fcl.CollisionObject(self.box, transform.Transform(transform.Quaternion())), fcl.CollisionObject( self.sphere, transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])), fcl.DistanceRequest(True)) self.assertEqual(dis, -1.0) self.assertEqual(result.min_distance, -1.0) for i in result.nearest_points: for j in i: self.assertAlmostEqual(0.0, j)
def test_collision_and_distance_box_sphere(self): dis, result = fcl.distance( fcl.CollisionObject(self.box, self.trans1), fcl.CollisionObject(self.sphere, self.trans2), fcl.DistanceRequest(True), ) self.assertEqual(dis, self._ref_dist) self.assertEqual(result.min_distance, self._ref_dist) self.assertEqual(result.nearest_points, self._nearest_points) self.assertEqual(result.b1, -1) self.assertEqual(result.b2, -1) dis, result = fcl.distance( fcl.CollisionObject(self.box, transform.Transform(transform.Quaternion())), fcl.CollisionObject(self.sphere, transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])), fcl.DistanceRequest(True), ) self.assertEqual(dis, -1.0) self.assertEqual(result.min_distance, -1.0) for i in result.nearest_points: for j in i: self.assertAlmostEqual(0.0, j)
def test_distance_box_sphere_translated(self): ret, result = fcl.collide( fcl.CollisionObject(self.box, self.trans1), fcl.CollisionObject(self.sphere, self.trans2), fcl.CollisionRequest()) self.assertEqual(result.contacts, []) self.assertEqual(result.cost_sources, []) dis, result = fcl.distance( fcl.CollisionObject(self.box, self.trans1), fcl.CollisionObject(self.sphere, self.trans2), fcl.DistanceRequest(True)) self.assertEqual(dis, self._ref_dist) self.assertEqual(result.min_distance, self._ref_dist) self.assertEqual(result.nearest_points, self._nearest_points) self.assertEqual(result.o2.radius, self._radius) self.assertEqual(result.o1.side, self._side)
def test_distance_box_sphere_translated(self): ret, result = fcl.collide( fcl.CollisionObject(self.box, self.trans1), fcl.CollisionObject(self.sphere, self.trans2), fcl.CollisionRequest(), ) self.assertEqual(result.contacts, []) self.assertEqual(result.cost_sources, []) dis, result = fcl.distance( fcl.CollisionObject(self.box, self.trans1), fcl.CollisionObject(self.sphere, self.trans2), fcl.DistanceRequest(True), ) self.assertEqual(dis, self._ref_dist) self.assertEqual(result.min_distance, self._ref_dist) self.assertEqual(result.nearest_points, self._nearest_points) self.assertEqual(result.o2.radius, self._radius) self.assertEqual(result.o1.side, self._side)
def getLinkDistance(self, l0_name, l1_name, joint_q): # type: (str, str, np._ArrayLike[float]) -> float '''get shortest distance from link with id l0 to link with id l1 for posture joint_q''' from fcl import fcl, collision_data, transform #get link rotation and position in world frame q = iDynTree.VectorDynSize.fromList(joint_q) self.model.dynComp.setRobotState(q, self.dq_zero, self.dq_zero, self.world_gravity) if l0_name in self.model.linkNames: # if robot link f0 = self.model.dynComp.getFrameIndex(l0_name) t0 = self.model.dynComp.getWorldTransform(f0) rot0 = t0.getRotation().toNumPy() pos0 = t0.getPosition().toNumPy() s0 = self.config['scaleCollisionHull'] else: # if world link pos0 = self.link_cuboid_hulls[l0_name][1] rot0 = eulerAnglesToRotationMatrix(self.link_cuboid_hulls[l0_name][2]) s0 = 1 if l1_name in self.model.linkNames: # if robot link f1 = self.model.dynComp.getFrameIndex(l1_name) t1 = self.model.dynComp.getWorldTransform(f1) rot1 = t1.getRotation().toNumPy() pos1 = t1.getPosition().toNumPy() s1 = self.config['scaleCollisionHull'] else: # if world link pos1 = self.link_cuboid_hulls[l1_name][1] rot1 = eulerAnglesToRotationMatrix(self.link_cuboid_hulls[l1_name][2]) s1 = 1 # TODO: use pos and rot of boxes for vals from geometry tags # self.link_cuboid_hulls[l0_name][1], [2] b = np.array(self.link_cuboid_hulls[l0_name][0]) * s0 p = np.array(self.link_cuboid_hulls[l0_name][1]) b0_center = 0.5*np.array([(b[1][0])+(b[0][0]) + p[0], (b[1][1])+(b[0][1]) + p[1], (b[1][2])+(b[0][2]) + p[2]]) b0 = fcl.Box(b[1][0]-b[0][0], b[1][1]-b[0][1], b[1][2]-b[0][2]) b = np.array(self.link_cuboid_hulls[l1_name][0]) * s1 p = np.array(self.link_cuboid_hulls[l1_name][1]) b1_center = 0.5*np.array([(b[1][0])+(b[0][0]) + p[0], (b[1][1])+(b[0][1]) + p[1], (b[1][2])+(b[0][2]) + p[2]]) b1 = fcl.Box(b[1][0]-b[0][0], b[1][1]-b[0][1], b[1][2]-b[0][2]) # move box to pos + box center pos (model has pos in link origin, box has zero at center) o0 = fcl.CollisionObject(b0, transform.Transform(rot0, pos0+b0_center)) o1 = fcl.CollisionObject(b1, transform.Transform(rot1, pos1+b1_center)) distance, d_result = fcl.distance(o0, o1, collision_data.DistanceRequest(True)) if distance < 0: if self.config['verbose'] > 1: print("Collision of {} and {}".format(l0_name, l1_name)) # get proper collision and depth since optimization should also know how much constraint is violated cr = collision_data.CollisionRequest() cr.enable_contact = True cr.enable_cost = True collision, c_result = fcl.collide(o0, o1, cr) # sometimes no contact is found even though distance is less than 0? if len(c_result.contacts): distance = c_result.contacts[0].penetration_depth return distance
ret, result = fcl.collide(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0), transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])), fcl.CollisionObject(fcl.Sphere(4.0), transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])), fcl.CollisionRequest()) print "-- Collision result: ", ret for contact in result.contacts: print contact.o1 print contact.o2 for cost_source in result.cost_sources: print cost_source dis, result = fcl.distance(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0), transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])), fcl.CollisionObject(fcl.Sphere(4.0), transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])), fcl.DistanceRequest(True)) print "-- Distance result: ", dis print result.nearest_points ret, result = fcl.collide(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0), transform.Transform(transform.Quaternion())), fcl.CollisionObject(fcl.Sphere(4.0), transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])), fcl.CollisionRequest()) print "-- Collision result: ", ret for contact in result.contacts: print contact.o1 print contact.o2
def contact(self,obj): # 30 mm tolerance=30 ret, result = fcl.distance(self.bvh, obj.bvh, cd.DistanceRequest(True)) return True if ret<tolerance else False
fcl.CollisionObject( fcl.Sphere(4.0), transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])), fcl.CollisionRequest()) print "-- Collision result: ", ret for contact in result.contacts: print contact.o1 print contact.o2 for cost_source in result.cost_sources: print cost_source dis, result = fcl.distance( fcl.CollisionObject( fcl.Box(1.0, 2.0, 3.0), transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])), fcl.CollisionObject( fcl.Sphere(4.0), transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])), fcl.DistanceRequest(True)) print "-- Distance result: ", dis print result.nearest_points ret, result = fcl.collide( fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0), transform.Transform(transform.Quaternion())), fcl.CollisionObject( fcl.Sphere(4.0), transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])), fcl.CollisionRequest()) print "-- Collision result: ", ret