def setUp(self):
self.objs = [
fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0)),
fcl.CollisionObject(fcl.Sphere(4.0)),
fcl.CollisionObject(fcl.Cone(5.0, 6.0))
]
self.manager = fcl.DynamicAABBTreeCollisionManager()
self.res = fcl.CollisionResult()
# self.manager.collide(self.res, self.collide_callback_func)
self._side = (1.0, 2.0, 3.0)
self._radius = 4.0
self._nearest_points = [(-0.5, 0.0, 0.0), (4.0, 0.0, 0.0)]
self._ref_dist = 15.5
self.pt1 = [10.0, 0.0, 0.0]
self.pt2 = [-10.0, 0.0, 0.0]
self.box = fcl.Box(*self._side)
self.sphere = fcl.Sphere(self._radius)
self.cyl = fcl.Cylinder(7.0, 8.0)
self.trans1 = transform.Transform(transform.Quaternion(), self.pt1)
self.trans2 = transform.Transform(transform.Quaternion(), self.pt2)
def test_collision_box_sphere(self):
ret, result = fcl.collide(
fcl.CollisionObject(fcl.Box(*self._side),
transform.Transform(transform.Quaternion())),
fcl.CollisionObject(
fcl.Sphere(self._radius),
transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])),
fcl.CollisionRequest())
self.assertTrue(len(result.contacts) == 1)
self.assertTrue(len(result.cost_sources) == 0)
con = result.contacts[0]
self.assertAlmostEqual(con.penetration_depth, 0.0)
self.assertTrue(con.penetration_depth < sys.float_info.min)
self.assertTrue(isinstance(con.o1, fcl.Box))
self.assertTrue(isinstance(con.o2, fcl.Sphere))
def __init__(self, part_num):
""" Room Parameters """
self.room_dimensions = [7.50, 7.50]
self.room_offset = [-1.25, -1.25]
""" Walls """
self.wall_1 = fcl.CollisionObject(fcl.Box(8.0, 0.5, 0.5),transform.Transform(transform.Quaternion(), [2.50, -1.25, 0.25]))
self.wall_2 = fcl.CollisionObject(fcl.Box(0.5, 8.0, 0.5),transform.Transform(transform.Quaternion(), [6.25, 2.50, 0.25]))
self.wall_3 = fcl.CollisionObject(fcl.Box(8.0, 0.5, 0.5),transform.Transform(transform.Quaternion(), [2.50, 6.25, 0.25]))
self.wall_4 = fcl.CollisionObject(fcl.Box(0.5, 8.0, 0.5),transform.Transform(transform.Quaternion(), [-1.25, 2.50, 0.25]))
""" Obstacles """
self.obs_1 = fcl.CollisionObject(fcl.Box(2.0, 2.0, 1.0),transform.Transform(transform.Quaternion(), [2.50, 2.50, 0.50]))
self.obs_2 = fcl.CollisionObject(fcl.Box(0.75, 0.75, 0.75),transform.Transform(transform.Quaternion(), [5.0, 1.0, 0.375]))
self.obs_3 = fcl.CollisionObject(fcl.Box(0.75, 0.75, 0.75),transform.Transform(transform.Quaternion(), [3.0, 5.0, 0.375]))
self.obs_4 = fcl.CollisionObject(fcl.Box(0.5, 0.5, 0.5),transform.Transform(transform.Quaternion(), [0.0, 3.0, 0.25]))
""" Table Obstacle """
self.obs_5_1 = fcl.CollisionObject(fcl.Box(0.4, 0.4, 0.2),transform.Transform(transform.Quaternion(), [0.6, 0.0, 0.1]))
self.obs_5_2 = fcl.CollisionObject(fcl.Box(0.05, 0.05, 0.15),transform.Transform(transform.Quaternion(), [0.5, 0.1, 0.275]))
self.obs_5_3 = fcl.CollisionObject(fcl.Box(0.05, 0.05, 0.15),transform.Transform(transform.Quaternion(), [0.5, -0.1, 0.275]))
self.obs_5_4 = fcl.CollisionObject(fcl.Box(0.05, 0.25, 0.05),transform.Transform(transform.Quaternion(), [0.5, 0.0, 0.375]))
""" Part 1 YouBot """
self.youbot_Box = fcl.Box(0.7, 0.5, 0.6)
""" Part 2 YouBot """
self.youbot_base = fcl.Box(0.5701, 0.3570, 0.1060)
self.youbot_arm_1 = fcl.Box(0.1697, 0.1699, 0.1060)
self.youbot_arm_2 = fcl.Box(0.22, 0.0745, 0.0823)
self.youbot_arm_3 = fcl.Box(0.1920, 0.0591, 0.0750)
self.youbot_arm_4 = fcl.Box(0.2401, 0.0581, 0.0987)
""" YouBot """
self.section = part_num
self.youbot = fcl.CollisionObject(self.youbot_Box,transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0]))
""" Obstacles as a list """
self.obs = [self.wall_1, self.wall_2, self.wall_3, self.wall_4,
self.obs_1, self.obs_2, self.obs_3, self.obs_4,
self.obs_5_1, self.obs_5_2, self.obs_5_3, self.obs_5_4]
self.res = fcl.CollisionResult()
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
from fcl import fcl, transform
objs = [
fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0)),
fcl.CollisionObject(fcl.Sphere(4.0)),
fcl.CollisionObject(fcl.Cone(5.0, 6.0))
]
# Register objects to DynamicAABBTreeCollisionManager
manager = fcl.DynamicAABBTreeCollisionManager()
print "Before resgister: ", manager.size()
manager.registerObjects(objs)
print "After register 1 : ", manager.size()
manager.registerObject(fcl.CollisionObject(fcl.Cylinder(7.0, 8.0)))
print "After register 2 : ", manager.size()
# Use Callback function
def cb_func(obj1, obj2, res):
print "cb_func start"
ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
print "result: ", ret
return ret
res = fcl.CollisionResult()
manager.collide(res, cb_func)
# Collision calcuration
ret, result = fcl.collide(
fcl.CollisionObject(