def init_robot(config): """ Initializes a robot """ robot = None subscriber = None initialized = False while not initialized: try: robot = YuMiRobot(debug=config['robot_off']) robot.set_v(config['control']['standard_velocity']) robot.set_z(config['control']['standard_zoning']) if config['control']['use_left']: arm = robot.left arm.goto_state(YMC.L_HOME_STATE) home_pose = YMC.L_PREGRASP_POSE else: arm = robot.right arm.goto_state(YMC.R_HOME_STATE) home_pose = YMC.R_AWAY_STATE subscriber = YuMiSubscriber() subscriber.start() initialized = True except YuMiCommException as ymc: if robot is not None: robot.stop() if subscriber is not None and subscriber._started: subscriber.stop() logging.error(str(ymc)) logging.error( 'Failed to initialize YuMi. Check the FlexPendant and connection to the YuMi.' ) human_input = raw_input('Hit [ENTER] when YuMi is ready') return robot, subscriber, arm, home_pose
""" Helper script to move YuMi back to home pose Author: Jeff Mahler """ from yumipy import YuMiRobot, YuMiState from yumipy import YuMiConstants as YMC if __name__ == '__main__': y = YuMiRobot() y.set_z('z50') y.set_v(500) y.left.close_gripper() robot = y arm = y.left arm.goto_pose(YMC.L_PREGRASP_POSE, wait_for_res=True) # shake test radius = 0.1 angle = np.pi / 8 delta_T = RigidTransform(translation=[0, 0, -radius], from_frame='gripper', to_frame='gripper') R_shake = np.array([[1, 0, 0], [0, np.cos(angle), -np.sin(angle)], [0, np.sin(angle), np.cos(angle)]]) delta_T_up = RigidTransform(rotation=R_shake, translation=[0, 0, radius], from_frame='gripper', to_frame='gripper') delta_T_down = RigidTransform(rotation=R_shake.T,
time_str = "2020-11-10 16:33:00" timestamp = time.mktime( datetime.strptime(time_str, "%Y-%m-%d %H:%M:%S").timetuple()) # log = open('/home/urp/experiment/robotlog.txt', 'a') timeServer = 'time.windows.com' c = ntplib.NTPClient() response = c.request(timeServer, version=3) # log_str = "Time difference(Server time - Local time): %.4fs \n" %response.offset # log.write(log_str) # log.close() time_diff = response.offset ###################################################################### # y.reset_home() y.set_v(300) # y.calibrate_grippers() # y.open_grippers() # while (time.time() < timestamp): # continue # main_work() # y.right.goto_pose(RigidTransform(translation = [0.28, -0.1 , 0.07], rotation = right_side_rotation)) # y.right.goto_pose(RigidTransform(translation = [0.28, 0.04 , 0.07], rotation = right_side_rotation)) y.right.goto_pose( RigidTransform(translation=[0.28, 0, 0.2], rotation=right_top_rotation)) # y.right.goto_pose(RigidTransform(translation = [0.28, 0, 0.06], rotation = right_top_rotation))
def playback(args): cfg = YamlConfig(args.config_path) demo_name = args.demo_name supervisor = args.supervisor trial_num = args.trial_num if cfg['mode'] not in ('poses', 'states'): y.stop() raise ValueError( "Unknown playback mode! Only accepts 'poses' or 'joints'. Got {0}". format(cfg['mode'])) # init robot logging.info("Init robot.") y = YuMiRobot() y.set_v(cfg['v']) y.set_z(cfg['z']) # load demo data demo_records = CSVModel.load( os.path.join(cfg['data_path'], 'demo_records.csv')) demo_record = demo_records.get_by_cols({ 'demo_name': demo_name, 'trial_num': trial_num, 'supervisor': supervisor }) trial_path = demo_record['trial_path'] demo_host_cfg = YamlConfig(os.path.join(trial_path, 'demo_config.yaml')) # parse demo trajectory # TODO: enforce fps fps = demo_host_cfg['fps'] _, left_data = zip( *load(os.path.join(trial_path, '{0}_left.jb'.format(cfg['mode'])))) _, right_data = zip( *load(os.path.join(trial_path, '{0}_right.jb'.format(cfg['mode'])))) _, gripper_left_evs = zip( *load(os.path.join(trial_path, 'grippers_evs_left.jb'))) _, gripper_right_evs = zip( *load(os.path.join(trial_path, 'grippers_evs_right.jb'))) seqs = { 'left': Sequence([t[1] for t in left_data]), 'right': Sequence([t[1] for t in right_data]), 'gripper_left': Sequence(gripper_left_evs), 'gripper_right': Sequence(gripper_right_evs) } # subsampling subsample_factor = cfg['subsample'] subsampled_seqs = { 'left': seqs['left'].subsampler(subsample_factor), 'right': seqs['right'].subsampler(subsample_factor), 'gripper_left': seqs['gripper_left'].subsampler(subsample_factor, retain_features=True), 'gripper_right': seqs['gripper_right'].subsampler(subsample_factor, retain_features=True) } # concating non-moving steps if cfg['concat']: concat_data = { 'left': [subsampled_seqs['left'].data[0]], 'right': [subsampled_seqs['right'].data[0]], 'gripper_left': [subsampled_seqs['gripper_left'].data[0]], 'gripper_right': [subsampled_seqs['gripper_right'].data[0]] } last_lp = concat_data['left'][0] last_rp = concat_data['right'][0] for t in range( 1, min([len(seq.data) for seq in subsampled_seqs.values()])): lg_t = subsampled_seqs['gripper_left'].data[t] rg_t = subsampled_seqs['gripper_right'].data[t] lp_t = subsampled_seqs['left'].data[t] rp_t = subsampled_seqs['right'].data[t] if lg_t is not None or rg_t is not None or \ lp_t != last_lp or rp_t != last_rp: concat_data['gripper_right'].append(rg_t) concat_data['gripper_left'].append(lg_t) concat_data['left'].append(lp_t) concat_data['right'].append(rp_t) last_lp = lp_t last_rp = rp_t concat_seqs = { 'left': Sequence(concat_data['left']), 'right': Sequence(concat_data['right']), 'gripper_left': Sequence(concat_data['gripper_left']), 'gripper_right': Sequence(concat_data['gripper_right']), } else: concat_seqs = subsampled_seqs N = min([len(seq.data) for seq in concat_seqs.values()]) # processing time steps where zoning should be set to fine gripper_zoning = [None for _ in range(N)] for t in range(N - 1): if concat_seqs['gripper_left'].data[t] != None or \ concat_seqs['gripper_right'].data[t] != None: if t == 0: y.set_z('fine') else: gripper_zoning[t - 1] = 'fine' gripper_zoning[t + 1] = cfg['z'] # perform setup motions logging.info("Loading demo and performing setups.") y.reset_home() y.open_grippers() demo_path = os.path.join(trial_path, '{0}.py'.format(demo_name)) demo_obj = DemoWrapper.load(demo_path, y) demo_obj.setup() # record torque and other debug data if needed if cfg['record_torque']['use']: ysub = YuMiSubscriber() ysub.start() data_torque_left = DataStreamRecorder('torques_left', ysub.left.get_torque, cache_path=cfg['cache_path'], save_every=cfg['save_every']) data_torque_right = DataStreamRecorder('torques_right', ysub.right.get_torque, cache_path=cfg['cache_path'], save_every=cfg['save_every']) syncer = DataStreamSyncer([data_torque_left, data_torque_right], fps) syncer.start() sleep(0.5) syncer.pause() syncer.flush() syncer.resume(reset_time=True) # perform trajectory logging.info("Playing trajectory") for t in range(N): left_item = concat_seqs['left'].data[t] right_item = concat_seqs['right'].data[t] gripper_left_item = concat_seqs['gripper_left'].data[t] gripper_right_item = concat_seqs['gripper_right'].data[t] if cfg['mode'] == 'poses': y.left.goto_pose(left_item, relative=True, wait_for_res=False) y.right.goto_pose(right_item, relative=True, wait_for_res=True) else: y.left.goto_state(left_item, wait_for_res=False) y.right.goto_state(right_item, wait_for_res=True) if gripper_left_item != None and gripper_right_item != None: getattr(y.left, gripper_left_item)(wait_for_res=False) getattr(y.right, gripper_right_item)(wait_for_res=True) elif gripper_left_item != None: getattr(y.left, gripper_left_item)() elif gripper_right_item != None: getattr(y.right, gripper_right_item)() z = gripper_zoning[t] if z is not None: logging.info("Setting zone to {0}".format(z)) y.set_z(z) if cfg['record_torque']['use']: syncer.pause() torque_model = CSVModel.get_or_create( os.path.join(cfg['data_path'], 'playback_torques_record.csv'), [('supervisor', 'str'), ('demo_name', 'str'), ('trial_num', 'int'), ('playback_num', 'int'), ('playback_path', 'str')]) last_torque_record = torque_model.get_by_cols( { 'demo_name': demo_name, 'trial_num': trial_num, 'supervisor': supervisor }, direction=-1) if last_torque_record == None: playback_num = 1 else: playback_num = last_torque_record['playback_num'] + 1 playback_path = os.path.join(trial_path, 'playback_torques', str(playback_num)) if not os.path.exists(playback_path): os.makedirs(playback_path) data_torque_left.save_data(playback_path) data_torque_right.save_data(playback_path) torque_model.insert({ 'supervisor': supervisor, 'demo_name': demo_name, 'trial_num': trial_num, 'playback_num': playback_num, 'playback_path': playback_path }) basename = os.path.basename(args.config_path) target_file_path = os.path.join(playback_path, basename) shutil.copyfile(args.config_path, target_file_path) ysub.stop() syncer.stop() # perform takedown motions logging.info("Taking down..") y.set_v(cfg['v']) y.set_z(cfg['z']) demo_obj.takedown() y.reset_home() y.open_grippers() y.stop()
def run(arm, n, v, zone, output_path): y = YuMiRobot(log_state_histories=True) y.set_v(v) y.set_z(zone) y_arm = getattr(y, arm) #collect data t, m, d = move_times(y_arm.goto_state, targets, n) #save data np.savez(os.path.join(output_path, 'total_times'), t) np.savez(os.path.join(output_path, 'motion_times'), m) np.savez(os.path.join(output_path, 'diff_times'), d) x_range = np.arange(len(t)) fig = plt.figure(figsize=(12, 8)) ax = fig.gca() ax.plot(x_range, t, 'r-', label='Total times') ax.plot(x_range, m, 'g-', label='Motion Times') ax.plot(x_range, d, 'b-', label='Latency Times') legend = ax.legend(loc='best', shadow=False) frame = legend.get_frame() for label in legend.get_texts(): label.set_fontsize('large') for label in legend.get_lines(): label.set_linewidth(1.5) ax.set_title("YuMi Command Times", fontsize=20) ax.set_xlabel("Command", fontsize=14) ax.set_ylabel("Seconds", fontsize=14) fig.savefig(os.path.join(output_path, 'yumi_comm_times.pdf'), format='pdf') #histograms for all 3 times for data, name in ((t, 'total_times'), (m, 'motion_times'), (d, 'latencies')): fig = plt.figure(figsize=(12, 8)) ax = fig.gca() mean = np.mean(data) std = np.std(data) stats_str = 'mean: {:.3g}\nstd: {:.3g}'.format(mean, std) props = dict(facecolor='white', alpha=0.5) ax.set_title( 'Histogram of 2-way YuMi Communication Times: {0}'.format(name), fontsize=20) ax.set_xlabel('Seconds', fontsize=18) ax.set_ylabel('Normalized Count', fontsize=18) # place a text box in upper left in axes coords ax.text(0.05, 0.95, stats_str, transform=ax.transAxes, fontsize=14, verticalalignment='top', bbox=props) h = plt.hist(data, normed=True, bins=30) fig.savefig(os.path.join(output_path, 'hist_yumi_2_way_{0}.pdf'.format(name)), format='pdf')
""" Helper script to move YuMi back to home pose Author: Jeff Mahler """ from autolab_core import RigidTransform import numpy as np from yumipy import YuMiRobot, YuMiState from yumipy import YuMiConstants as YMC if __name__ == '__main__': y = YuMiRobot() y.set_z('z50') y.set_v(1000) y.left.close_gripper() robot = y arm = y.left arm.goto_pose(YMC.L_PREGRASP_POSE, wait_for_res=True) # shake test radius = 0.2 angle = np.pi / 64 delta_T = RigidTransform(translation=[0,0,radius], from_frame='gripper', to_frame='gripper') R_shake = np.array([[1, 0, 0], [0, np.cos(angle), -np.sin(angle)], [0, np.sin(angle), np.cos(angle)]]) delta_T_up = RigidTransform(rotation=R_shake, translation=[0,0,-radius], from_frame='gripper', to_frame='gripper') delta_T_down = RigidTransform(rotation=R_shake.T, translation=[0,0,-radius], from_frame='gripper', to_frame='gripper') T_shake_up = YMC.L_PREGRASP_POSE.as_frames('gripper', 'world') * delta_T_up * delta_T
class _YuMiArmPoller(Process): def __init__(self, pose_q, cmds_q, ret_q, z, v, arm_name): Process.__init__(self) self.pose_q = pose_q self.cmds_q = cmds_q self.ret_q = ret_q self.z = z self.v = v self.arm_name = arm_name self.forward_poses = False self.filter = IdentityFilter() self.filter_times = [] self.cmd_times = [] def run(self): self.move_counter = 0 if self.arm_name == "left": self.y = YuMiRobot(include_right=False) self.arm = self.y.left elif self.arm_name == "right": self.y = YuMiRobot(include_left=False) self.arm = self.y.right self.y.set_v(self.v) self.y.set_z(self.z) while True: try: if not self.cmds_q.empty(): cmd = self.cmds_q.get() if cmd[0] == 'forward': self.forward_poses = cmd[1] if cmd[1]: self.filter.reset() self.y.set_v(self.v) self.y.set_z(self.z) self.move_counter = 0 elif cmd[0] == 'stop': break elif cmd[0] == 'filter': self.filter = cmd[1] elif cmd[0] == 'count': while self.ret_q.qsize() > 0: self.ret_q.get_nowait() self.ret_q.put(self.move_counter) elif cmd[0] == 'method': args = cmd[3]['args'] kwargs = cmd[3]['kwargs'] method_name = cmd[2] if cmd[1] == 'both': method = getattr(self.y, method_name) elif cmd[1] == 'single': method = getattr(self.arm, method_name) retval = method(*args, **kwargs) while self.ret_q.qsize() > 0: self.ret_q.get_nowait() if retval is not None: self.ret_q.put(retval) elif self.forward_poses and not self.pose_q.empty(): self.move_counter += 1 try: # start = time() pose = self.pose_q.get() filtered_pose = self.filter.apply(pose) # self.filter_times.append(time() - start) # print '{} filter time is {}'.format(self.arm_name, np.mean(self.filter_times)) try: # start = time() res = self.arm.goto_pose(filtered_pose, relative=True) # self.cmd_times.append(time() - start) # print '{} cmd time is {}'.format(self.arm_name, np.mean(self.cmd_times)) except YuMiControlException: logging.warn("Pose unreachable!") except Empty: pass sleep(0.001) except KeyboardInterrupt: logging.debug("Shutting down {0} arm poller".format(self.arm_name)) self.y.stop() def stop(self): self.cmds_q.put(('stop',)) def set_forward(self, val): self.cmds_q.put(('forward', val)) def send_cmd(self, packet): self.cmds_q.put(packet) def set_filter(self, motion_filter): self.cmds_q.put(('filter', motion_filter))
import numpy as np import time from yumipy import YuMiRobot, YuMiState from autolab_core import RigidTransform if __name__ == '__main__': num_attempts = 3 state = YuMiState([51.16, -99.4, 21.57, -107.19, 84.11, 94.61, -36.00]) robot = YuMiRobot(debug=False) robot.set_v(50) for i in range(num_attempts): print 'Trying collision', i robot.right.goto_state(state) robot.right.goto_pose_delta([0, 0.236, 0]) robot.right.goto_pose_delta([0.053, 0, 0]) robot.right.goto_pose_delta([0, 0, -0.145]) time.sleep(3)
def playback(args): cfg = YamlConfig(args.config_path) demo_name = args.demo_name supervisor = args.supervisor trial_num = args.trial_num if cfg['mode'] not in ('poses', 'states'): y.stop() raise ValueError("Unknown playback mode! Only accepts 'poses' or 'joints'. Got {0}".format(cfg['mode'])) # init robot logging.info("Init robot.") y = YuMiRobot() y.set_v(cfg['v']) y.set_z(cfg['z']) # load demo data demo_records = CSVModel.load(os.path.join(cfg['data_path'], 'demo_records.csv')) demo_record = demo_records.get_by_cols({ 'demo_name': demo_name, 'trial_num': trial_num, 'supervisor': supervisor }) trial_path = demo_record['trial_path'] demo_host_cfg = YamlConfig(os.path.join(trial_path, 'demo_config.yaml')) # parse demo trajectory # TODO: enforce fps fps = demo_host_cfg['fps'] _, left_data = zip(*load(os.path.join(trial_path, '{0}_left.jb'.format(cfg['mode'])))) _, right_data = zip(*load(os.path.join(trial_path, '{0}_right.jb'.format(cfg['mode'])))) _, gripper_left_evs = zip(*load(os.path.join(trial_path, 'grippers_evs_left.jb'))) _, gripper_right_evs = zip(*load(os.path.join(trial_path, 'grippers_evs_right.jb'))) seqs = { 'left': Sequence([t[1] for t in left_data]), 'right': Sequence([t[1] for t in right_data]), 'gripper_left': Sequence(gripper_left_evs), 'gripper_right': Sequence(gripper_right_evs) } # subsampling subsample_factor = cfg['subsample'] subsampled_seqs = { 'left': seqs['left'].subsampler(subsample_factor), 'right': seqs['right'].subsampler(subsample_factor), 'gripper_left': seqs['gripper_left'].subsampler(subsample_factor, retain_features=True), 'gripper_right': seqs['gripper_right'].subsampler(subsample_factor, retain_features=True) } # concating non-moving steps if cfg['concat']: concat_data = { 'left': [subsampled_seqs['left'].data[0]], 'right': [subsampled_seqs['right'].data[0]], 'gripper_left': [subsampled_seqs['gripper_left'].data[0]], 'gripper_right': [subsampled_seqs['gripper_right'].data[0]] } last_lp = concat_data['left'][0] last_rp = concat_data['right'][0] for t in range(1, min([len(seq.data) for seq in subsampled_seqs.values()])): lg_t = subsampled_seqs['gripper_left'].data[t] rg_t = subsampled_seqs['gripper_right'].data[t] lp_t = subsampled_seqs['left'].data[t] rp_t = subsampled_seqs['right'].data[t] if lg_t is not None or rg_t is not None or \ lp_t != last_lp or rp_t != last_rp: concat_data['gripper_right'].append(rg_t) concat_data['gripper_left'].append(lg_t) concat_data['left'].append(lp_t) concat_data['right'].append(rp_t) last_lp = lp_t last_rp = rp_t concat_seqs = { 'left': Sequence(concat_data['left']), 'right': Sequence(concat_data['right']), 'gripper_left': Sequence(concat_data['gripper_left']), 'gripper_right': Sequence(concat_data['gripper_right']), } else: concat_seqs = subsampled_seqs N = min([len(seq.data) for seq in concat_seqs.values()]) # processing time steps where zoning should be set to fine gripper_zoning = [None for _ in range(N)] for t in range(N-1): if concat_seqs['gripper_left'].data[t] != None or \ concat_seqs['gripper_right'].data[t] != None: if t == 0: y.set_z('fine') else: gripper_zoning[t-1] = 'fine' gripper_zoning[t+1] = cfg['z'] # perform setup motions logging.info("Loading demo and performing setups.") y.reset_home() y.open_grippers() demo_path = os.path.join(trial_path, '{0}.py'.format(demo_name)) demo_obj = DemoWrapper.load(demo_path, y) demo_obj.setup() # record torque and other debug data if needed if cfg['record_torque']['use']: ysub = YuMiSubscriber() ysub.start() data_torque_left = DataStreamRecorder('torques_left', ysub.left.get_torque, cache_path=cfg['cache_path'], save_every=cfg['save_every']) data_torque_right = DataStreamRecorder('torques_right', ysub.right.get_torque, cache_path=cfg['cache_path'], save_every=cfg['save_every']) syncer = DataStreamSyncer([data_torque_left, data_torque_right], fps) syncer.start() sleep(0.5) syncer.pause() syncer.flush() syncer.resume(reset_time=True) # perform trajectory logging.info("Playing trajectory") for t in range(N): left_item = concat_seqs['left'].data[t] right_item = concat_seqs['right'].data[t] gripper_left_item = concat_seqs['gripper_left'].data[t] gripper_right_item = concat_seqs['gripper_right'].data[t] if cfg['mode'] == 'poses': y.left.goto_pose(left_item, relative=True, wait_for_res=False) y.right.goto_pose(right_item, relative=True, wait_for_res=True) else: y.left.goto_state(left_item, wait_for_res=False) y.right.goto_state(right_item, wait_for_res=True) if gripper_left_item != None and gripper_right_item != None: getattr(y.left, gripper_left_item)(wait_for_res=False) getattr(y.right, gripper_right_item)(wait_for_res=True) elif gripper_left_item != None: getattr(y.left, gripper_left_item)() elif gripper_right_item != None: getattr(y.right, gripper_right_item)() z = gripper_zoning[t] if z is not None: logging.info("Setting zone to {0}".format(z)) y.set_z(z) if cfg['record_torque']['use']: syncer.pause() torque_model = CSVModel.get_or_create( os.path.join(cfg['data_path'], 'playback_torques_record.csv'), [ ('supervisor', 'str'), ('demo_name', 'str'), ('trial_num', 'int'), ('playback_num', 'int'), ('playback_path', 'str') ] ) last_torque_record = torque_model.get_by_cols({ 'demo_name': demo_name, 'trial_num': trial_num, 'supervisor': supervisor }, direction=-1) if last_torque_record == None: playback_num = 1 else: playback_num = last_torque_record['playback_num'] + 1 playback_path = os.path.join(trial_path, 'playback_torques', str(playback_num)) if not os.path.exists(playback_path): os.makedirs(playback_path) data_torque_left.save_data(playback_path) data_torque_right.save_data(playback_path) torque_model.insert({ 'supervisor': supervisor, 'demo_name': demo_name, 'trial_num': trial_num, 'playback_num': playback_num, 'playback_path': playback_path }) basename = os.path.basename(args.config_path) target_file_path = os.path.join(playback_path, basename) shutil.copyfile(args.config_path, target_file_path) ysub.stop() syncer.stop() # perform takedown motions logging.info("Taking down..") y.set_v(cfg['v']) y.set_z(cfg['z']) demo_obj.takedown() y.reset_home() y.open_grippers() y.stop()
rotation=right_front_rotation)) # example of rigid transform: RigidTransform(translation = [x_pos(back to front), y_pos(right to left), z_pos], rotation=[w, x, y, z]) # To use rpy rotation, function rpy_to_wxyz(r, p, y) is implemented above left_top_rotation = rpy_to_wxyz(-90, 0, 180) left_front_rotation = rpy_to_wxyz(-90, 0, -90) left_side_rotation = rpy_to_wxyz(0, 0, -90) right_top_rotation = rpy_to_wxyz(90, 0, 180) right_side_rotation = rpy_to_wxyz(0, 0, 90) right_front_rotation = rpy_to_wxyz(90, 0, 90) right_top_give_rotation = rpy_to_wxyz(90, 0, 210) right_side_give_rotation = rpy_to_wxyz(0, 30, 90) right_front_give_rotation = rpy_to_wxyz(90, 0, 120) if __name__ == '__main__': y = YuMiRobot() y.set_z('z50') y.set_v(450) print('robot_action.py') y.open_grippers() y.reset_home() plate_catch(y) plate_drop(y) y.reset_home() y.open_grippers() y.stop()
while (count < train_num): catching_speed = random.randint(150, 1500) giving_speed = random.randint(150, 1500) catching_dir = random.randint(1, 3) pos = random.randint(1, 3) job = random.randint(1, 2) if (job == 1 and pos == 3): continue count += 1 print("Train count : %d" % count) if (job == 1): y.set_v(catching_speed) write_log.write_log(1, 0, catching_speed, catching_dir, pos, time_diff) robot_action.bottle_catch(y, catching_dir, pos) else: y.set_v(catching_speed) write_log.write_log(1, 0, catching_speed, catching_dir, pos, time_diff) robot_action.bottle_catch(y, catching_dir, pos) y.set_v(giving_speed) write_log.write_log(2, 0, giving_speed, catching_dir, pos, time_diff) robot_action.bottle_give(y, catching_dir, pos) time.sleep(1) os.system('play -nq -t alsa synth {} sine {}'.format(1, 440))
""" Helper script to move YuMi back to home pose Author: Jeff Mahler """ from yumipy import YuMiRobot, YuMiState from yumipy import YuMiConstants as YMC if __name__ == '__main__': y = YuMiRobot() y.set_z('fine') y.set_v(200) y.left.close_gripper() y.right.close_gripper() state = YuMiState([69.42, -75.37, 20.21, -110.77, 91.29, -62.35, -43.74]) #y.left.goto_pose(YMC.L_HOME_POSE) y.right.goto_state(state) #y.stop() #exit(0) for i, tup in enumerate(YMC.BOX_CLOSE_SEQUENCE): print i arm, box_state = tup if arm == 'L': if isinstance(box_state, YuMiState): y.left.goto_state(box_state) elif isinstance(box_state, list): y.left.goto_pose_delta(box_state) else: y.left.goto_pose(box_state)