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))