def get_collision_infos_for_constraints( self, robot, link_index, current_link_state, next_link_state, distance, current_robot_state, next_robot_state, zero_vec, trajectory, group, time_step_count, initial_signed_distance_, current_normal_T_times_jacobian_, next_normal_T_times_jacobian_): for constraint in self.collision_constraints: if robot.id != constraint: cast_closest_points = self.get_convex_sweep_closest_points_dummy( robot.id, # self.collision_constraints, constraint, link_index, current_link_state, next_link_state, distance) for closest_point in cast_closest_points: closest_pt_on_A_at_t = closest_point.position_on_a closest_pt_on_A_at_t_plus_1 = closest_point.position_on_a1 normal_ = np.vstack( closest_point.contact_normal_on_b).reshape(3, 1) normal_ = utils.normalize_vector(normal_) closest_point._replace(contact_normal_on_b=normal_) dist = closest_point.contact_distance fraction = closest_point.contact_fraction if closest_point.link_index_a == link_index and dist < 0: current_state_jacobian_matrix = self.formulate_jacobian_matrix( robot.id, link_index, current_robot_state, current_link_state, closest_pt_on_A_at_t, trajectory, group, time_step_count, zero_vec) next_state_jacobian_matrix = self.formulate_jacobian_matrix( robot.id, link_index, next_robot_state, next_link_state, closest_pt_on_A_at_t_plus_1, trajectory, group, time_step_count + 1, zero_vec) initial_signed_distance_.append(dist) current_normal_T_times_jacobian_.append( np.matmul(fraction * normal_.T, current_state_jacobian_matrix)) next_normal_T_times_jacobian_.append( np.matmul((1 - fraction) * normal_.T, next_state_jacobian_matrix))
def get_robot_self_collision_infos( self, robot_id, link_index, current_link_state, next_link_state, distance, current_robot_state, next_robot_state, zero_vec, trajectory, group, time_step_count, initial_signed_distance, current_normal_T_times_jacobian, next_normal_T_times_jacobian): cast_closest_points = self.get_convex_sweep_closest_points_dummy( robot_id, robot_id, link_index, current_link_state, next_link_state, distance) for cp in cast_closest_points: if cp.link_index_a > -1 and cp.link_index_b > -1: link_a = self.joint_id_to_info[cp.link_index_a].link_name link_b = self.joint_id_to_info[cp.link_index_b].link_name if cp.link_index_a == link_index and cp.link_index_a != cp.link_index_b and not self.ignored_collisions[ link_a, link_b]: closest_pt_on_A_at_t = cp.position_on_a closest_pt_on_A_at_t_plus_1 = cp.position_on_a1 normal_ = 1 * np.vstack(cp.contact_normal_on_b).reshape( 3, 1) normal_ = utils.normalize_vector(normal_) cp._replace(contact_normal_on_b=normal_) dist = cp.contact_distance fraction = cp.contact_fraction if dist < 0: # self.print_contact_points(cp, time_step_count, link_index) current_state_jacobian_matrix = self.formulate_jacobian_matrix( robot_id, link_index, current_robot_state, current_link_state, closest_pt_on_A_at_t, trajectory, group, time_step_count, zero_vec) next_state_jacobian_matrix = self.formulate_jacobian_matrix( robot_id, link_index, next_robot_state, next_link_state, closest_pt_on_A_at_t_plus_1, trajectory, group, time_step_count + 1, zero_vec) initial_signed_distance.append(dist) current_normal_T_times_jacobian.append( np.matmul(fraction * normal_.T, current_state_jacobian_matrix)) next_normal_T_times_jacobian.append( np.matmul((1 - fraction) * normal_.T, next_state_jacobian_matrix))