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,
y = -float(grid_height) / 2 + grid_center_y + float(j * grid_height) / num_pts_y # form robot pose R_robot_world = np.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]]) t_robot_world = np.array([x, y, gripper_height]) T_robot_world = RigidTransform(rotation=R_robot_world, translation=t_robot_world, from_frame='gripper', to_frame='world') robot_poses.append(T_robot_world) # start robot y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF) y.set_z('fine') y.reset_home() global clicked_pt # iteratively go to poses robot_points_camera = [] for robot_pose in robot_poses: # reset clicked pt clicked_pt = None # move to pose y.right.goto_pose(robot_pose, wait_for_res=True) # capture image color_im, depth_im, _ = sensor.frames() depth_im = depth_im.inpaint(0.25)
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')
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))
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()
class ServiceBridge: def __init__(self): self.robot = YuMiRobot(arm_type='remote') self.left_arm = self.robot.left self.left_arm.calibrate_gripper() self.right_arm = self.robot.right self.right_arm.calibrate_gripper() self._create_services() rospy.loginfo("[ServiceBridge] Node is up!") self.current_tool_l = 'gripper' self.current_tool_r = 'gripper' self.tools = { 'gripper': [0, 0, 136, 0, 0, 0, 1], 'suction': [63.5, 18.5, 37.5, 0, 0, 0, 1], 'calibration': [0, 0, 0, 0, 0, 0, 1] } self.arms_wait_joint = {'left': [-1.53013010964, -2.31535371672, 0.57962382732, 1.8151423679999998, 1.8772760875199999, 0.037873643639999996, 0.36808992828], 'right': [1.5475834016399999, -2.3253020931599995, 0.58520888076, 4.45861797432, 1.8971728404, -0.04206243372, -0.34033919399999996]} def _create_services(self): rospy.Service("~/goto_pose", GotoPose, self.goto_pose_cb) rospy.Service("~/goto_pose_plan", GotoPose, self.goto_pose_plan_cb) rospy.Service("~/goto_pose_sync", GotoPoseSync, self.goto_pose_sync_cb) rospy.Service("~/goto_joints", GotoJoint, self.goto_joints_cb) rospy.Service("~/close_gripper", YumipyTrigger, self.close_gripper_cb) rospy.Service("~/open_gripper", YumipyTrigger, self.open_gripper_cb) rospy.Service("~/move_gripper", MoveGripper, self.move_gripper_cb) rospy.Service("~/get_gripper_width", YumipyTrigger, self.get_gripper_width) #rospy.Service("/set_speed", , self.set_speed_cb) rospy.Service("~/set_tool", SetTool, self.set_tool_cb) rospy.Service("~/set_zone", SetZ, self.set_zone_cb) rospy.Service("~/reset_home", YumipyTrigger, self.reset_home_cb) rospy.Service("~/calibrate_gripper", YumipyTrigger, self.calibrate_gripper_cb) rospy.Service("~/turn_on_suction", YumipyTrigger, self.turn_on_suction_cb) rospy.Service("~/turn_off_suction", YumipyTrigger, self.turn_off_suction_cb) rospy.Service("~/goto_wait_pose", Trigger, self.goto_wait_pose_cb) rospy.Service("~/goto_wait_joint", YumipyTrigger, self.goto_wait_joint_cb) def goto_pose_cb(self, req): response = GotoPoseResponse() response.success = False if (req.arm != 'left' and req.arm != 'right') or len(req.position) != 3 or len(req.quat) != 4: return response pose = '' for index, p in enumerate(req.position): pose += str(p) + ' ' for q in req.quat: pose += str(q) + ' ' pose = message_to_pose(pose, 'tool') if req.arm == 'left': ret = self.left_arm.goto_pose(pose, wait_for_res=req.wait_for_res) else: ret = self.right_arm.goto_pose(pose, wait_for_res=req.wait_for_res) if ret is not None: response.success = True return response def goto_pose_plan_cb(self, req): response = GotoPoseResponse() response.success = False if (req.arm != 'left' and req.arm != 'right') or len(req.position) != 3 or len(req.quat) != 4: return response pose = '' for index, p in enumerate(req.position): pose += str(p) + ' ' for q in req.quat: pose += str(q) + ' ' pose = message_to_pose(pose, 'tool') if req.arm == 'left': ret = self.left_arm.goto_pose_shortest_path(pose, wait_for_res=req.wait_for_res, tool=self.current_tool_l) else: ret = self.right_arm.goto_pose_shortest_path(pose, wait_for_res=req.wait_for_res, tool=self.current_tool_r) if ret is not None: response.success = True return response def goto_pose_sync_cb(self, req): response = GotoPoseResponse() response.success = False if len(req.position_left) != 3 or len(req.quat_left) != 4 or len(req.position_right) != 3 \ or len(req.quat_right) != 4: rospy.logerr('one of the position or quat info is incorrect') return response pose_left = '' for p in req.position_left: pose_left += str(p) + ' ' for q in req.quat_left: pose_left += str(q) + ' ' pose_left = message_to_pose(pose_left, 'tool') pose_right = '' for p in req.position_right: pose_right += str(p) + ' ' for q in req.quat_right: pose_right += str(q) + ' ' pose_right = message_to_pose(pose_right, 'tool') ret = self.robot.goto_pose_sync(pose_left, pose_right) response.success = True return response def goto_joints_cb(self, req): response = GotoJointResponse() response.success = False if (req.arm != 'left' and req.arm != 'right') or len(req.joint) != 7: return response joints = '' for j in req.joint: joints += str(j) + ' ' state = message_to_state(joints) if req.arm == 'left': self.left_arm.goto_state(state) else: self.right_arm.goto_state(state) response.success = True return response def close_gripper_cb(self, req): response = YumipyTriggerResponse() response.success = True if req.arm == 'left': self.left_arm.close_gripper() elif req.arm == 'right': self.right_arm.close_gripper() else: rospy.logerr("[Close Gripper] No arm named {}".format(req.arm)) response.success = False response.message = "No arm named {}".format(req.arm) return response response.message = 'Successfully close {} gripper'.format(req.arm) return response def open_gripper_cb(self, req): response = YumipyTriggerResponse() response.success = True if req.arm == 'left': self.left_arm.open_gripper() elif req.arm == 'right': self.right_arm.open_gripper() else: rospy.logerr("[Open Gripper] No arm named {}".format(req.arm)) response.success = False response.message = "No arm named {}".format(req.arm) return response response.message = 'Successfully open {} gripper'.format(req.arm) return response def move_gripper_cb(self, req): response = MoveGripperResponse() response.success = False arm = None if req.arm == 'left': arm = self.left_arm elif req.arm == 'right': arm = self.right_arm if arm is not None: if req.width <= 0: arm.close_gripper() elif req.width >= 0.025: arm.open_gripper() else: arm.move_gripper(req.width) response.success = True return response # def set_speed_cb(self, req): # # TODO write # def set_zone_cb(self, req): zone_value = req.z self.robot.set_z(zone_value) return SetZResponse() def set_tool_cb(self, req): if req.arm == 'left': arm = self.left_arm self.current_tool_l = req.tool if req.arm == 'right': arm = self.right_arm self.current_tool_r = req.tool elif req.arm != 'right' and req.arm != 'left': rospy.logerr("[Set Tool] No arm named {}".format(req.arm)) tcp = self.tools[req.tool] pose = '' for p in tcp: pose += str(p) + ' ' pose_msg = message_to_pose(pose, 'tool') arm.set_tool(pose=pose_msg) return SetToolResponse() def get_gripper_width(self, req): response = YumipyTriggerResponse() response.success = True if req.arm == 'left': width = self.left_arm.get_gripper_width() elif req.arm == 'right': width = self.right_arm.get_gripper_width() else: rospy.logerr("[Get Gripper Width] No arm named {}".format(req.arm)) response.success = False response.message = "No arm named {}".format(req.arm) return response response.message = str(width) return response def reset_home_cb(self, req): response = YumipyTriggerResponse() response.success = True if req.arm == 'left': pose = '364.3 291.6 123.39 0.06213 0.86746 -0.10842 0.48156' pose = message_to_pose(pose, 'tool') ret = self.left_arm.goto_pose_shortest_path(pose, wait_for_res=True, tool='gripper') self.left_arm.reset_home() elif req.arm == 'right': pose = '381 -314.7 136.97 0.05760 -0.84288 -0.11252 -0.52304' pose = message_to_pose(pose, 'tool') ret = self.right_arm.goto_pose_shortest_path(pose, wait_for_res=True, tool='gripper') self.right_arm.reset_home() elif req.arm == 'all': # pose = '364.3 291.6 123.39 0.06213 0.86746 -0.10842 0.48156' # pose = message_to_pose(pose, 'tool') ret = self.left_arm.goto_pose_shortest_path( message_to_pose('364.3 291.6 123.39 0.06213 0.86746 -0.10842 0.48156', 'tool'), wait_for_res=True, tool='gripper') self.left_arm.reset_home() # pose = '381 -314.7 136.97 0.05760 -0.84288 -0.11252 -0.52304' # pose = message_to_pose(pose, 'tool') ret = self.right_arm.goto_pose_shortest_path( message_to_pose('381 -314.7 136.97 0.05760 -0.84288 -0.11252 -0.52304', 'tool'), wait_for_res=True, tool='gripper') self.right_arm.reset_home() else: rospy.logerr("[Reset Home] No arm named {}".format(req.arm)) response.success = False response.message = "No arm named {}".format(req.arm) return response response.message = 'Successfully reset {} arm to home'.format(req.arm) return response def calibrate_gripper_cb(self, req): response = YumipyTriggerResponse() response.success = True if req.arm == 'left': self.left_arm.calibrate_gripper() elif req.arm == 'right': self.right_arm.calibrate_gripper() else: rospy.logerr("[Reset Home] No arm named {}".format(req.arm)) response.success = False response.message = "No arm named {}".format(req.arm) return response response.message = 'Successfully calibrate {} gripper'.format(req.arm) return response def turn_on_suction_cb(self, req): response = YumipyTriggerResponse() response.success = True if req.arm == 'left': self.left_arm.turn_on_suction() elif req.arm == 'right': self.right_arm.turn_on_suction() else: rospy.logerr("[Turn On Suction] No arm named {}".format(req.arm)) response.success = False response.message = "No arm named {}".format(req.arm) return response response.message = 'Successfully Turn on {} suction'.format(req.arm) return response def turn_off_suction_cb(self, req): response = YumipyTriggerResponse() response.success = True if req.arm == 'left': self.left_arm.turn_off_suction() elif req.arm == 'right': self.right_arm.turn_off_suction() else: rospy.logerr("[Turn Off Suction] No arm named {}".format(req.arm)) response.success = False response.message = "No arm named {}".format(req.arm) return response response.message = 'Successfully Turn off {} suction'.format(req.arm) return response def goto_wait_pose_cb(self, req): arms_quat = {'left': [0, 0, -0.9238795, 0.3826834], 'right': [0, 0, 0.9238795, 0.3826834]} arms_wait_pose = {'left': [356, 200, 100], 'right': [356, -200, 100]} pose = '' for p in arms_wait_pose['left']: pose += str(p) + ' ' for q in arms_quat['left']: pose += str(q) + ' ' pose = message_to_pose(pose, 'tool') self.left_arm.goto_pose_shortest_path(pose, tool=self.current_tool_l) pose = '' for p in arms_wait_pose['right']: pose += str(p) + ' ' for q in arms_quat['right']: pose += str(q) + ' ' pose = message_to_pose(pose, 'tool') self.right_arm.goto_pose_shortest_path(pose, tool=self.current_tool_r) return TriggerResponse() def goto_wait_joint_cb(self, req): if req.arm == 'left': left_joint = self.arms_wait_joint['left'] state = "" for j in left_joint: state += str(j*radian2degree) + ' ' state = message_to_state(state) self.left_arm.goto_state(state) elif req.arm == 'right': right_joint = self.arms_wait_joint['right'] state = "" for j in right_joint: state += str(j * radian2degree) + ' ' state = message_to_state(state) self.right_arm.goto_state(state) return YumipyTriggerResponse()