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 in_collision(self, link_a, link_b, distance):
link_id_a = self.get_pybullet_link_id(link_a)
link_id_b = self.get_pybullet_link_id(link_b)
return len(pw.getClosestPoints(self._pybullet_id, self._pybullet_id, distance, link_id_a, link_id_b)) > 0
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