def transform_self_collision(self, collision):
"""
:type collision: Collision
:rtype: Collision
"""
link_a = collision.get_original_link_a()
link_b = collision.get_original_link_b()
new_link_a, new_link_b = self.robot.get_chain_reduced_to_controlled_joints(
link_a, link_b)
if new_link_a > new_link_b:
collision = collision.reverse()
new_link_a, new_link_b = new_link_b, new_link_a
new_b_T_r = self.robot.get_fk_np(new_link_b, self.robot_root)
new_a_T_r = self.robot.get_fk_np(new_link_a, self.robot_root)
collision.set_link_a(new_link_a)
collision.set_link_b(new_link_b)
new_b_T_map = np.dot(new_b_T_r, self.root_T_map)
new_a_P_pa = np.dot(np.dot(new_a_T_r, self.root_T_map),
np_point(*collision.get_position_on_a_in_map()))
new_b_P_pb = np.dot(new_b_T_map,
np_point(*collision.get_position_on_b_in_map()))
# r_P_pb = np.dot(self.root_T_map, np_point(*closest_point.position_on_b))
new_b_V_n = np.dot(new_b_T_map,
np_vector(*collision.get_contact_normal_in_map()))
collision.set_position_on_a_in_a(new_a_P_pa[:-1])
collision.set_position_on_b_in_b(new_b_P_pb[:-1])
collision.set_contact_normal_in_b(new_b_V_n[:-1])
return collision
def transform_external_collision(self, collision):
"""
:type collision: Collision
:rtype: Collision
"""
movable_joint = self.robot.get_controlled_parent_joint(collision.get_original_link_a())
new_a = self.robot.get_child_link_of_joint(movable_joint)
new_a_T_r = self.robot.get_fk_np(new_a, self.robot_root)
collision.set_link_a(new_a)
new_a_P_pa = np.dot(np.dot(new_a_T_r, self.root_T_map), np_point(*collision.get_position_on_a_in_map()))
r_P_pb = np.dot(self.root_T_map, np_point(*collision.get_position_on_b_in_map()))
r_V_n = np.dot(self.root_T_map, np_vector(*collision.get_contact_normal_in_map()))
collision.set_position_on_a_in_a(new_a_P_pa[:-1])
collision.set_position_on_b_in_root(r_P_pb[:-1])
collision.set_contact_normal_in_root(r_V_n[:-1])
return collision