def check_collisions(self, cut_off_distances):
"""
:param cut_off_distances: (robot_link, body_b, link_b) -> cut off distance. Contacts between objects not in this
dict or further away than the cut off distance will be ignored.
:type cut_off_distances: dict
:param self_collision_d: distances grater than this value will be ignored
:type self_collision_d: float
:type enable_self_collision: bool
:return: (robot_link, body_b, link_b) -> ContactInfo
:rtype: dict
"""
collisions = Collisions(self.robot)
robot_name = self.robot.get_name()
for (robot_link, body_b,
link_b), distance in cut_off_distances.items():
if robot_name == body_b:
object_id = self.robot.get_pybullet_id()
link_b_id = self.robot.get_pybullet_link_id(link_b)
else:
object_id = self.__get_pybullet_object_id(body_b)
if link_b != CollisionEntry.ALL:
link_b_id = self.get_object(body_b).get_pybullet_link_id(
link_b)
robot_link_id = self.robot.get_pybullet_link_id(robot_link)
if body_b == robot_name or link_b != CollisionEntry.ALL:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
3, robot_link_id, link_b_id)
]
else:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
3, robot_link_id)
]
if len(contacts) > 0:
try:
body_b_object = self.get_object(body_b)
except KeyError:
body_b_object = self.robot
for contact in contacts:
if link_b == CollisionEntry.ALL:
link_b = body_b_object.pybullet_link_id_to_name(
contact.link_index_b)
k = (robot_link, body_b, link_b)
collisions.add(
k,
ClosestPointInfo(contact.position_on_a,
contact.position_on_b,
contact.contact_distance, distance,
robot_link, body_b, link_b,
contact.contact_normal_on_b, k))
return collisions
def check_collisions(self, cut_off_distances):
"""
:param cut_off_distances: (robot_link, body_b, link_b) -> cut off distance. Contacts between objects not in this
dict or further away than the cut off distance will be ignored.
:type cut_off_distances: dict
:param self_collision_d: distances grater than this value will be ignored
:type self_collision_d: float
:type enable_self_collision: bool
:return: (robot_link, body_b, link_b) -> ContactInfo
:rtype: dict
"""
# TODO I think I have to multiply distance with something
collisions = defaultdict(lambda: None)
checked_things = set()
for k, distance in cut_off_distances.items():
(robot_link, body_b, link_b) = k
r_k = (link_b, body_b, robot_link)
if r_k in checked_things:
if r_k in collisions:
collisions[k] = self.__flip_contact_info(collisions[r_k])
continue
robot_link_id = self.robot.get_pybullet_link_id(robot_link)
if self.robot.get_name() == body_b:
object_id = self.robot.get_pybullet_id()
link_b_id = self.robot.get_pybullet_link_id(link_b)
else:
object_id = self.__get_pybullet_object_id(body_b)
if link_b != CollisionEntry.ALL:
link_b_id = self.get_object(body_b).get_pybullet_link_id(
link_b)
if body_b == self.robot.get_name() or link_b != CollisionEntry.ALL:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
3, robot_link_id, link_b_id)
]
else:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
3, robot_link_id)
]
if len(contacts) > 0:
collisions.update(
{k: min(contacts, key=lambda x: x.contact_distance)})
pass
checked_things.add(k)
return collisions
def __flip_contact_info(self, contact_info):
return ContactInfo(
contact_info.contact_flag, contact_info.body_unique_id_b,
contact_info.body_unique_id_a, contact_info.link_index_b,
contact_info.link_index_a, contact_info.position_on_b,
contact_info.position_on_a, [
-contact_info.contact_normal_on_b[0],
-contact_info.contact_normal_on_b[1],
-contact_info.contact_normal_on_b[2]
], contact_info.contact_distance, contact_info.normal_force,
contact_info.lateralFriction1, contact_info.lateralFrictionDir1,
contact_info.lateralFriction2, contact_info.lateralFrictionDir2)
def __should_flip_collision(self, position_on_a_in_map, link_a):
"""
:type collision: ContactInfo
:rtype: bool
"""
new_p = Pose()
new_p.position = Point(*position_on_a_in_map)
new_p.orientation.w = 1
self.__move_hack(new_p)
hack_id = self.__get_pybullet_object_id(self.hack_name)
body_a_id = self.robot.get_pybullet_id()
try:
contact_info3 = ContactInfo(*[
x for x in p.getClosestPoints(hack_id, body_a_id, 0.001)
if abs(x[8] + 0.005) < 0.0005
][0])
return not (contact_info3.body_unique_id_b == body_a_id
and contact_info3.link_index_b
== self.robot.get_pybullet_link_id(link_a))
except Exception as e:
return True
def __should_flip_contact_info(self, contact_info):
"""
:type contact_info: ContactInfo
:rtype: bool
"""
new_p = Pose()
new_p.position = Point(*contact_info.position_on_a)
new_p.orientation.w = 1
self.__move_hack(new_p)
hack_id = self.__get_pybullet_object_id(self.hack_name)
try:
contact_info3 = ContactInfo(*[
x for x in p.getClosestPoints(
hack_id, contact_info.body_unique_id_a, 0.001)
if abs(x[8] + 0.005) < 1e-5
][0])
return not (
contact_info3.body_unique_id_b == contact_info.body_unique_id_a
and contact_info3.link_index_b == contact_info.link_index_a)
except Exception as e:
return True
def check_collisions(self, cut_off_distances, collision_list_size=15):
"""
:param cut_off_distances: (robot_link, body_b, link_b) -> cut off distance. Contacts between objects not in this
dict or further away than the cut off distance will be ignored.
:type cut_off_distances: dict
:param self_collision_d: distances grater than this value will be ignored
:type self_collision_d: float
:type enable_self_collision: bool
:return: (robot_link, body_b, link_b) -> Collision
:rtype: Collisions
"""
collisions = Collisions(self.robot, collision_list_size)
robot_name = self.robot.get_name()
for (robot_link, body_b,
link_b), distance in cut_off_distances.items():
if robot_name == body_b:
object_id = self.robot.get_pybullet_id()
link_b_id = self.robot.get_pybullet_link_id(link_b)
else:
object_id = self.__get_pybullet_object_id(body_b)
if link_b != CollisionEntry.ALL:
link_b_id = self.get_object(body_b).get_pybullet_link_id(
link_b)
robot_link_id = self.robot.get_pybullet_link_id(robot_link)
if body_b == robot_name or link_b != CollisionEntry.ALL:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
1.1, robot_link_id, link_b_id)
]
else:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
1.1, robot_link_id)
]
if len(contacts) > 0:
try:
body_b_object = self.get_object(body_b)
except KeyError:
body_b_object = self.robot
pass
for contact in contacts: # type: ContactInfo
if link_b == CollisionEntry.ALL:
link_b_tmp = body_b_object.pybullet_link_id_to_name(
contact.link_index_b)
else:
link_b_tmp = link_b
if self.__should_flip_collision(contact.position_on_a,
robot_link):
flipped_normal = [
-contact.contact_normal_on_b[0],
-contact.contact_normal_on_b[1],
-contact.contact_normal_on_b[2]
]
collision = Collision(robot_link, body_b, link_b_tmp,
contact.position_on_b,
contact.position_on_a,
flipped_normal,
contact.contact_distance)
collisions.add(collision)
else:
collision = Collision(robot_link, body_b, link_b_tmp,
contact.position_on_a,
contact.position_on_b,
contact.contact_normal_on_b,
contact.contact_distance)
collisions.add(collision)
return collisions