def transform_object(self, pose):
T = roshelper.matrix_from_pose(pose)
euler = tf.transformations.euler_from_matrix(T.transpose(), 'rxyz')
#mesh library performs rotatin with DCM rather than R matrix
trans = tf.transformations.translation_from_matrix(T)
mesh_new = deepcopy(self.mesh)
mesh_new.rotate([.5, 0.0,0.0], euler[0])
mesh_new.rotate([0.0, .5 ,0.0], euler[1])
mesh_new.rotate([0.0, 0.0, .5], euler[2])
mesh_new.x += trans[0]
mesh_new.y += trans[1]
mesh_new.z += trans[2]
vertices = list(np.array(deepcopy(self.trimesh.vertices)))
vertices_transformed = []
for vertex in vertices:
point_transformed = helper.vector_transform(T, vertex)
vertices_transformed.append(point_transformed)
return mesh_new, vertices_transformed
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 initialize_stable_placements(self):
"""convert samples taken in nominal pose to all stable placements poses"""
#samples are expressed in proposals_base
base_pose = self.sampler.base_initial
# angle_list = list(np.linspace(np.pi / 4, np.pi / 4 + 2* np.pi, 4))
angle_list = list(np.linspace(0, 2 * np.pi, 12))
for placement_id in range(
len(self.sampler.object.stable_placement_dict['convex_face_3d']
)):
print('generating grasps for placement id: ', placement_id)
grasp_id = 0
#1. find transformation to stable placement from base (transformation includes rot + trans)
T = self.sampler.object.stable_placement_dict['T'][placement_id]
object_pose = roshelper.pose_from_matrix(T, frame_id="yumi_body")
#2. rotate all grasp points, normals, grasp_poses
grasp_id_list_new = []
grasp_point_list_new = []
grasp_normal_list_new = []
grasp_gripper_pose_list_new = []
grasp_width_list_new = []
grasp_T_list_new = []
grasp_T_pose_list_new = []
grasp_base_pose_list_new = []
grasp_object_pose_list_new = []
grasp_collisions_list = []
grasp_sample_id_list = []
#iterate over sample points
for point_list, normal_list, sample_id in zip(
self.dict_grasp['points'], self.dict_grasp['face_normals'],
self.dict_grasp['sample_ids']):
point_list_new = []
normal_list_new = []
gripper_pose_list_new = []
T_list_new = []
T_pose_list_new = []
for point, normal in zip(point_list, normal_list):
point_new = helper.vector_transform(T, point)
T_rot = deepcopy(T)
T_rot[:, 3] = [0, 0, 0, 1]
normal_new = helper.vector_transform(T_rot, normal)
point_list_new.append(point_new)
normal_list_new.append(normal_new)
pose_list, grasp_width, T_gripper_body_list, is_flipped = grasp_from_proposal(
point_list, normal_list, T,
self.sampler.sampling_pose_initial)
if is_flipped:
tmp = normal_list_new[0]
normal_list_new[0] = normal_list_new[1]
normal_list_new[1] = tmp
T_list_new.append([T, T])
T_pose_list_new.append(T_gripper_body_list)
for angle in angle_list:
#publish gripper reference frame
# roshelper.handle_block_pose(pose_list[0], self.sampler.planner.br, 'proposals_base', 'gripper_left_blue')
# roshelper.handle_block_pose(pose_list[1], self.sampler.planner.br, 'proposals_base', 'gripper_right_blue')
#i. convert to gripper frame
gripper_left_gripper_left = roshelper.convert_reference_frame(
pose_list[0],
pose_list[0],
roshelper.unit_pose(),
frame_id="gripper_left")
gripper_right_gripper_left = roshelper.convert_reference_frame(
pose_list[1],
pose_list[0],
roshelper.unit_pose(),
frame_id="gripper_left")
#ii. rotate grippers by angle about y axis of gripper left frame
T_pose_rotation_gripper_left = tfm.euler_matrix(
0, angle, 0, 'rxyz')
pose_rotation_gripper_left = roshelper.pose_from_matrix(
T_pose_rotation_gripper_left, 'gripper_left')
gripper_left_rotated_gripper_left = roshelper.transform_pose(
gripper_left_gripper_left, pose_rotation_gripper_left)
gripper_right_rotated_gripper_left = roshelper.transform_pose(
gripper_right_gripper_left, pose_rotation_gripper_left)
#iii. convert back to proposals_base
gripper_left_rotated_proposals_base = roshelper.convert_reference_frame(
gripper_left_rotated_gripper_left,
roshelper.unit_pose(),
pose_list[0],
frame_id="proposals_base")
gripper_right_rotated_proposals_base = roshelper.convert_reference_frame(
gripper_right_rotated_gripper_left,
roshelper.unit_pose(),
pose_list[0],
frame_id="proposals_base")
#3. check collisions between grippers and table
gripper_pose_list_new = [
gripper_left_rotated_proposals_base,
gripper_right_rotated_proposals_base
]
collision_list = self.check_collisions(
gripper_pose_list_new)
grasp_id_list_new.append(placement_id)
grasp_point_list_new.append(np.array(point_list_new))
grasp_normal_list_new.append(np.array(normal_list_new))
grasp_gripper_pose_list_new.append(
np.array(gripper_pose_list_new))
grasp_width_list_new.append(grasp_width)
grasp_T_list_new.append(np.array(T_list_new))
grasp_T_pose_list_new.append(np.array(T_pose_list_new))
grasp_base_pose_list_new.append(base_pose)
grasp_object_pose_list_new.append(object_pose)
grasp_collisions_list.append(collision_list)
grasp_sample_id_list.append(grasp_id)
grasp_id += 1
self.samples_dict['placement_id'].append(grasp_id_list_new)
self.samples_dict['points'].append(grasp_point_list_new)
self.samples_dict['face_normals'].append(grasp_normal_list_new)
self.samples_dict['gripper_poses'].append(
grasp_gripper_pose_list_new)
self.samples_dict['grasp_width'].append(grasp_width_list_new)
self.samples_dict['T'].append(grasp_T_list_new)
self.samples_dict['T_pose'].append(grasp_T_pose_list_new)
self.samples_dict['base_pose'].append(grasp_base_pose_list_new)
self.samples_dict['object_pose'].append(grasp_object_pose_list_new)
self.samples_dict['collisions'].append(grasp_collisions_list)
self.samples_dict['sample_ids'].append(grasp_sample_id_list)