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