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,