def add_objects(self): # Add each object to robot workspace at x,y location and orientation (random or pre-loaded) logging.info('Adding objects to the scene..') self.object_handles = [] self.num_obj_clear = 0 for object_idx in range(len(self.obj_mesh_ind)): curr_mesh_file = os.path.join( self.obj_mesh_dir, self.mesh_list[self.obj_mesh_ind[object_idx]]) if self.test_preset_cases: curr_mesh_file = self.test_obj_mesh_files[object_idx] curr_shape_name = 'shape_%02d' % object_idx drop_x = (self.workspace_limits[0][1] - self.workspace_limits[0][0] - 0.2) * np.random.random_sample() + \ self.workspace_limits[0][0] + 0.1 drop_y = (self.workspace_limits[1][1] - self.workspace_limits[1][0] - 0.2) * np.random.random_sample() + \ self.workspace_limits[1][0] + 0.1 object_position = [drop_x, drop_y, 0.15] object_orientation = [ 2 * np.pi * np.random.random_sample(), 2 * np.pi * np.random.random_sample(), 2 * np.pi * np.random.random_sample() ] if self.test_preset_cases: object_position = [ self.test_obj_positions[object_idx][0], self.test_obj_positions[object_idx][1], self.test_obj_positions[object_idx][2] ] object_orientation = [ self.test_obj_orientations[object_idx][0], self.test_obj_orientations[object_idx][1], self.test_obj_orientations[object_idx][2] ] object_color = [ self.obj_mesh_color[object_idx][0], self.obj_mesh_color[object_idx][1], self.obj_mesh_color[object_idx][2] ] ret_resp, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction( self.sim_client, 'remoteApiCommandServer', vrep.sim_scripttype_childscript, 'importShape', [0, 0, 255, 0], object_position + object_orientation + object_color, [curr_mesh_file, curr_shape_name], bytearray(), vrep.simx_opmode_blocking) if ret_resp == 8: logging.error( 'Failed to add new objects to simulation. Restarting..') self.setup_sim() else: curr_shape_handle = ret_ints[0] self.object_handles.append(curr_shape_handle) if not self.test_preset_cases: time.sleep(0.5) self.prev_obj_positions = [] self.obj_positions = []
def grasp_centroid_data(): wd = '/home/lou00015/data/gsp/' cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) eid = 0 nb_grasp = 25 if cid != -1: pos = [0, 0, 0.15] while True: vrep.simxStartSimulation(cid, vrep.simx_opmode_blocking) panda = Robot(cid) obj_name, obj_hdl = add_object(cid, 'imported_part_0', pos) time.sleep(1.0) cloud = panda.get_pointcloud() centroid = np.average(cloud, axis=0) print('centroid: ', centroid) if len(cloud) == 0: print('no cloud found') continue elif centroid[2] > 0.045: print('perception error') continue np.save(wd + 'cloud_' + str(eid) + '.npy', cloud) # save point cloud cloud = np.delete(cloud, np.where(cloud[:,2]<0.015), axis=0) pose_set, pt_set = grasp_pose_generation(45, cloud, nb_grasp) pose = pose_set[10] pt = centroid emptyBuff = bytearray() landing_mtx = [pose[0][0],pose[0][1],pose[0][2],pt[0], pose[1][0],pose[1][1], pose[1][2],pt[1], pose[2][0],pose[2][1],pose[2][2],pt[2]] np.save(wd + 'action_' + str(eid) + '.npy', landing_mtx) # save action vrep.simxCallScriptFunction(cid, 'landing', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) ending_mtx = [pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0], pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1], pose[2][2], pt[2]+0.15] vrep.simxCallScriptFunction(cid, 'ending', vrep.sim_scripttype_childscript, 'setending', [], ending_mtx, [], emptyBuff, vrep.simx_opmode_blocking) time.sleep(1.0) print('executing experiment %d: ' % (eid)) print('at: ', pt) vrep.simxCallScriptFunction(cid, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [], [], [], emptyBuff, vrep.simx_opmode_blocking) while True: res, finish = vrep.simxGetIntegerSignal(cid, "finish", vrep.simx_opmode_oneshot_wait) if finish == 18: res, end_pos = vrep.simxGetObjectPosition(cid, obj_hdl, -1, vrep.simx_opmode_blocking) break if end_pos[2]>0.05: label = 1 else: label = 0 print(label) f = open(wd + 'label.txt', 'a+') f.write(str(label)) f.close() eid += 1 else: print('Failed to connect to simulation (V-REP remote API server). Exiting.') exit()
def __init__(self, clientID): self.cid = clientID self.dummybyte = bytearray() sim_ret, cam_depth_handle = vrep.simxGetObjectHandle( self.cid, 'kinect_depth', vrep.simx_opmode_blocking) self.depth_handle = cam_depth_handle sim_ret, cam_rgb_handle = vrep.simxGetObjectHandle( self.cid, 'kinect_rgb', vrep.simx_opmode_blocking) self.rgb_handle = cam_rgb_handle res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( self.cid, 'kinect_depth', vrep.sim_scripttype_childscript, 'getMatrix', [], [], [], self.dummybyte, vrep.simx_opmode_blocking) self.depth_m = np.asarray( [[retFloats[0], retFloats[1], retFloats[2], retFloats[3]], [retFloats[4], retFloats[5], retFloats[6], retFloats[7]], [retFloats[8], retFloats[9], retFloats[10], retFloats[11]]])
def simulation_experiment_challenging(): cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0, 1]]) object_inventory = [ 'imported_part_0', 'imported_part_2', 'imported_part_3', 'imported_part_6', 'imported_part_7', 'imported_part_8' ] cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) root = 'C:/Users/louxi/Desktop/icra2021/zb_2/' p = Perception() # perception module p g = Manipulation() positions = [[0.20, 0, 0.15], [-0.20, 0, 0.15], [0, -0.20, 0.15], [0, 0.20, 0.15]] env_cloud = np.load(root + 'env.npy') if cid != -1: i = 0 planning = 0 vrep.simxStartSimulation(cid, operationMode=vrep.simx_opmode_blocking) panda = Robot(cid) for i in range(10): add_object(cid, 'imported_part_' + str(i), random.choice(positions)) while i < 51: print('experiment number: ', i) rgb_raw, depth_raw = panda.get_rgbd_image() # Perception m_list = p.segmentation(rgb_raw, depth_raw) # segment scene target_object = random.choice(object_inventory) print(target_object) anchor = np.asarray( Image.open( os.path.join( 'C:/Users\louxi\Desktop\icra2021\siamese_data', target_object, 'rgb_image_' + str(random.randint(0, 499)) + '.png'))) anchor = center_crop_150(anchor) object_mask = p.classification(m_list, rgb_raw, anchor) # load anchor here # object_depth[np.where(object_mask < 0.5)] = 0 rgb_raw[np.where(object_mask < 0.5)] = 0 object_depth = np.copy(depth_raw) object_depth[np.where(object_mask < 0.5)] = 0 object_cloud = get_pointcloud(object_depth, cam_intrinsics, panda.depth_m) ws = [-0.25, 0.25, -0.25, 0.25, 0.001, 0.10] segmented_cloud = np.array([ pts for pts in object_cloud if pts[0] < ws[1] and pts[0] > ws[0] and pts[1] < ws[3] and pts[1] > ws[2] and pts[2] < ws[5] and pts[2] > ws[4] ]) if len(segmented_cloud) == 0: continue # pptk.viewer(segmented_cloud) # # fig = plt.figure(0) # sub_1 = fig.add_subplot(1, 2, 1) # sub_1.imshow(anchor) # sub_2 = fig.add_subplot(1, 2, 2) # sub_2.imshow(rgb_raw) # plt.show() initial_poses, dummy_points = grasp_pose_generation( 90, segmented_cloud, 200) # collision_free_poses = g.carp(env_cloud, np.average(segmented_cloud, axis=0), initial_poses) landing_mtx = g.grasping(segmented_cloud, np.average(segmented_cloud, axis=0), initial_poses) ending_mtx = np.copy(landing_mtx) ending_mtx[-1] = ending_mtx[-1] + 0.15 emptyBuff = bytearray() res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing0', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing1', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing2', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing3', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing4', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( cid, 'landing5', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) print(retFloats) input('s') vrep.simxCallScriptFunction(cid, 'landing', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) vrep.simxCallScriptFunction(cid, 'ending', vrep.sim_scripttype_childscript, 'setending', [], ending_mtx, [], emptyBuff, vrep.simx_opmode_blocking) time.sleep(0.5) vrep.simxCallScriptFunction(cid, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [], [], [], emptyBuff, vrep.simx_opmode_blocking) while True: res, finish = vrep.simxGetIntegerSignal( cid, "finish", vrep.simx_opmode_blocking) if finish == 18: res, result = vrep.simxGetIntegerSignal( cid, "collision", vrep.simx_opmode_blocking) if result == 1: planning = planning + 1 print('result is %d. start next experiment' % result) break i = i + 1 # vrep.simxStopSimulation(cid, operationMode=vrep.simx_opmode_blocking) time.sleep(3.0) # input('s') print(planning)
def simulation_experiment_standard(): cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240], [0, 0, 1]]) object_inventory = [ 'imported_part_0', 'imported_part_5', 'imported_part_2', 'imported_part_3', 'imported_part_6', 'imported_part_9' ] cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) root = 'C:/Users/louxi/Desktop/icra2021/zb_2/' p = Perception() # perception module p g = Manipulation() # positions = [[0.20, 0, 0.15], [-0.20, 0, 0.15], [0, -0.20, 0.15], [0, 0.20, 0.15]] env_cloud = np.load(root + 'env.npy') emptyBuff = bytearray() if cid != -1: i = 0 planning = 0 vrep.simxStartSimulation(cid, operationMode=vrep.simx_opmode_blocking) panda = Robot(cid) # for i in range(10): # add_object(cid, 'imported_part_' + str(i), random.choice(positions)) # for i in range(10): # add_object(cid, 'imported_part_' + str(i), [random.uniform(-0.10, 0.10), random.uniform(-0.10, 0.10), 0.15]) while True: # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing0', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_0.npy', np.array(retFloats)) # time.sleep(1) # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing1', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_1.npy', np.array(retFloats)) # # time.sleep(1) # # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing2', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_2.npy', np.array(retFloats)) # # time.sleep(1) # # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing3', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_3.npy', np.array(retFloats)) # # time.sleep(1) # # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing4', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_4.npy', np.array(retFloats)) # # time.sleep(1) # # res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(cid, 'landing5', vrep.sim_scripttype_childscript, 'getlanding', [], # [], [], emptyBuff, vrep.simx_opmode_blocking) # print(retFloats) # np.save('pose_5.npy', np.array(retFloats)) # # time.sleep(1) # # input('s') # objects_nb = input('number') objects_nb = '0' rgb_raw, depth_raw = panda.get_rgbd_image() # im = Image.fromarray(rgb_raw) # im.save('imported_part_5.png') # input('save?') # Perception m_list = p.segmentation(rgb_raw, depth_raw) # segment scene anchor = np.asarray( Image.open( 'C:/Users\louxi\Desktop\icra2021\siamese_data\inventory/' + object_inventory[int(objects_nb)] + '.png')) anchor = center_crop_150(anchor) object_mask = p.classification(m_list, rgb_raw, anchor) # load anchor here # object_depth[np.where(object_mask < 0.5)] = 0 rgb_raw[np.where(object_mask < 0.5)] = 0 # plt.imshow(rgb_raw) # plt.show() object_depth = np.copy(depth_raw) object_depth[np.where(object_mask < 0.5)] = 0 object_cloud = get_pointcloud(object_depth, cam_intrinsics, panda.depth_m) ws = [-0.25, 0.25, -0.25, 0.25, 0.001, 0.10] segmented_cloud = np.array([ pts for pts in object_cloud if pts[0] < ws[1] and pts[0] > ws[0] and pts[1] < ws[3] and pts[1] > ws[2] and pts[2] < ws[5] and pts[2] > ws[4] ]) # pptk.viewer(segmented_cloud) recognized = 'yes' if recognized == 'yes': initial_poses, dummy_points = grasp_pose_generation( 60, segmented_cloud, 200) # collision_free_poses = g.carp(env_cloud, np.average(segmented_cloud, axis=0), initial_poses) # print(len(collision_free_poses)) pt = np.average(segmented_cloud, axis=0) print('landing' + str(i)) pose = np.load('pose_' + str(i) + '.npy') i = i + 1 # landing_mtx = [pose[0][0], pose[0][1], pose[0][2], pt[0], # pose[1][0], pose[1][1], pose[1][2], pt[1], # pose[2][0], pose[2][1], pose[2][2], pt[2]] landing_mtx = pose print(landing_mtx) # vis_grasp(pose, pt, segmented_cloud) ending_mtx = np.copy(landing_mtx) ending_mtx[-1] = ending_mtx[-1] + 0.15 # recognized = input('correct?') if recognized == 'yes': vrep.simxCallScriptFunction( cid, 'landing', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) vrep.simxCallScriptFunction( cid, 'ending', vrep.sim_scripttype_childscript, 'setending', [], ending_mtx, [], emptyBuff, vrep.simx_opmode_blocking) time.sleep(0.5) vrep.simxCallScriptFunction( cid, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [], [], [], emptyBuff, vrep.simx_opmode_blocking) while True: res, finish = vrep.simxGetIntegerSignal( cid, "finish", vrep.simx_opmode_blocking) if finish == 18: res, result = vrep.simxGetIntegerSignal( cid, "collision", vrep.simx_opmode_blocking) if result == 1: planning = planning + 1 print('result is %d. start next experiment' % result) break time.sleep(3.0) else: continue
def gsp_test(): wd = '/home/lou00015/data/gsp_test/' model = GSP3d().cuda() model.load_state_dict(torch.load('gsp.pt')) model.eval() cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) eid = 0 nb_grasp = 300 if cid != -1: pos = [0, 0, 0.15] while True: vrep.simxStartSimulation(cid, vrep.simx_opmode_blocking) panda = Robot(cid) obj_name, obj_hdl = add_object(cid, 'imported_part_0', pos) time.sleep(1.0) cloud = panda.get_pointcloud() centroid = np.average(cloud, axis=0) if len(cloud) == 0: print('no cloud found') continue elif centroid[2] > 0.045: print('perception error') continue # np.save(wd + 'cloud_' + str(eid) + '.npy', cloud) # save point cloud cloud = np.delete(cloud, np.where(cloud[:, 2] < 0.015), axis=0) v = voxelize(cloud - centroid, 0.1) pose_set, pt_set = grasp_pose_generation(45, cloud, nb_grasp) x1, x2 = [], [] emptyBuff = bytearray() for i in range(nb_grasp): pose = pose_set[i] pt = pt_set[i] landing_mtx = np.asarray([ pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0], pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1], pose[2][2], pt[2] ]) x1.append(v) x2.append(landing_mtx) x1, x2 = np.stack(x1), np.stack(x2) X1 = torch.tensor(x1.reshape((x2.shape[0], 1, 32, 32, 32)), dtype=torch.float, device=device) X2 = torch.tensor(x2.reshape((x2.shape[0], 12)), dtype=torch.float, device=device) yhat = model(X1, X2) yhat = yhat.detach().cpu().numpy() scores = np.asarray(yhat) scores = scores.reshape((nb_grasp, )) g_index = np.argmax(scores) print('Highest score: {}, the {}th.'.format( str(scores[g_index]), str(g_index))) pose = pose_set[g_index] pt = centroid landing_mtx = np.asarray([ pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0], pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1], pose[2][2], pt[2] ]) vrep.simxCallScriptFunction(cid, 'landing', vrep.sim_scripttype_childscript, 'setlanding', [], landing_mtx, [], emptyBuff, vrep.simx_opmode_blocking) ending_mtx = [ pose[0][0], pose[0][1], pose[0][2], pt[0], pose[1][0], pose[1][1], pose[1][2], pt[1], pose[2][0], pose[2][1], pose[2][2], pt[2] + 0.15 ] vrep.simxCallScriptFunction(cid, 'ending', vrep.sim_scripttype_childscript, 'setending', [], ending_mtx, [], emptyBuff, vrep.simx_opmode_blocking) time.sleep(1.0) print('executing experiment %d: ' % g_index) print('at: ', pt) vrep.simxCallScriptFunction(cid, 'Sphere', vrep.sim_scripttype_childscript, 'grasp', [], [], [], emptyBuff, vrep.simx_opmode_blocking) while True: res, finish = vrep.simxGetIntegerSignal( cid, "finish", vrep.simx_opmode_oneshot_wait) if finish == 18: res, end_pos = vrep.simxGetObjectPosition( cid, obj_hdl, -1, vrep.simx_opmode_blocking) break if end_pos[2] > 0.05: label = 1 else: label = 0 print(label) # f = open(wd + 'label.txt', 'a+') # f.write(str(label)) # f.close() eid += 1 else: print( 'Failed to connect to simulation (V-REP remote API server). Exiting.' ) exit()