コード例 #1
0
ファイル: robot.py プロジェクト: skumra/romannet
 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 = []
コード例 #2
0
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()
コード例 #3
0
 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]]])
コード例 #4
0
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)
コード例 #5
0
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
コード例 #6
0
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()