def gripper_from_proposal(grasp_point_list, grasp_normal_list):
'''Return gripper pose from sampled grasp points (in sampling_base frame)'''
dist_grasp = np.linalg.norm(grasp_point_list[0] - grasp_point_list[1])
if dist_grasp < 0.01:
print(
'[Grasping] Distance between grasp is small (<0.01). Something is wrong.'
)
pose_list = []
for point, normal in zip(grasp_point_list, grasp_normal_list):
y_vec = normal
if normal[0] == 0 and normal[1] == 0:
x_vec = np.array([1, 0, 0])
else:
x_vec = np.array([0, 0, -1])
z_vec = np.cross(x_vec, y_vec)
x_vec = x_vec / np.linalg.norm(x_vec)
y_vec = y_vec / np.linalg.norm(y_vec)
z_vec = z_vec / np.linalg.norm(z_vec)
# Normalized object frame
hand_orient_norm = np.vstack((x_vec, y_vec, z_vec))
hand_orient_norm = hand_orient_norm.transpose()
quat = roshelper.mat2quat(hand_orient_norm)
#define hand pose
pose_list.append(roshelper.list2pose_stamped(list(point) + list(quat)))
return pose_list, dist_grasp #[left, right]
def eliminate_dual_placements(self):
for place_id, placement in enumerate(self.stable_placement_list):
for point_id, point in enumerate(self.projected_points):
if np.linalg.norm(point - self.projected_points[place_id]) < 0.001 and self.stable_placement_list[place_id] and self.stable_placement_list[point_id] and point_id != place_id:
self.stable_placement_list[place_id] = False
self.stable_placement_dict = {}
self.stable_placement_dict['face'] = []
self.stable_placement_dict['id'] = []
self.stable_placement_dict['vector'] = []
self.stable_placement_dict['T'] = []
self.stable_placement_dict['T_rot'] = []
self.stable_placement_dict['mesh'] = []
self.stable_placement_dict['convex_face_planar'] = []
self.stable_placement_dict['convex_face_3d'] = []
self.stable_placement_dict['normal_face_3d'] = []
self.stable_placement_dict['convex_face_stable_config'] = []
self.stable_placement_dict['normal_stable_config'] = []
self.stable_placement_dict['neighboors'] = []
self.stable_placement_dict['common_points'] = []
self.stable_placement_dict['polygon'] = []
self.stable_placement_dict['sides'] = []
place_id = 0
vector_base = np.array([0,0,-1])
for counter, placement in enumerate(self.stable_placement_list):
if placement:
face = counter
id = place_id
vector = self.projected_points[counter] - self.centroid
T_rot = helper.rotation_matrix(vector_base, vector).transpose()
trans = tf.transformations.translation_from_matrix(T_rot)
quat = tf.transformations.quaternion_from_matrix(T_rot)
pose = roshelper.list2pose_stamped(np.concatenate((trans, quat)))
mesh_rotated, trimesh_rotated = self.transform_object(pose)
trans[2] -= mesh_rotated.min_[2]
T = deepcopy(T_rot)
T[2,3] = trans[2]
pose_object = roshelper.list2pose_stamped(np.concatenate((trans, quat)))
mesh, trimesh_transformed = self.transform_object(pose_object)
convex_face_planar, convex_face_3d, normal_hull_3d = \
self.compute_convex_hulls(self.projected_points[counter],
self.projected_points[counter] - self.centroid)
polygon = self.define_polygon(convex_face_planar)
side_list = []
for counter, vertice in enumerate(polygon.vertices):
if counter<len(polygon.vertices)-1:
side_list.append([vertice, polygon.vertices[counter+1]])
else:
side_list.append([vertice, polygon.vertices[0]])
perimeter_list = []
for side in side_list:
side_dict = {}
side_dict['center_b'] = (side[1] + side[0]) / 2
side_dict['center_3d_b'] = np.array([side_dict['center_b'][0], side_dict['center_b'][1], 0])
side_dict['length'] = np.linalg.norm(side[1] - side[0])
axis = (side[1] - side[0]) / side_dict['length']
side_dict['contact_x_b'] = np.array([axis[0], axis[1], 0])
side_dict['contact_z_b'] = np.array([0,0,-1])
side_dict['contact_y_b'] = np.cross(side_dict['contact_z_b'], side_dict['contact_x_b'])
if np.dot(side_dict['center_3d_b'], side_dict['contact_y_b']) < 0:
side_dict['contact_x_b'] = -np.array([axis[0], axis[1], 0])
side_dict['contact_y_b'] = np.cross(side_dict['contact_z_b'], side_dict['contact_x_b'])
side_dict['edge_frame_b'] = [side_dict['contact_x_b'],
side_dict['contact_y_b'],
side_dict['contact_z_b']]
x_vec = (side_dict['center_b'] - polygon.obj_centroid) / (np.linalg.norm(side_dict['center_b'] - polygon.obj_centroid))
z_vec = np.array([0,0,1])
y_vec = np.cross(z_vec, x_vec)
side_dict['contact_frame_b'] = [x_vec, y_vec, z_vec]
side_dict['contact_angle'] = float(helper.unwrap([helper.angle_2d(x_vec)- np.pi], 0, 2*np.pi))
side_dict['Ccb'] = helper.C3_2d(side_dict['contact_angle'])
vertices_rotated_list = []
for vertex in list(polygon.vertices):
vertices_rotated_list.append(np.matmul(side_dict['Ccb'].transpose(), vertex))
polygon_rotated = self.define_polygon(np.array(vertices_rotated_list))
side_dict['polygon_rotated'] = polygon_rotated
perimeter_list.append(side_dict)
perimeter_list.sort(key=util.sort_angle)
self.stable_placement_dict['face'].append(face)
self.stable_placement_dict['id'].append(id)
self.stable_placement_dict['vector'].append(vector)
self.stable_placement_dict['T_rot'].append(T_rot)
self.stable_placement_dict['T'].append(T)
self.stable_placement_dict['mesh'].append(mesh)
self.stable_placement_dict['convex_face_planar'].append(convex_face_planar)
self.stable_placement_dict['convex_face_3d'].append(convex_face_3d)
self.stable_placement_dict['normal_face_3d'].append(normal_hull_3d)
self.stable_placement_dict['polygon'].append(polygon)
self.stable_placement_dict['sides'].append(perimeter_list)
place_id += 1
#~ find neighboors of each stable placements (levering primitive)
for convex_face_id, convex_face in enumerate(self.stable_placement_dict['convex_face_3d']):
neighboor_list, common_points_list = self.find_neighboor_faces(convex_face_id, self.stable_placement_dict['convex_face_3d'])
self.stable_placement_dict['neighboors'].append(neighboor_list)
self.stable_placement_dict['common_points'].append(common_points_list)
T = self.stable_placement_dict['T'][convex_face_id]
T_rot = self.stable_placement_dict['T_rot'][convex_face_id]
face_rotated_list = []
normal_rotated_list = []
for convex_face_id_2, convex_face_2 in enumerate(self.stable_placement_dict['convex_face_3d']):
face_stable_config = util.transform_point_list(convex_face_2, T)
normal_stable_config = self.stable_placement_dict['vector'][convex_face_id_2]
normal_new = helper.vector_transform(T_rot, normal_stable_config / np.linalg.norm(normal_stable_config))
face_rotated_list.append(face_stable_config)
normal_rotated_list.append(normal_new)
self.stable_placement_dict['convex_face_stable_config'].append(face_rotated_list)
self.stable_placement_dict['normal_stable_config'].append(normal_rotated_list)
# frank hack: reorder placements such that [0] element is identity orientation
for counter, T in enumerate(self.stable_placement_dict['T']):
# frank hack: make sure that nominal placement has identity orientation
if T[0, 0] == 1 and T[1, 1] == 1 and T[2, 2] == 1:
nominal_placement = counter
break
for key in self.stable_placement_dict.keys():
first_placement_tmp = self.stable_placement_dict[key][0]
self.stable_placement_dict[key][0] = self.stable_placement_dict[key][nominal_placement]
self.stable_placement_dict[key][nominal_placement] = first_placement_tmp
def visualize(self, flag="collisions"):
if flag == "placements" or flag == "all" or flag == "collisions":
for i in range(len(self.samples_dict['object_pose'])):
visualize_helper.delete_markers()
roshelper.handle_block_pose(
self.samples_dict['object_pose'][i][0], self.sampler.br,
'proposals_base', 'object')
roshelper.handle_block_pose(self.sampler.base_initial,
self.sampler.br, 'yumi_body',
'proposals_base')
visualize_helper.visualize_grasps(
self.sampler.br,
self.samples_dict['points'][i],
pose=self.samples_dict['base_pose'][i][0],
id=0)
for x in range(10):
print('placement: ', i)
visualize_helper.visualize_object(
self.samples_dict['object_pose'][i][0],
filepath="package://" +
self.sampler.object.object_name,
frame_id="proposals_base",
name="/object_placement",
color=[1, 0.5, 0, 1])
rospy.sleep(.1)
if flag == "collisions":
for grasp_id in range(
len(self.samples_dict['gripper_poses'][i])):
gripper_pose_left = self.samples_dict['gripper_poses'][
i][grasp_id][0]
gripper_pose_right = self.samples_dict[
'gripper_poses'][i][grasp_id][1]
wrist_to_tip = roshelper.list2pose_stamped(
[0.0, 0.071399, -0.14344421, 0.0, 0.0, 0.0, 1.0],
'')
wrist_left = roshelper.convert_reference_frame(
wrist_to_tip, roshelper.unit_pose(),
gripper_pose_left, "proposals_base")
wrist_right = roshelper.convert_reference_frame(
wrist_to_tip, roshelper.unit_pose(),
gripper_pose_right, "proposals_base")
collisions = self.samples_dict['collisions'][i][
grasp_id]
rospy.sleep(.2)
if all(collisions[x] == 0
for x in range(len(collisions))):
visualize_helper.visualize_object(
self.samples_dict['object_pose'][i][0],
filepath=
"package://config/descriptions/meshes/objects/realsense_box_experiments.stl",
frame_id="proposals_base",
name="/object_placement",
color=[1, 0.5, 0, 1])
visualize_helper.visualize_object(
wrist_left,
filepath=
"package://config/descriptions/meshes/mpalm/mpalms_all_coarse.stl",
frame_id="proposals_base",
name="/gripper_left")
visualize_helper.visualize_object(
wrist_right,
filepath=
"package://config/descriptions/meshes/mpalm/mpalms_all_coarse.stl",
frame_id="proposals_base",
name="/gripper_right")
rospy.sleep(.2)
elif flag == "mesh" or flag == "all":
for id in range(6):
self.visualize_placement(id, self.samples_dict['points'])
self.visualize_placement(id,
self.collision_free_samples['points'])
roshelper.handle_block_pose(self.sampler.object_pose_initial,
self.sampler.br, 'proposals_base',
'stable_placement')
rospy.sleep(.05)
def check_collisions(self, gripper_pose_list):
wrist_to_tip = roshelper.list2pose_stamped(
[0.0, 0.071399, -0.14344421, 0.0, 0.0, 0.0, 1.0], '')
wrist_left_proposal = roshelper.convert_reference_frame(
wrist_to_tip, roshelper.unit_pose(), gripper_pose_list[0],
"proposals_base")
wrist_right_proposal = roshelper.convert_reference_frame(
wrist_to_tip, roshelper.unit_pose(), gripper_pose_list[1],
"proposals_base")
wrist_left_world = roshelper.convert_reference_frame(
wrist_left_proposal, roshelper.unit_pose(),
self.sampler.base_initial, "yumi_body")
wrist_right_world = roshelper.convert_reference_frame(
wrist_right_proposal, roshelper.unit_pose(),
self.sampler.base_initial, "yumi_body")
self.sampler.gripper_left.setCollisionPose(
self.sampler.gripper_left.collision_object, wrist_left_world)
self.sampler.gripper_right.setCollisionPose(
self.sampler.gripper_right.collision_object, wrist_right_world)
collision_list = []
collision_list.append(
objects.is_collision(self.sampler.gripper_left.collision_object,
self.sampler.table.collision_object))
collision_list.append(
objects.is_collision(self.sampler.gripper_right.collision_object,
self.sampler.table.collision_object))
# visualize_helper.visualize_object(wrist_left_world,
# filepath="package://config/descriptions/meshes/mpalm/mpalms_all_coarse.stl",
# frame_id="yumi_body", name="/gripper_left")
# visualize_helper.visualize_object(wrist_right_world,
# filepath="package://config/descriptions/meshes/mpalm/mpalms_all_coarse.stl",
# frame_id="yumi_body", name="/gripper_right")
# visualize_helper.visualize_object(roshelper.list2pose_stamped(self.sampler.table_pose, "yumi_body"),
# filepath="package://config/descriptions/meshes/yumi/table_top_collision.stl",
# name="/table_pose2", color=(0., 0., 1., 1.),
# frame_id="/yumi_body")
# roshelper.handle_block_pose(gripper_pose_list[0], self.sampler.planner.br, "proposals_base", "gripper_left")
# roshelper.handle_block_pose(gripper_pose_list[1], self.sampler.planner.br, "proposals_base", "gripper_right")
# roshelper.handle_block_pose(wrist_left, self.sampler.planner.br, "proposals_base", "wrist_left")
# print ('collision_list: ', collision_list)
# rospy.sleep(.4)
# wrist_left_world.pose.position.y += 0.05
# wrist_right_world.pose.position.y += 0.05
# self.sampler.planner.gripper_left.setCollisionPose(self.sampler.planner.gripper_left.collision_object,
# wrist_left_world,
# self.sampler.planner.listener,
# type="PoseStamped",
# frame_id="yumi_body")
# self.sampler.planner.gripper_right.setCollisionPose(self.sampler.planner.gripper_right.collision_object,
# wrist_right_world,
# self.sampler.planner.listener,
# type="PoseStamped",
# frame_id="yumi_body")
# collision_list = []
# collision_list.append(objects.is_collision(self.sampler.planner.gripper_left.collision_object,
# self.sampler.planner.table.collision_object))
# collision_list.append(objects.is_collision(self.sampler.planner.gripper_right.collision_object,
# self.sampler.planner.table.collision_object))
# visualize_helper.visualize_object(wrist_left_world,
# filepath="package://config/descriptions/meshes/mpalm/mpalms_all_coarse.stl",
# frame_id="yumi_body", name="/gripper_left")
# visualize_helper.visualize_object(wrist_right_world,
# filepath="package://config/descriptions/meshes/mpalm/mpalms_all_coarse.stl",
# frame_id="yumi_body", name="/gripper_right")
# visualize_helper.visualize_object(roshelper.list2pose_stamped(self.sampler.table_pose, "yumi_body"),
# filepath="package://config/descriptions/meshes/yumi/table_top_collision.stl",
# name="/table_pose2", color=(0., 0., 1., 1.),
# frame_id="/yumi_body")
# print (collision_list)
return collision_list
#initialize ROS
rospy.init_node('planning_test')
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
#1. build environment
# object_name = "cylinder_simplify.stl"
object_name = "realsense_box_experiments.stl"
gripper_name="mpalms_all_coarse.stl"
table_name="table_top_collision.stl"
_object = Object(mesh_name="config/descriptions/meshes/objects/" + object_name)
table = CollisionBody(mesh_name="config/descriptions/meshes/table/" + table_name)
trans, quat = listener.lookupTransform("yumi_body", "table_top", rospy.Time(0))
table_pose = trans + quat
table.setCollisionPose(table.collision_object, roshelper.list2pose_stamped(table_pose, "yumi_body"))
gripper_left = CollisionBody(mesh_name="config/descriptions/meshes/mpalm/" + gripper_name)
gripper_right = CollisionBody(mesh_name="config/descriptions/meshes/mpalm/" + gripper_name)
# q0 = roshelper.list2pose_stamped([0.4500000000000001, -0.040000000000000056, 0.07145000425107054, 0.4999999999999997, 0.4999999999999997, 0.4999999999999997, 0.5000000000000003])
q0 = roshelper.list2pose_stamped([0.45,0,0,0,0,0,1])
#3. sample object
sampler = sampling.Sampling(q0, _object, table, gripper_left, gripper_right, listener, br)
#4. grasp samplers
# grasp_samples = grasp_sampling.GraspSampling(sampler,
# num_samples=3,
# is_visualize=True)
#5. lever samples
lever_samples = lever_sampling.LeverSampling(sampler)
node_sequence, intersection_dict_lever = sampling.search_placement_graph(grasp_samples=None,
lever_samples=lever_samples,
