def test_sphere(self):
print("------------------------------ testing sphere ------------------------------")
start = [0.28, 1.0, 0.3]
end = [0.28, 1.8, 0.3]
sphere = self.create_constraint(shape=p.GEOM_SPHERE, radius=0.3, position=start, mass=1)
wall = self.create_constraint(shape=p.GEOM_BOX, size=[0.5, 0.01, 0.5], position=[0, 1.2, 0], mass=1)
trajectory = []
for i in range(3):
traj = self.interpolate(start[i], end[i], 3).tolist()
trajectory.append(traj)
trajectory = np.vstack(trajectory).T
for i in range(len(trajectory)):
p.resetBasePositionAndOrientation(sphere, trajectory[i], [0.0, 0.0, 0.0, 1])
if i < len(trajectory) - 1:
closet_points = p.getClosestPoints(sphere, wall, distance=0.10)
start = time.time()
cast_points = p.getConvexSweepClosestPoints(sphere, wall, distance=0.10,
bodyAfromPosition=trajectory[i],
bodyAtoPosition=trajectory[i + 1],
bodyAfromOrientation=[0.0, 0.0, 0.0, 1],
bodyAtoOrientation=[0.0, 0.0, 0.0, 1]
)
for i in range(len(closet_points)):
if len(closet_points):
if closet_points[i][8] < 0:
print("-------------closest points -----------------")
print("link A index: ", closet_points[i][3])
print("link B index: ", closet_points[i][4])
print("point on A: ", closet_points[i][5])
print("point on B: ", closet_points[i][6])
print("contact normal on B: ", closet_points[i][7])
print("contact distance: ", closet_points[i][8])
print("-------------************ -----------------")
for k in range(len(cast_points)):
if len(cast_points):
if cast_points[i][9] < 0:
print("-------------cast_points -----------------")
print("link A index: ", cast_points[k][3])
print("link B index: ", cast_points[k][4])
print("point on A(t): ", cast_points[k][5])
print("point on A(t+1): ", cast_points[k][6])
print("point on B: ", cast_points[k][7])
print("contact normal on B: ", cast_points[k][8])
print("contact distance: ", cast_points[k][9])
print("contact fraction: ", cast_points[k][10])
print("-------------************ -----------------")
if cast_points[k][10] == 0:
self.assertEquals(np.isclose(cast_points[k][9], closet_points[i][8], atol=0.01), True)
else:
self.assertEquals(np.isclose(cast_points[k][10], 0.5, atol=0.01), True)
print("------------------------------ testing sphere done------------------------------")
def get_convex_sweep_closest_points(self, body_a, body_b, link_index_a, current_state, next_state, distance=0.1):
start = time.time()
cast_closest_points = [CastClosestPointInfo(*x) for x in
sim.getConvexSweepClosestPoints(body_a,
bodyB=body_b,
linkIndexA=link_index_a,
distance=distance,
bodyAfromPosition=current_state[0],
bodyAfromOrientation=current_state[1],
bodyAtoPosition=next_state[0],
bodyAtoOrientation=next_state[1]
)]
end = time.time()
self.collision_check_time += end - start
return cast_closest_points
def test_kukka_arm(self):
print("------------------------------ testing kukka arm ------------------------------")
kukaId = p.loadURDF(self.location_prefix + "kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True)
wall = self.create_constraint(shape=p.GEOM_BOX, size=[0.5, 0.01, 0.18], position=[0.95, 0.3, 0], mass=1)
self.step_simulation_for(0.05)
trajectory = []
# print jointInfo[1], jointNameToId[jointInfo[1]]
start_state = [1.5708024764979394, 1.5712326366904628, 1.57079632708463, 1.5707855978401637,
-1.5701336236638301,
1.5719725305810723, 1.570793057646014]
end_state = [0.6799090616158451, 1.5729501835585853, 1.5704022441549654, 1.6398254945800432,
-1.5625212113651783,
1.596406342019127, 1.568985184095934]
for i in range(len(start_state)):
trajectory.append(self.interpolate(start_state[i], end_state[i], 3, 5))
trajectory = np.asarray(trajectory).T
useRealTimeSimulation = 0
if useRealTimeSimulation:
p.setRealTimeSimulation(useRealTimeSimulation)
jointNameToId = {}
numJoints = p.getNumJoints(kukaId)
joints = ['lbr_iiwa_joint_1', 'lbr_iiwa_joint_2', 'lbr_iiwa_joint_3', 'lbr_iiwa_joint_4',
'lbr_iiwa_joint_5', 'lbr_iiwa_joint_6', 'lbr_iiwa_joint_7']
for i in range(numJoints):
jointInfo = p.getJointInfo(kukaId, i)
# print jointInfo
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
for i in range(numJoints):
p.resetJointState(kukaId, jointNameToId[joints[i]], start_state[i])
for i in range(len(trajectory)):
next_link_state = None
if i < (len(trajectory) - 1):
self.reset_joint_states_to(kukaId, trajectory[i + 1], joints, jointNameToId)
next_link_state = p.getLinkState(kukaId, 6, computeLinkVelocity=1,
computeForwardKinematics=1)
self.reset_joint_states_to(kukaId, trajectory[i], joints, jointNameToId)
current_link_state = p.getLinkState(kukaId, 6, computeLinkVelocity=1,
computeForwardKinematics=1)
time.sleep(0.5)
closest_points = p.getClosestPoints(kukaId, wall, linkIndexA=6, distance=0.10)
for j in range(len(closest_points)):
if len(closest_points):
if closest_points[j][8] < 0:
print("-------------closest points -----------------")
print("link A index: ", closest_points[j][3])
print("link B index: ", closest_points[j][4])
print("point on A: ", closest_points[j][5])
print("point on B: ", closest_points[j][6])
print("contact normal on B: ", closest_points[j][7])
print("contact distance: ", closest_points[j][8])
print("-------------************ -----------------")
if next_link_state is not None:
cast_points = p.getConvexSweepClosestPoints(kukaId, wall, distance=0.10, linkIndexA=6,
bodyAfromPosition=current_link_state[0],
bodyAtoPosition=next_link_state[0],
bodyAfromOrientation=current_link_state[1],
bodyAtoOrientation=next_link_state[1]
)
for k in range(len(cast_points)):
if len(cast_points):
if cast_points[k][9] < 0:
print("-------------cast_points -----------------")
print("link A index: ", cast_points[k][3])
print("link B index: ", cast_points[k][4])
print("point on A(t): ", cast_points[k][5])
print("point on A(t+1): ", cast_points[k][6])
print("point on B: ", cast_points[k][7])
print("contact normal on B: ", cast_points[k][8])
print("contact distance: ", cast_points[k][9])
print("contact fraction: ", cast_points[k][10])
print("-------------************ -----------------")
self.assertEquals(np.isclose(cast_points[k][10], 0.52, atol=0.01), True)
self.assertEquals(np.isclose(cast_points[k][9], -0.07, atol=0.01), True)
print("------------------------------ testing kukka arm done------------------------------")
def test_box(self):
print("------------------------------ testing boxes ------------------------------")
start = [0.28, 1.0, 0.2]
start1 = [-0.28, 1.05, 0.2]
end = [0.28, 2.0, 0.2]
trajectory = [[0.28, 1.0, 0.3], [0.28, 1.8, 0.3], ]
trajectory1 = [[-0.28, 0.7, 0.3], [-0.28, 1.8, 0.3], ]
box = self.create_constraint(shape=p.GEOM_BOX, size=[0.05, 0.35, 0.15], position=start, mass=1)
box1 = self.create_constraint(shape=p.GEOM_BOX, size=[0.05, 0.35, 0.15], position=start1,
orientation=[0.8, 1.8, 0.2, 1], mass=1)
wall = self.create_constraint(shape=p.GEOM_BOX, size=[0.5, 0.01, 0.5], position=[0, 1.4, 0], mass=1)
print ("----------------------------box 1------------------------------------")
for i in range(len(trajectory)):
p.resetBasePositionAndOrientation(box, trajectory[i], [0.0, 0.0, 0.0, 1])
time.sleep(2)
closet_points = p.getClosestPoints(box, wall, distance=0.10)
for j in range(len(closet_points)):
print("-------------closest points -----------------")
print("link A index: ", closet_points[j][3])
print("link B index: ", closet_points[j][4])
print("point on A: ", closet_points[j][5])
print("point on B: ", closet_points[j][6])
print("contact normal on B: ", closet_points[j][7])
print("contact distance: ", closet_points[j][8])
print("-------------************ -----------------")
if i < (len(trajectory) - 1):
cast_points = p.getConvexSweepClosestPoints(box, wall, distance=0.10,
bodyAfromPosition=trajectory[i],
bodyAtoPosition=trajectory[i + 1],
bodyAfromOrientation=[0.0, 0.0, 0.0, 1],
bodyAtoOrientation=[0.0, 0.0, 0.0, 1]
)
for k in range(len(cast_points)):
print("-------------cast_points -----------------")
print("link A index: ", cast_points[k][3])
print("link B index: ", cast_points[k][4])
print("point on A(t): ", cast_points[k][5])
print("point on A(t+1): ", cast_points[k][6])
print("point on B: ", cast_points[k][7])
print("contact normal on B: ", cast_points[k][8])
print("contact distance: ", cast_points[k][9])
print("contact fraction: ", cast_points[k][10])
print("-------------************ -----------------")
self.assertEquals(np.isclose(cast_points[k][10], 0.5, atol=0.01), True)
print ("----------------------------box 2------------------------------------")
for i in range(len(trajectory1)):
print ("------------------trajectory[" + str(i) + "]: ", trajectory1[i])
p.resetBasePositionAndOrientation(box1, trajectory1[i], [0.8, 1.8, 0.2, 1])
time.sleep(0.5)
closet_points = p.getClosestPoints(box1, wall, distance=0.10)
for j in range(len(closet_points)):
print("-------------closest points -----------------")
print("link A index: ", closet_points[j][3])
print("link B index: ", closet_points[j][4])
print("point on A: ", closet_points[j][5])
print("point on B: ", closet_points[j][6])
print("contact normal on B: ", closet_points[j][7])
print("contact distance: ", closet_points[j][8])
print("-------------************ -----------------")
if i < (len(trajectory1) - 1):
cast_points = p.getConvexSweepClosestPoints(box1, wall, distance=0.10,
bodyAfromPosition=trajectory1[i],
bodyAtoPosition=trajectory1[i + 1],
bodyAfromOrientation=[0.8, 1.8, 0.2, 1],
bodyAtoOrientation=[0.8, 1.8, 0.2, 1]
)
for k in range(len(cast_points)):
print("-------------cast_points -----------------")
print("link A index: ", cast_points[k][3])
print("link B index: ", cast_points[k][4])
print("point on A(t): ", cast_points[k][5])
print("point on A(t+1): ", cast_points[k][6])
print("point on B: ", cast_points[k][7])
print("contact normal on B: ", cast_points[k][8])
print("contact distance: ", cast_points[k][9])
print("contact fraction: ", cast_points[k][10])
print("-------------************ -----------------")
self.assertEquals(np.isclose(cast_points[k][10], 0.63, atol=0.01), True)
print("------------------------------ testing boxes done------------------------------")
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