def formulate_collision_infos(self,
robot,
trajectory,
group,
distance=0.2):
initial_signed_distance = []
current_normal_T_times_jacobian = []
next_normal_T_times_jacobian = []
start_state = self.get_current_states_for_given_joints(robot.id, group)
time_step_count = 0
for previous_time_step_of_trajectory, current_time_step_of_trajectory, \
next_time_step_of_trajectory in utils.iterate_with_previous_and_next(trajectory):
time_step_count += 1
if next_time_step_of_trajectory is not None:
next_robot_state, next_link_states \
= self.get_joint_and_link_states_at(robot.id, next_time_step_of_trajectory, group)
current_robot_state, current_link_states \
= self.get_joint_and_link_states_at(robot.id, current_time_step_of_trajectory, group)
current_robot_state = current_robot_state[0]
next_robot_state = next_robot_state[0]
zero_vec = [0.0] * len(current_robot_state)
for link_index, current_link_state, next_link_state in itertools.izip(
self.planning_group_ids, current_link_states,
next_link_states):
# Shivendra: ? Don't know how current_normal_T_times_jacobian, next_normal_T_times_jacobian are actually filled
self.get_collision_infos_for_constraints(
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)
self.get_robot_self_collision_infos(
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)
if len(initial_signed_distance) > 0:
initial_signed_distance = np.vstack(initial_signed_distance)
if len(current_normal_T_times_jacobian) > 0:
current_normal_T_times_jacobian = np.vstack(
current_normal_T_times_jacobian)
if len(next_normal_T_times_jacobian) > 0:
next_normal_T_times_jacobian = np.vstack(
next_normal_T_times_jacobian)
self.reset_joint_states(robot.id, start_state, group)
return initial_signed_distance, current_normal_T_times_jacobian, next_normal_T_times_jacobian
def is_trajectory_collision_free(self,
robot_id,
trajectory,
group,
collision_safe_distance=0.05):
collision_free = True
start_state = self.get_current_states_for_given_joints(robot_id, group)
time_step_count = 0
for previous_time_step_of_trajectory, current_time_step_of_trajectory, \
next_time_step_of_trajectory in utils.iterate_with_previous_and_next(trajectory):
time_step_count += 1
if next_time_step_of_trajectory is not None:
next_link_states = self.get_link_states_at(
robot_id, next_time_step_of_trajectory, group)
current_link_states = self.get_link_states_at(
robot_id, current_time_step_of_trajectory, group)
self.reset_joint_states_to(robot_id,
current_time_step_of_trajectory,
group)
for link_index, current_link_state, next_link_state in itertools.izip(
self.planning_group_ids, current_link_states,
next_link_states):
if next_link_state is not None:
for constraint in self.collision_constraints:
# Shivendra: Changed convex sweep closest point to just discrete time closest point checks
cast_closest_points = self.get_convex_sweep_closest_points_dummy(
robot_id,
constraint,
link_index_a=link_index,
distance=collision_safe_distance,
current_state=current_link_state,
next_state=next_link_state)
for cp in cast_closest_points:
dist = cp.contact_distance
if dist < 0 and cp.body_unique_id_a != cp.body_unique_id_b:
# self.print_contact_points(cp, time_step_count, link_index)
collision_free = False
# breaking if at least on collision hit has found
break
cast_closest_points = self.get_convex_sweep_closest_points_dummy(
robot_id,
robot_id,
link_index_a=link_index,
distance=collision_safe_distance,
current_state=current_link_state,
next_state=next_link_state)
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]:
dist = cp.contact_distance
if dist < 0:
# self.print_contact_points(cp, time_step_count, link_index)
collision_free = False
# breaking if at least on collision hit has found
break
if not collision_free:
# breaking if at least on collision hit has found
break
if not collision_free:
# breaking if at least on collision hit has found
break
self.reset_joint_states(robot_id, start_state, group)
return collision_free
def is_trajectory_collision_free(self, robot_id, trajectory, group, collision_safe_distance=0.05):
collision_free = True
start_state = self.get_current_states_for_given_joints(robot_id, group)
time_step_count = 0
for previous_time_step_of_trajectory, current_time_step_of_trajectory, \
next_time_step_of_trajectory in utils.iterate_with_previous_and_next(trajectory):
time_step_count += 1
if next_time_step_of_trajectory is not None:
next_link_states = self.get_link_states_at(robot_id, next_time_step_of_trajectory, group)
current_link_states = self.get_link_states_at(robot_id, current_time_step_of_trajectory, group)
self.reset_joint_states_to(robot_id, current_time_step_of_trajectory, group)
for link_index, current_link_state, next_link_state in itertools.izip(self.planning_group_ids,
current_link_states,
next_link_states):
if next_link_state is not None:
for constraint in self.collision_constraints:
cast_closest_points = [CastClosestPointInfo(*x) for x in
sim.getConvexSweepClosestPoints(robot_id, constraint,
linkIndexA=link_index,
distance=collision_safe_distance,
bodyAfromPosition=current_link_state[0],
bodyAfromOrientation=current_link_state[
1],
# bodyAfromOrientation=[0, 0, 0, 1],
bodyAtoPosition=next_link_state[0],
bodyAtoOrientation=next_link_state[1],
# bodyAtoOrientation=[0, 0, 0, 1],
)]
for cp in cast_closest_points:
dist = cp.contact_distance
if dist < 0 and cp.body_unique_id_a != cp.body_unique_id_b:
# self.print_contact_points(cp, time_step_count, link_index)
collision_free = False
# breaking if at least on collision hit has found
break
cast_closest_points = [CastClosestPointInfo(*x) for x in
sim.getConvexSweepClosestPoints(robot_id, robot_id,
linkIndexA=link_index,
# linkIndexB=link_index_B,
distance=collision_safe_distance,
bodyAfromPosition=current_link_state[
0],
bodyAfromOrientation=
current_link_state[1],
# bodyAfromOrientation=[0, 0, 0, 1],
bodyAtoPosition=next_link_state[0],
bodyAtoOrientation=next_link_state[1],
# bodyAtoOrientation=[0, 0, 0, 1],
)]
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]:
dist = cp.contact_distance
if dist < 0:
# self.print_contact_points(cp, time_step_count, link_index)
collision_free = False
# breaking if at least on collision hit has found
break
if not collision_free:
# breaking if at least on collision hit has found
break
if not collision_free:
# breaking if at least on collision hit has found
break
self.reset_joint_states(robot_id, start_state, group)
return collision_free