def main(): folders = get_folders(path=DATA_PATH) p_mode_velocity_file = 'p_mode_velocities.csv' scene_objects = 'scene_objects.npy' success = 0 for i in range(507, 999): folder = DATA_PATH + 'Trial_%d/' % i with Interface() as I: # LOAD ROBOT robot = I.load_model(ROBOT_URDF_PATH) # LOAD TABLE table = I.load_model(TABLE, Pose(Point(0.4, 0.0, 0.05), Euler(1.57, 0.0, 0.0)), fixed_base=True, scaling=1.0) # TODO set camera I.set_camera_pose(0.6, 155, -50, [0.2, 0.0, 0.5]) scene = np.load(os.path.join(folder, scene_objects))[2:, 0] block = I.load_model(CUBE_URDF, Pose(Point(*scene[0])), fixed_base=False) basket = I.load_model(BASKET_URDF, Pose(Point(*scene[1])), fixed_base=False) I.save_body_conf() if os.path.isfile(os.path.join(folder, p_mode_velocity_file)): velocities = Pd(os.path.join( folder, p_mode_velocity_file)).read().values I.replay_velocities(robot, velocities) if inside_basket(I, block, basket): I.restore_body_conf() time.sleep(1.0) I.replay_velocities(robot, velocities, filename=os.path.join(folder, 'img')) success += 1 if success == 270: break print('Trials : ', folder, ' , Success: ', success)
def main(): for i in range(TRIALS): SAVE_PATH = DATA_PATH + 'Trial_%d/' % i mkdir(SAVE_PATH) p_mode_velocity_file = SAVE_PATH + 'p_mode_velocities.csv' scene_object_file = SAVE_PATH + 'scene_objects' start = time.time() while time.time() - start <= 120.0: with Interface(save=True, filename=p_mode_velocity_file) as I: # LOAD ROBOT robot = I.load_model(ROBOT_URDF_PATH) # LOAD TABLE table = I.load_model(TABLE, Pose(Point(0.4, 0.0, 0.05), Euler(1.57, 0.0, 0.0)), fixed_base=True, scaling=1.0) #TODO set camera I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7]) #LOAD_BLOCKS blocks = load_blocks(I) if len(blocks) != len(BLOCKS): continue time.sleep(1.0) # Gripper ID gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK) # Neutral Pose neutralPose = Pose(Point(0.35, 0.0, 0.5), Euler(0.0, 3.14, 1.57)) I.save_body_conf(save=True, filename=scene_object_file) offset = 0.005 x_offset = [ 0.0, 0.0, 0.0, offset, offset, offset, -offset, -offset, -offset ] y_offset = [ 0.0, offset, -offset, 0.0, offset, -offset, 0.0, offset, -offset ] for k in range(9): # Set initial state I.restore_body_conf() I.set_initial_state(robot) success = False # Plan motion paths = {} for j in range(0, len(blocks), 2): initialize = True if j == 0 else False path = plan(I, robot, gripper_link, blocks[j:j + 2], neutralPose, initialize=initialize, x_offset=x_offset[k], y_offset=y_offset[k]) if path is not None: paths[j] = path else: paths = {} break if len(paths) > 0: I.reset_simulation(robot) time.sleep(1.0) I.command(robot, paths[0][0]) I.command(robot, paths[0][1]) I.command(robot, paths[0][2]) I.gripper(robot, 'close') I.command(robot, paths[0][3]) I.command(robot, paths[0][4]) I.command(robot, paths[0][5]) I.gripper(robot, 'open') I.command(robot, paths[0][6]) time.sleep(1.0) if on_top(I, *blocks[0:2]): I.command(robot, paths[2][0]) I.command(robot, paths[2][1]) I.gripper(robot, 'close') I.command(robot, paths[2][2]) I.command(robot, paths[2][3]) I.command(robot, paths[2][4]) I.gripper(robot, 'open') I.command(robot, paths[2][5]) time.sleep(1.0) if on_top(I, *blocks[2:4]): I.command(robot, paths[4][0]) I.command(robot, paths[4][1]) I.gripper(robot, 'close') I.command(robot, paths[4][2]) I.command(robot, paths[4][3]) I.command(robot, paths[4][4]) I.gripper(robot, 'open') I.command(robot, paths[4][5]) time.sleep(1.0) if on_top(I, *blocks[4:6]): success = True time.sleep(1.0) I.vel.write_csv() break if success: break
# coding=utf-8 from pblt import Interface import time from geometry import Point, Pose, Euler MOVABLE_ARM_JOINTS = [1, 2, 3, 4, 5, 6] ROBOT_URDF_PATH = '../models/ur5_description/urdf/ur5.urdf' END_EFFECTOR_LINK = 'end_effector_link' TABLE = '../models/table/table.urdf' CUBE_URDF = '../models/cube/%s_cube.urdf' I = Interface(save=True) I.connect() x_offset = 0.33 y_offset = 0.15 robot = I.load_model(ROBOT_URDF_PATH) redBlock1 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.3 + x_offset, -0.1 + y_offset, 0.265)), fixed_base=False) redBlock2 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.35 + x_offset, 0.05 + y_offset, 0.265)), fixed_base=False) table = I.load_model(TABLE, Pose(Point(0.35 + x_offset, 0.0 + y_offset, 0.05), Euler(0.0, 0.0, 1.57)), fixed_base=False, scaling=0.3)
def ur5(): I = Interface() I.connect() MOVABLE_ARM_JOINTS = [1, 2, 3, 4, 5, 6] ROBOT_URDF_PATH = '../models/ur5_description/urdf/ur5.urdf' END_EFFECTOR_LINK = 'end_effector_link' TABLE = '../models/table/table.urdf' CUBE_URDF = '../models/cube/%s_cube.urdf' # ------------------------UR5--------------------------# x_offset = 0.33 y_offset = 0.15 robot = I.load_model(ROBOT_URDF_PATH) redBlock1 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.3 + x_offset, -0.1 + y_offset, 0.265)), fixed_base=False) redBlock2 = I.load_model(CUBE_URDF % 'red', Pose( Point(0.35 + x_offset, 0.05 + y_offset, 0.265)), fixed_base=False) table = I.load_model(TABLE, Pose(Point(0.35 + x_offset, 0.0 + y_offset, 0.05), Euler(0.0, 0.0, 1.57)), fixed_base=False, scaling=0.3) # ------------------------Jaco---------------------------# velocities = Csv('UR5_velocities.csv', [ 'joints_1', 'joints_2', 'joints_3', 'joints_4', 'joints_5', 'joints_6', 'joints_11', 'joints_13', 'joints_15', 'joints_16', 'joints_17', 'joints_18' ], mode='r').read() torques = Csv('UR5_torques.csv', [ 'joints_1', 'joints_2', 'joints_3', 'joints_4', 'joints_5', 'joints_6', 'joints_11', 'joints_13', 'joints_15', 'joints_16', 'joints_17', 'joints_18' ], mode='r').read() action = Csv('UR5_action.csv', ['action'], mode='r').read() for joint, values in velocities.items(): velocities[joint] = map(float, values) velocities = np.vstack(velocities.values()) for joint, values in torques.items(): torques[joint] = map(float, values) torques = np.vstack(torques.values()) I.replay(robot, [velocities.T, torques.T, action.values()[0]]) # time.sleep(5.0) I.disconnect()
def jaco(): I = Interface() I.connect() #------------------------Jaco---------------------------# ROBOT_URDF_PATH = '../models/jaco_pkg/jaco_description/urdf/test_jaco.urdf' BLOCK_URDF = "../ss-pybullet-master/models/drake/objects/block_for_pick_and_place_mid_size.urdf" END_EFFECTOR_LINK = 'jaco_eef_link' TABLE = '../models/table/table.urdf' CUBE_URDF = '../models/cube/%s_cube.urdf' TRAY_URDF = '../models/tray/tray.urdf' BASKET_URDF = '../models/basket/basket.urdf' robot = I.load_model(ROBOT_URDF_PATH) redBlock1 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.22, -0.2, 0.265)), fixed_base=False) basket = I.load_model(BASKET_URDF, Pose(Point(0.35, 0.05, 0.265)), fixed_base=False) table = I.load_model(TABLE, Pose(Point(0.35, 0.0, 0.05), Euler(0.0, 0.0, 1.57)), fixed_base=False, scaling=0.3) #------------------------Jaco---------------------------# # velocities = Csv('velocities_UR5.csv',['joints_1','joints_2','joints_3','joints_4','joints_5','joints_6','joints_11','joints_13','joints_15','joints_16','joints_17','joints_18'],mode = 'r').read() # torques= Csv('torques_UR5.csv',['joints_1','joints_2','joints_3','joints_4','joints_5','joints_6','joints_11','joints_13','joints_15','joints_16','joints_17','joints_18'],mode = 'r').read() velocities = Csv('JACO_velocities.csv', [ 'joints_3', 'joints_5', 'joints_7', 'joints_9', 'joints_11', 'joints_13', 'joints_16', 'joints_19', 'joints_22' ], mode='r').read() torques = Csv('JACO_torques.csv', [ 'joints_3', 'joints_5', 'joints_7', 'joints_9', 'joints_11', 'joints_13', 'joints_16', 'joints_19', 'joints_22' ], mode='r').read() action = Csv('JACO_action.csv', ['action'], mode='r').read() for joint, values in velocities.items(): velocities[joint] = map(float, values) velocities = np.vstack(velocities.values()) for joint, values in torques.items(): torques[joint] = map(float, values) torques = np.vstack(torques.values()) I.replay(robot, [velocities.T, torques.T, action.values()[0]]) time.sleep(2.0) I.disconnect()
def main(): with open(pathname(os.getcwd(), '..', '..', 'machines.json'), 'r') as f: js = json.load(f) DATA_PATH = js[IP]['DATA_PATH'] trial = js[IP]['trial_start'] while True: if trial == js[IP]['trial_stop']: break start = time.time() while time.time() - start <= 120.0: SAVE_PATH = DATA_PATH + 'Trial_%d/' % trial mkdir(SAVE_PATH) p_mode_velocity_file = SAVE_PATH + 'p_mode_velocities.csv' scene_object_file = SAVE_PATH + 'scene' with Interface(mode='direct', save=True, filename=p_mode_velocity_file) as I: # LOAD ROBOT robot = I.load_model(ROBOT_URDF_PATH) # LOAD TABLE table = I.load_model(TABLE, Pose(Point(0.4, 0.0, 0.05), Euler(1.57, 0.0, 0.0)), fixed_base=True, scaling=1.0) # TODO set camera I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7]) # LOAD_BLOCKS blocks = load_blocks(I) if len(blocks) != len(BLOCKS): continue time.sleep(1.0) # Gripper ID gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK) # Neutral Pose neutralPose = Pose(Point(0.35, 0.0, 0.5), Euler(0.0, 3.14, 1.57)) I.save_body_conf(save=True, filename=scene_object_file) I.set_initial_state(robot) offset = 0.005 x_offset = [0.0, offset, -offset, -offset] y_offset = [0.0, offset, -offset, 0.0] shuffle(blocks) color = ['red'] for k in range(1): # Set initial state I.restore_body_conf() success = False # Plan motion paths = [] velocities = OrderedDict() for j in range(1): initialize = True if j == 0 else False path = plan(I, robot, gripper_link, blocks, neutralPose, initialize=initialize, x_offset=x_offset[k], y_offset=y_offset[k]) if path is not None: paths.append(path) else: paths = [] break if len(paths) > 0: I.reset_simulation(robot) # time.sleep(1.0) I.command(robot, paths[0][0]) I.command(robot, paths[0][1]) I.command(robot, paths[0][2]) I.gripper(robot, 'close') I.command(robot, paths[0][3]) I.command(robot, paths[0][4]) I.command(robot, paths[0][5]) I.gripper(robot, 'open') I.command(robot, paths[0][6]) if on_top(I, *blocks[0:2]): I.keep_pose(body=robot) I.vel.write_csv() velocities = I.vel.array I.restore_body_conf() I.reset_simulation(robot) I.replay_velocities(robot, velocities) if on_top(I, *blocks[0:2]): I.restore_body_conf() I.reset_simulation(robot) time.sleep(1.0) I.replay_velocities( robot, velocities, path=os.path.abspath(SAVE_PATH)) if on_top(I, *blocks[0:2]): success = True np.save( os.path.join(SAVE_PATH, 'velocities'), np.array(I.target_vel)) np.save( os.path.join(SAVE_PATH, 'joints_configurations'), np.array(I.joint_val)) trial += 1 break else: shutil.rmtree(SAVE_PATH) if success: break else: if folder_exists(SAVE_PATH): shutil.rmtree(SAVE_PATH)
# coding=utf-8 from pblt import Interface, MOVABLE_ARM_JOINTS import time from geometry import Point, Pose, Euler ROBOT_URDF_PATH = '../models/jaco_pkg/jaco_description/urdf/test_jaco.urdf' END_EFFECTOR_LINK = 'jaco_eef_link' TABLE = '../models/table/table.urdf' CUBE_URDF = '../models/cube/%s_cube.urdf' BASKET_URDF = '../models/basket/basket.urdf' with Interface() as I: robot = I.load_model(ROBOT_URDF_PATH) table = I.load_model(TABLE, Pose(Point(0.35, 0.0, 0.05), Euler(0.0, 0.0, 1.57)), fixed_base=False, scaling=0.3) redBlock1 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.22, -0.2, 0.265)), fixed_base=False) basket = I.load_model(BASKET_URDF, Pose(Point(0.35, 0.05, 0.265)), fixed_base=False) def move_to(target_pose, gripper_link): paths = [] for poses in target_pose:
limits = {'x_min': 0.2, 'x_max': 0.35, 'y_min': -0.3, 'y_max': 0.3} block_thresh = 0.125 TRIALS = 10000 BLOCKS = [ "%(color)sBlock%(indice)d" % ({ 'color': name, 'indice': indice }) for name in ['red', 'green', 'blue'] for indice in [1, 2] ] BLOCK_SIZE = 0.05 Block = namedtuple('Block', ['pose', 'handle']) I = Interface() I.connect() # LOAD ROBOT robot = I.load_model(ROBOT_URDF_PATH) # LOAD TABLE table = I.load_model(TABLE, Pose(Point(0.4, 0.0, 0.05), Euler(1.57, 0.0, 0.0)), fixed_base=True, scaling=1.0) # TODO set camera camTargetPos = [0, 0, 0] cameraUp = [0, 0, 1] cameraPos = [1, 1, 1]
def main(): with open(pathname(os.getcwd(), '..', '..', 'machines.json'), 'r') as f: js = json.load(f) DATA_PATH = js[IP]['DATA_PATH'] trial = js[IP]['trial_start'] while True: if trial == js[IP]['trial_stop']: break start = time.time() while time.time() - start <= 120.0: SAVE_PATH = DATA_PATH + 'Trial_%d/' % trial mkdir(SAVE_PATH) p_mode_velocity_file = SAVE_PATH + 'p_mode_velocities.csv' scene_object_file = SAVE_PATH + 'scene_objects' with Interface(mode='direct', save=True, filename=p_mode_velocity_file) as I: # LOAD ROBOT robot = I.load_model(ROBOT_URDF_PATH) # LOAD TABLE table = I.load_model(TABLE, Pose(Point(0.4, 0.0, 0.05), Euler(1.57, 0.0, 0.0)), fixed_base=True, scaling=1.0) # TODO set camera I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7]) # LOAD SCENE OBJECTS scene_object = load_objects( I, **dict(zip(SCENE_OBJECT, [CUBE_URDF, BASKET_URDF]))) if len(scene_object) != len(SCENE_OBJECT): continue # Gripper ID gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK) # Neutral Pose neutralPose = Pose(Point(0.35, 0.0, 0.5), Euler(0.0, 3.14, 1.57)) I.save_body_conf(save=True, filename=scene_object_file) I.set_initial_state(robot) offset = 0.005 x_offset = [ 0.0, 0.0, 0.0, offset, offset, offset, -offset, -offset, -offset ] y_offset = [ 0.0, offset, -offset, 0.0, offset, -offset, 0.0, offset, -offset ] for k in range(2): # Set initial state I.restore_body_conf() success = False initialize = True path = plan(I, robot, gripper_link, scene_object, neutralPose, initialize=initialize, x_offset=x_offset[k], y_offset=y_offset[k]) if len(path) > 0: I.reset_simulation(robot) time.sleep(1.0) I.command(robot, path[0]) I.command(robot, path[1]) I.command(robot, path[2]) I.gripper(robot) I.command(robot, path[3]) I.command(robot, path[4]) I.command(robot, path[5]) I.gripper(robot, 'open') I.command(robot, path[6]) if inside_basket(I, *scene_object): success = True I.keep_pose(body=robot) I.vel.write_csv() velocities = I.vel.array I.restore_body_conf() I.reset_simulation(robot) I.replay_velocities(robot, velocities) if inside_basket(I, *scene_object): I.restore_body_conf() I.reset_simulation(robot) time.sleep(1.0) I.replay_velocities( robot, velocities, path=os.path.abspath(SAVE_PATH)) if inside_basket(I, *scene_object): np.save( os.path.join(SAVE_PATH, 'velocities'), np.array(I.target_vel)) np.save( os.path.join(SAVE_PATH, 'joints_configurations'), np.array(I.joint_val)) trial += 1 else: shutil.rmtree(SAVE_PATH) else: shutil.rmtree(SAVE_PATH) break else: shutil.rmtree(SAVE_PATH) if success: break
TABLE_HEIGHT = 0.126 limits = {'x_min': 0.2, 'x_max': 0.35, 'y_min': -0.3, 'y_max': 0.3} block_thresh = 0.125 BLOCKS = [ "%(color)sBlock%(indice)d" % ({ 'color': name, 'indice': indice }) for name in ['red'] for indice in [1, 2] ] BLOCK_SIZE = 0.05 Block = namedtuple('Block', ['pose', 'handle', 'name']) I = Interface() I.connect() robot = I.load_model(ROBOT_URDF_PATH) # table = I.load_model(TABLE, Pose(Point(0.4, 0.0, 0.05), Euler(1.57, 0.0, 0.0)), # fixed_base = True, scaling = 1.0) # I.load_model(CUBE_URDF, Pose(Point(0.3, -0.1, 0.126)) # , # fixed_base=True, scaling=1.0) # # # I.load_model(GREEN_BASKET_URDF, Pose(Point(0.4, 0.25, 0.126)) , # fixed_base=True, scaling=1.0) # I.load_model(BASKET_URDF, Pose(Point(0.45, 0.1, 0.126)) ,
def main(): trial = 2495 while True: if trial == 3000: break start = time.time() while time.time() - start <= 120.0: SAVE_PATH = DATA_PATH + 'Trial_%d/' % trial mkdir(SAVE_PATH) p_mode_velocity_file = SAVE_PATH + 'VELOCITIES/' scene_object_file = SAVE_PATH + 'SCENE_OBJECTS/' blocks_goals_file = SAVE_PATH + 'GOALS/' mkdir(scene_object_file) mkdir(p_mode_velocity_file) with Interface(mode='direct', save=True, filename=p_mode_velocity_file + 'p_mode_velocities.csv') as I: # LOAD ROBOT robot = I.load_model(ROBOT_URDF_PATH) # LOAD TABLE table = I.load_model(TABLE, Pose(Point(0.4, 0.0, 0.05), Euler(1.57, 0.0, 0.0)), fixed_base=True, scaling=1.0) #TODO set camera I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7]) #LOAD_BLOCKS blocks = load_blocks(I) if len(blocks) != len(BLOCKS): continue time.sleep(1.0) # Gripper ID gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK) # Neutral Pose neutralPose = Pose(Point(0.35, 0.0, 0.5), Euler(0.0, 3.14, 1.57)) I.save_body_conf(save=True, filename=scene_object_file + 'scene') I.set_initial_state(robot) offset = 0.005 x_offset = [ 0.0, 0.0, 0.0, offset, offset, offset, -offset, -offset, -offset ] y_offset = [ 0.0, offset, -offset, 0.0, offset, -offset, 0.0, offset, -offset ] blocks = [blocks[k * 2:2 * (k + 1)] for k in range(3)] shuffle(blocks) blocks = blocks[0] + blocks[1] + blocks[2] color = [blocks[0].name, blocks[2].name, blocks[4].name] for k in range(9): # Set initial state I.restore_body_conf() success = False # Plan motion paths = {} velocities = OrderedDict() for j in range(0, len(blocks), 2): initialize = True if j == 0 else False path = plan(I, robot, gripper_link, blocks[j:j + 2], neutralPose, initialize=initialize, x_offset=x_offset[k], y_offset=y_offset[k]) if path is not None: paths[j] = path else: paths = {} break if len(paths) > 0: I.reset_simulation(robot) # time.sleep(1.0) I.command(robot, paths[0][0]) I.command(robot, paths[0][1]) I.command(robot, paths[0][2]) I.gripper(robot, 'close') I.command(robot, paths[0][3]) I.command(robot, paths[0][4]) I.command(robot, paths[0][5]) I.gripper(robot, 'open') I.command(robot, paths[0][6]) # time.sleep(1.0) if on_top(I, *blocks[0:2]): velocities[color[0]] = I.vel.array shape = I.vel.array.shape[0] I.command(robot, paths[2][0]) I.command(robot, paths[2][1]) I.gripper(robot, 'close') I.command(robot, paths[2][2]) I.command(robot, paths[2][3]) I.command(robot, paths[2][4]) I.gripper(robot, 'open') I.command(robot, paths[2][5]) # time.sleep(1.0) if on_top(I, *blocks[2:4]): velocities[color[1]] = I.vel.array[shape:] shape = I.vel.array.shape[0] I.command(robot, paths[4][0]) I.command(robot, paths[4][1]) I.gripper(robot, 'close') I.command(robot, paths[4][2]) I.command(robot, paths[4][3]) I.command(robot, paths[4][4]) I.gripper(robot, 'open') I.command(robot, paths[4][5]) # time.sleep(1.0) if on_top(I, *blocks[4:6]): velocities[color[2]] = I.vel.array[shape:] success = True time.sleep(1.0) for e, colors in enumerate( velocities.keys()): mkdir(blocks_goals_file + '%d_' % e + colors) np.save( blocks_goals_file + '%d_' % e + colors + '/' + colors, velocities[colors]) I.vel.write_csv() I.restore_body_conf() I.reset_simulation(robot) time.sleep(1.0) task_velocities = np.concatenate( velocities.values()) I.replay_velocities(robot, task_velocities) final_stacking = [] for k in range(0, len(blocks), 2): final_stacking.append( on_top(I, *blocks[k:k + 2])) if all(final_stacking): I.restore_body_conf() I.reset_simulation(robot) time.sleep(1.0) replay_final_stacking = [] I.replay_velocities( robot, task_velocities, path=os.path.abspath( os.path.join( SAVE_PATH, 'VELOCITIES'))) for k in range(0, len(blocks), 2): replay_final_stacking.append( on_top(I, *blocks[k:k + 2])) if all(replay_final_stacking): #save np.save( os.path.join( SAVE_PATH, 'VELOCITIES', 'velocities'), np.array(I.target_vel)) np.save( os.path.join( SAVE_PATH, 'VELOCITIES', 'joints_configurations'), np.array(I.joint_val)) trial += 1 else: shutil.rmtree(SAVE_PATH) else: shutil.rmtree(SAVE_PATH) break else: shutil.rmtree(SAVE_PATH) if success: break