def test(name1, command1, name2, command2):
robot1, robot2 = index_from_name(robots, name1), index_from_name(
robots, name2)
if (robot1 == robot2) or not collisions:
return False
# TODO: check collisions between pairs of inflated adjacent element
for traj1, traj2 in randomize(
product(command1.trajectories, command2.trajectories)):
# TODO: use swept aabbs for element checks
aabbs1, aabbs2 = traj1.get_aabbs(), traj2.get_aabbs()
swept_aabbs1 = {
link: aabb_union(link_aabbs[link] for link_aabbs in aabbs1)
for link in aabbs1[0]
}
swept_aabbs2 = {
link: aabb_union(link_aabbs[link] for link_aabbs in aabbs2)
for link in aabbs2[0]
}
swept_overlap = [
(link1, link2)
for link1, link2 in product(swept_aabbs1, swept_aabbs2)
if aabb_overlap(swept_aabbs1[link1], swept_aabbs2[link2])
]
if not swept_overlap:
continue
# for l1 in set(map(itemgetter(0), swept_overlap)):
# draw_aabb(swept_aabbs1[l1], color=RED)
# for l2 in set(map(itemgetter(1), swept_overlap)):
# draw_aabb(swept_aabbs2[l2], color=BLUE)
for index1, index2 in product(randomize(range(len(traj1.path))),
randomize(range(len(traj2.path)))):
overlap = [(link1, link2)
for link1, link2 in swept_overlap if aabb_overlap(
aabbs1[index1][link1], aabbs2[index2][link2])]
#overlap = list(product(aabbs1[index1], aabbs2[index2]))
if not overlap:
continue
set_configuration(robot1, traj1.path[index1])
set_configuration(robot2, traj2.path[index2])
#wait_if_gui()
#if pairwise_collision(robot1, robot2):
# return True
for link1, link2 in overlap:
if pairwise_link_collision(robot1, link1, robot2, link2):
#wait_if_gui()
return True
return False
def get_world_aabb(self):
return aabb_union(get_aabb(body)
for body in self.fixed) # self.all_bodies
def get_base_aabb(self):
franka_links = get_link_subtree(self.robot, self.franka_link)
base_links = get_link_subtree(self.robot, self.base_link)
return aabb_union(
get_aabb(self.robot, link)
for link in set(base_links) - set(franka_links))