def cartesian(): rospy.init_node("move_to_position") limb = intera_interface.Limb('right') limb.move_to_neutral() cars = [] for i in range(4): print("Enter point %d" % (i + 1)) done = False while not done: c = intera_external_devices.getch() if c in ['\x1b', '\x03']: return None elif c == 'p': done = True current_pose = limb.endpoint_pose() cars.append([current_pose['position'].x, current_pose['position'].y]) print('Point %d: (%.2f, %.2f)' % (i + 1, current_pose['position'].x, current_pose['position'].y)) cars = np.array(cars) X_min = cars[:, 0].min() X_max = cars[:, 0].max() Y_min = cars[:, 1].min() Y_max = cars[:, 1].max() return [[X_min, X_max], [Y_max, Y_min]]
def collect_goal_image(self, ind=0): savedir = self.recording_dir + '/goalimage' if not os.path.exists(savedir): os.makedirs(savedir) done = False print("Press g to take goalimage!") while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: # catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") if c == 'g': print 'taking goalimage' imagemain = self.recorder.ltob.img_cropped cv2.imwrite( savedir+ "/goal_main{}.png".format(ind), imagemain, [cv2.IMWRITE_PNG_STRATEGY_DEFAULT, 1]) state = self.get_endeffector_pos() with open(savedir + '/goalim{}.pkl'.format(ind), 'wb') as f: cPickle.dump({'main': imagemain, 'state': state}, f) break else: print 'wrong key!' print 'place object in different location!' pdb.set_trace()
def move_eef(self): action_size = 0.05 rate = rospy.Rate(100) action = np.array([0.0, 0.0, 0.0]) print('Move the end-effector and press d for done') print('''\r **Ready. Do one of the following. 8 : go forwards in x 2 : go backwards in x 6 : go forwards in y 4 : go backwards in y + : go forwards in z - : go backwards in z h : double speed l : half speed d : finish moving esc : quit ''') while not rospy.is_shutdown(): c = intera_external_devices.getch() if c: if (c in ['8']): action = np.array([1.0, 0.0, 0.0]) elif (c in ['2']): action = np.array([-1.0, 0.0, 0.0]) elif (c in ['6']): action = np.array([0.0, 1.0, 0.0]) elif (c in ['4']): action = np.array([0.0, -1.0, 0.0]) elif (c in ['+']): action = np.array([0.0, 0.0, 1.0]) elif (c in ['-']): action = np.array([0.0, 0.0, -1.0]) elif (c in ['5']): action = np.array([0.0, 0.0, 0.0]) elif (c in ['h']): action = np.array([0.0, 0.0, 0.0]) action_size = 2 * action_size elif (c in ['l']): action = np.array([0.0, 0.0, 0.0]) action_size = action_size / 2 elif c in ['\x1b', '\x03', 'f']: action = np.array([0.0, 0.0, 0.0]) rospy.signal_shutdown("Shutting Down...") elif c in ['d']: rospy.loginfo('Done Moving') rospy.set_param('manual_arm_move', False) print(self._right_arm.endpoint_pose()) return joint_vels = self.get_control(action, max_action=action_size) # Send contgrol to robot self._right_arm.set_joint_velocities(joint_vels) rate.sleep() return
def map_keyboard(head): done = False while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Exiting.") elif c == 'a': head.move_head(np.pi / 180.0) elif c == 'd': head.move_head(-np.pi / 180.0) elif c == 'q': head.move_head(5 * np.pi / 180.0) elif c == 'e': head.move_head(-5 * np.pi / 180.0) elif c == 'r': head.set_head(0.0) else: print_bindings()
def main(): print('Get marker pixel position ...') pixel_markers = pixel() print('Marker pixel = ', pixel_markers) print('Get marker catersian position ...') cartesian_markers = cartesian() print('Marker cartesian = ', cartesian_markers) if cartesian_markers: print("Save calib info? [y/n]") done = False while not done: c = intera_external_devices.getch() if c == 'n': done = True elif c == 'y': calib_info = {'pixel': pixel_markers, 'cartesian': cartesian_markers} with open(CALIB_INFO, 'w') as f: json.dump(calib_info, f) done = True print('Saved calib info at ' + CALIB_INFO)
def map_keyboard(side): limb = intera_interface.Limb(side) try: gripper = intera_interface.Gripper(side) except: has_gripper = False rospy.logerr("Could not initalize the gripper.") else: has_gripper = True joints = limb.joint_names() def set_j(limb, joint_name, delta): current_position = limb.joint_angle(joint_name) joint_command = {joint_name: current_position + delta} limb.set_joint_positions(joint_command) def set_g(action): if has_gripper: if action == "close": gripper.close() elif action == "open": gripper.open() elif action == "calibrate": gripper.calibrate() bindings = { '1': (set_j, [limb, joints[0], 0.1], joints[0] + " increase"), 'q': (set_j, [limb, joints[0], -0.1], joints[0] + " decrease"), '2': (set_j, [limb, joints[1], 0.1], joints[1] + " increase"), 'w': (set_j, [limb, joints[1], -0.1], joints[1] + " decrease"), '3': (set_j, [limb, joints[2], 0.1], joints[2] + " increase"), 'e': (set_j, [limb, joints[2], -0.1], joints[2] + " decrease"), '4': (set_j, [limb, joints[3], 0.1], joints[3] + " increase"), 'r': (set_j, [limb, joints[3], -0.1], joints[3] + " decrease"), '5': (set_j, [limb, joints[4], 0.1], joints[4] + " increase"), 't': (set_j, [limb, joints[4], -0.1], joints[4] + " decrease"), '6': (set_j, [limb, joints[5], 0.1], joints[5] + " increase"), 'y': (set_j, [limb, joints[5], -0.1], joints[5] + " decrease"), '7': (set_j, [limb, joints[6], 0.1], joints[6] + " increase"), 'u': (set_j, [limb, joints[6], -0.1], joints[6] + " decrease"), '8': (set_g, "close", side + " gripper close"), 'i': (set_g, "open", side + " gripper open"), '9': (set_g, "calibrate", side + " gripper calibrate") } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] if c == '8' or c == 'i' or c == '9': cmd[0](cmd[1]) print("command: %s" % (cmd[2], )) else: #expand binding to something like "set_j(right, 'j0', 0.1)" cmd[0](*cmd[1]) print("command: %s" % (cmd[2], )) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def main(): parser = argparse.ArgumentParser() parser.add_argument( "-d", "--log_dir", type=str, default='../logs', help="Path where the trajectories produced are stored") parser.add_argument( "-l", "--load_dir", type=str, default='../../assets/rl', help="Path where the policy networks are loaded") parser.add_argument( "-m", "--method_idx", type=int, default=0, help="The index of the method to use") parser.add_argument( "-r", "--randomisation_idx", type=int, default=0, help="The index of the randomisation regime to use") args = parser.parse_args(rospy.myargv()[1:]) log_dir = args.log_dir load_dir = args.load_dir method_idx = args.method_idx randomisation_idx = args.randomisation_idx rospy.init_node('reach_control', anonymous=True) # Parameters horizon = 50 repeats = 0 goal_no = 1 action_dim=3 internal_state_dim=14 internal_action_dim=7 params_dim=49 env_name='SawyerReach' # Setting up Sawyer print('Setting up the robot...') Right_arm = intera_interface.limb.Limb("right") print("Getting robot state... ") _rs = intera_interface.RobotEnable(CHECK_VERSION) _init_state = _rs.state().enabled print("Enabling robot... ") _rs.enable() print("Robot ready.") # Setup controller and observation maker observation_generator = Observation(Right_arm, reset_goal_pos= True, predefined_goal='goal_{}'.format(goal_no)) controller = Control(Right_arm) while not rospy.is_shutdown(): state_dim = 6 ##### SETTING UP THE POLICY ######### method = ['Single', 'LSTM', 'EPI', 'UPOSI'][method_idx] if(method == 'Single'): alg_idx = 1 elif(method == 'LSTM'): alg_idx = 2 elif(method == 'UPOSI'): alg_idx = 3 osi_l = 5 CAT_INTERNAL = True if CAT_INTERNAL: osi_input_dim = osi_l*(state_dim+action_dim+internal_state_dim+internal_action_dim) else: osi_input_dim = osi_l * (state_dim + action_dim) state_dim+=params_dim elif(method == 'EPI'): alg_idx = 4 embed_dim = 10 traj_l = 10 NO_RESET = True embed_input_dim = traj_l*(state_dim+action_dim) ori_state_dim = state_dim state_dim += embed_dim else: continue alg_name = ['sac', 'td3', 'lstm_td3', 'uposi_td3', 'epi_td3'][alg_idx] randomisation_type = ['reach_no-randomisation', 'reach_full-randomisation', \ 'reach_force-randomisation', 'reach_force-&-noise-randomisation'][randomisation_idx] number_random_params = [0, 14, 1, 3][randomisation_idx] folder_path = load_dir +'/' +method + '/' + alg_name + '/model/' path = folder_path + env_name + str( number_random_params) + '_' + alg_name policy = load(path=path, alg=alg_name, state_dim=state_dim, action_dim=action_dim) if method == 'UPOSI': osi_model = load_model(model_name='osi', path=path, input_dim = osi_input_dim, output_dim=params_dim ) elif method == 'EPI': embed_model = load_model(model_name='embedding', path=path, input_dim = embed_input_dim, output_dim = embed_dim ) embed_model.cuda() epi_policy_path = folder_path + env_name + str(number_random_params) + '_' + 'epi_ppo_epi_policy' epi_policy = load(path=epi_policy_path, alg='ppo', state_dim=ori_state_dim, action_dim=action_dim ) if (alg_name == 'lstm_td3'): # Initialize hidden state for lstm, (hidden, cell), each is (layer, batch, dim) hidden_out = (torch.zeros([1, 1, 512], dtype=torch.float).cuda(), \ torch.zeros([1, 1, 512], dtype=torch.float).cuda()) last_action = np.array([0, 0, 0]) ###################################### log_save_name = '{}_{}_{}_{}'.format(method, alg_name, randomisation_type, number_random_params) done = False while not done and not rospy.is_shutdown(): if (observation_generator.goal_pos_in_base is not None ): sys.stdout.write('\r **Observations ready**.') sys.stdout.flush() done = True else: sys.stdout.write('\rWaiting for observation initialisation...') sys.stdout.flush() controller.go_to_start() done = False while not done and not rospy.is_shutdown(): sys.stdout.write('\r **Ready. Press any key to start, f to quit**.') sys.stdout.flush() c = intera_external_devices.getch() if c in ['\x1b', '\x03', 'f']: rospy.signal_shutdown("Shutting Down...") return elif c: done = True #Initialise the trajectory logger log_path = '{}/reaching/{}/goal_{}/trajectory_log_{}.csv'.format(log_dir,log_save_name,goal_no,repeats) log_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), log_path) log_list = ["step", "time", "cmd_eef_vx", "cmd_eef_vy", "cmd_eef_vz", "eef_x", "eef_y", "eef_z", "eef_vx", "eef_vy", "eef_vz", "goal_x","goal_y","goal_z", "obs_0", "obs_1", "obs_2", "obs_3", "obs_4", "obs_5" ] logger = Logger(log_list, log_path) # Set control rate rate = rospy.Rate(10) # Time and iteration count i = 0 start = rospy.Time.now() if (alg_name == 'uposi_td3') or (alg_name == 'epi_td3'): epi_traj = [] while not rospy.is_shutdown(): # Grab observations elapsed = rospy.Time.now() - start observation = observation_generator.get_observation() c_observation = observation_generator.get_characterisation_observation() ##### Pre-action choosing operations if (alg_name == 'uposi_td3'): if len(epi_traj)>=osi_l: osi_input = stack_data(epi_traj, osi_l) pre_params = osi_model(osi_input).detach().numpy() else: zero_osi_input = np.zeros(osi_input_dim) pre_params = osi_model(zero_osi_input).detach().numpy() params_state = np.concatenate((pre_params, observation)) elif (alg_name == 'epi_td3'): # epi rollout first for each episode; if len(epi_traj) < traj_l: a = epi_policy.get_action(observation) action = np.clip(a, -epi_policy.action_range, epi_policy.action_range) s_a_r = np.concatenate((observation, action, [0])) # state, action, reward; no reward in reality epi_traj.append(s_a_r) if(len(epi_traj)>= traj_l): state_action_in_traj = np.array(epi_traj)[:, :-1] # remove the rewards embedding = embed_model(state_action_in_traj.reshape(-1)) embedding = embedding.detach().cpu().numpy() if NO_RESET: observation = observation # last observation else: # reset the env here and get new observation rospy.signal_shutdown('Cannot reset the environment in the real world') sys.exit(1) observation=np.concatenate((observation, embedding)) ############### #### CHOOSING THE ACTION ##### if (alg_name == 'lstm_td3'): hidden_in = hidden_out action, hidden_out=policy.get_action(observation, last_action, hidden_in, noise_scale=0.0) last_action = action elif (alg_name == 'uposi_td3'): # using internal state or not if CAT_INTERNAL: internal_state = np.concatenate((controller.get_joints_angle(), np.array(controller.get_joints_vel()))) full_state = np.concatenate([observation, internal_state]) else: full_state = observation action = policy.get_action(params_state, noise_scale=0.0) elif (alg_name == 'epi_td3') and len(epi_traj) < traj_l: # action already given by EPI policy pass else: action = policy.get_action(observation, noise_scale=0.0) ############################### if (np.isnan(action).any()): rospy.signal_shutdown('NAN in action') sys.exit(1) # joint_vels is the joint velocity COMMAND - the current joint velocities can be queried through controller.joint_vels joint_vels = controller.get_control(action) controller._right_arm.set_joint_velocities(joint_vels) if (alg_name == 'uposi_td3'): if CAT_INTERNAL: target_joint_action = controller.joint_array_from_joint_dict(joint_vels) print(action.shape)# print(target_joint_action.shape) full_action = np.concatenate([action, target_joint_action]) else: full_action = action epi_traj.append(np.concatenate((full_state, full_action))) ## Format the observations for logging and log elapsed_sec = elapsed.to_sec() eef_pos_in_base = c_observation['eef_pos_in_base'] eef_vel_in_base = c_observation['eef_vel_in_base'] goal_pos_in_base = c_observation['goal_pos_in_base'] action_in_base = controller.base_rot_in_eef.dot(action) logger.log(i, elapsed_sec, action_in_base[0], action_in_base[1], action_in_base[2], eef_pos_in_base[0], eef_pos_in_base[1], eef_pos_in_base[2], eef_vel_in_base[0], eef_vel_in_base[1], eef_vel_in_base[2], goal_pos_in_base[0], goal_pos_in_base[1], goal_pos_in_base[2], observation[0], observation[1], observation[2], observation[3], observation[4], observation[5] ) i += 1 if (i >= horizon): break rate.sleep() controller.set_neutral() done = False def _finish_message(): print('''Episode finished, press: f or esc to quit r to restart g to change the goal position''') _finish_message() while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: if c in ['\x1b', '\x03', 'f']: done = True rospy.signal_shutdown("Shutting Down...") elif c in ['g']: repeats = 0 goal_no +=1 observation_generator.reset(specific_goal ='goal_{}'.format(goal_no)) done = True elif c in ['r']: repeats+=1 done = True else: _finish_message()
def main(): parser = argparse.ArgumentParser() parser.add_argument( "-p", "--log_dir", type=str, default='../logs/manual_slide', help= "Path to the parent directory where the produced trajectories are stored" ) args = parser.parse_args(rospy.myargv()[1:]) log_dir = args.log_path rospy.init_node('reach_characterisation', anonymous=True) default_value = 0.1 a = default_value repeats = 0 # Setting up Sawyer print('Setting up the robot...') Right_arm = intera_interface.limb.Limb("right") print("Getting robot state... ") _rs = intera_interface.RobotEnable(CHECK_VERSION) _init_state = _rs.state().enabled print("Enabling robot... ") _rs.enable() print("Robot ready.") Right_arm.set_joint_position_speed(speed=0.1) # Setup controller and observation maker observation_generator = Observation(Right_arm) controller = Control(Right_arm) while not rospy.is_shutdown(): done = False while not done and not rospy.is_shutdown(): if (observation_generator.operational is not None): sys.stdout.write('\r **Observations ready**.') sys.stdout.flush() done = True else: sys.stdout.write('\rWaiting for observation initialisation...') sys.stdout.flush() controller.go_to_start() done = False while not done and not rospy.is_shutdown(): sys.stdout.write( '\r **Ready. Press any key to start, f to quit**.') sys.stdout.flush() c = intera_external_devices.getch() if c in ['\x1b', '\x03', 'f']: rospy.signal_shutdown("Shutting Down...") return elif c: done = True log_path = '{}/goal1/trajectory_log_{}.csv'.format(log_dir, repeats) log_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), log_path) log_list = [ "step", "time", "cmd_j5", "cmd_j6", "obj_x", "obj_y", "obj_z", "sin_z", "cos_z", "obj_vx", "obj_vy", "obj_vz", "z_angle", "obj_orn_x", "obj_orn_y", "obj_orn_z", "obj_orn_w", "rot_1", "rot_2", "rot_3", "rot_4", "rot_5", "rot_6", "rot_7", "rot_8", "rot_9", "fallen_object", "a_j5", "a_j6", "v_j5", "v_j6", ] logger = Logger(log_list, log_path) # Set control rate rate = rospy.Rate(10) # Time and iteration count i = 0 start = rospy.Time.now() while not rospy.is_shutdown(): # Grab observations elapsed = rospy.Time.now() - start observation = observation_generator.get_c_observation() # Set the action action = [a, a] joint_vels = controller.get_control(action) controller._right_arm.set_joint_velocities(joint_vels) ## Format the observations for logging and log elapsed_sec = elapsed.to_sec() logger.log(i, elapsed_sec, action[0], action[1], observation[0], observation[1], observation[2], observation[3], observation[4], observation[5], observation[6], observation[7], observation[8], observation[9], observation[10], observation[11], observation[12], observation[13], observation[14], observation[15], observation[16], observation[17], observation[18], observation[19], observation[20], observation[21], observation[22], observation[23], observation[24], observation[25], observation[26]) if (elapsed_sec > 2.): break i += 1 rate.sleep() done = False def _finish_message(): print('''Episode finished, press: f or esc to quit r to restart g to change the goal position''') _finish_message() while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: if c in ['\x1b', '\x03', 'f']: done = True rospy.signal_shutdown("Shutting Down...") controller.set_slide_neutral() elif c in ['r']: controller.set_slide_neutral() repeats += 1 done = True else: _finish_message()
def map_keyboard(limb): # initialize interfaces print("Getting robot state...") rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state() gripper = None original_deadzone = None def clean_shutdown(): if gripper and original_deadzone: gripper.set_dead_zone(original_deadzone) print("Exiting example.") try: gripper = intera_interface.Gripper(limb + '_gripper') except (ValueError, OSError) as e: rospy.logerr( "Could not detect an electric gripper attached to the robot.") clean_shutdown() return rospy.on_shutdown(clean_shutdown) def offset_position(offset_pos): cmd_pos = max( min(gripper.get_position() + offset_pos, gripper.MAX_POSITION), gripper.MIN_POSITION) gripper.set_position(cmd_pos) print(("commanded position set to {0} m".format(cmd_pos))) def update_velocity(offset_vel): cmd_speed = max( min(gripper.get_cmd_velocity() + offset_vel, gripper.MAX_VELOCITY), gripper.MIN_VELOCITY) gripper.set_cmd_velocity(cmd_speed) print(("commanded velocity set to {0} m/s".format(cmd_speed))) original_deadzone = gripper.get_dead_zone() # WARNING: setting the deadzone below this can cause oscillations in # the gripper position. However, setting the deadzone to this # value is required to achieve the incremental commands in this example gripper.set_dead_zone(0.001) rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone())) num_steps = 8.0 percent_delta = 1.0 / num_steps velocity_increment = (gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) * percent_delta position_increment = (gripper.MAX_POSITION - gripper.MIN_POSITION) * percent_delta bindings = { # key: (function, args, description) 'r': (gripper.reboot, [], "reboot"), 'c': (gripper.calibrate, [], "calibrate"), 'q': (gripper.close, [], "close"), 'o': (gripper.open, [], "open"), '+': (update_velocity, [velocity_increment], "increase velocity by {0}%".format(percent_delta * 100)), '-': (update_velocity, [-velocity_increment], "decrease velocity by {0}%".format(percent_delta * 100)), 's': (gripper.stop, [], "stop"), 'u': (offset_position, [-position_increment], "decrease position by {0}%".format(percent_delta * 100)), 'i': (offset_position, [position_increment], "increase position by {0}%".format(percent_delta * 100)), } done = False rospy.loginfo("Enabling robot...") rs.enable() print("Controlling grippers. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True elif c in bindings: cmd = bindings[c] print(("command: {0}".format(cmd[2]))) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(list(bindings.items()), key=lambda x: x[1][2]): print((" %s: %s" % (key, val[2]))) # force shutdown call if caught by key handler rospy.signal_shutdown("Example finished.")
def map_keyboard(side, redisInstance): min_q = [-3.0503, -3.8095, -3.0426, -3.0439, -2.9761, -2.9761, -4.7124] max_q = [3.0503, 2.2736, 3.0426, 3.0439, 2.9761, 2.9761, 4.7124] home_q = [ -0.11243359375, -1.15601367188, -0.00805859375, 2.16251171875, 0.0014658203125, 0.47171875, 3.31875488281 ] float_q = home_q limb = intera_interface.Limb(side) joints = limb.joint_names() def set_j(limb, joints, q): joint_command = { joints[0]: q[0], joints[1]: q[1], joints[2]: q[2], joints[3]: q[3], joints[4]: q[4], joints[5]: q[5], joints[6]: q[6] } limb.set_joint_positions(joint_command) done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): # Read q from REDIS flag = False redis_angles = redisInstance.get('cs225a::robot::sawyer::sensors::q') if redis_angles is not None: # Check if read from redis correctly q = redis_angles.split(" ") if len(q) is 7: # Check if 7 forces read for i in range(0, 7): angle_float = float(q[i]) if math.isnan(angle_float) or angle_float < min_q[ i] or angle_float > max_q[i]: flag = True else: float_q[i] = angle_float else: flag = True else: flag = True if flag: q = home_q # Write q to Robot set_j(limb, joints, float_q) print q # Check for escape characters c = intera_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.")
def map_keyboard(side): limb = intera_interface.Limb(side) print(limb.tip_state("right_hand_camera")) try: gripper = intera_interface.Gripper(side + '_gripper') except: has_gripper = False rospy.loginfo("The electric gripper is not detected on the robot.") else: has_gripper = True joints = limb.joint_names() increment = 0.1 def set_j(limb, joint_name, delta): current_position = limb.joint_angle(joint_name) joint_command = {joint_name: current_position + delta} limb.set_joint_positions(joint_command) def set_g(action): if has_gripper: if action == "close": gripper.close() elif action == "open": gripper.open() elif action == "calibrate": gripper.calibrate() def set_bindings(): global bindings bindings = { '1': (set_j, [limb, joints[0], increment], joints[0] + " increase"), 'q': (set_j, [limb, joints[0], -increment], joints[0] + " decrease"), '2': (set_j, [limb, joints[1], increment], joints[1] + " increase"), 'w': (set_j, [limb, joints[1], -increment], joints[1] + " decrease"), '3': (set_j, [limb, joints[2], increment], joints[2] + " increase"), 'e': (set_j, [limb, joints[2], -increment], joints[2] + " decrease"), '4': (set_j, [limb, joints[3], increment], joints[3] + " increase"), 'r': (set_j, [limb, joints[3], -increment], joints[3] + " decrease"), '5': (set_j, [limb, joints[4], increment], joints[4] + " increase"), 't': (set_j, [limb, joints[4], -increment], joints[4] + " decrease"), '6': (set_j, [limb, joints[5], increment], joints[5] + " increase"), 'y': (set_j, [limb, joints[5], -increment], joints[5] + " decrease"), '7': (set_j, [limb, joints[6], increment], joints[6] + " increase"), 'u': (set_j, [limb, joints[6], -increment], joints[6] + " decrease") } if has_gripper: bindings.update({ '8': (set_g, "close", side + " gripper close"), 'i': (set_g, "open", side + " gripper open"), '9': (set_g, "calibrate", side + " gripper calibrate") }) set_bindings() done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() #global bindings if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] if c == '8' or c == 'i' or c == '9': cmd[0](cmd[1]) print("command: %s" % (cmd[2], )) else: #expand binding to something like "set_j(right, 'j0', 0.1)" cmd[0](*cmd[1]) print("command: %s" % (cmd[2], )) elif c == 'n': increment_val = "Increment value (0.0 : 0.4) : " increment = constrain(float(raw_input(increment_val).lower()), 0.0, 0.4) set_bindings() else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") print(" n: Change increment value") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def map_keyboard(side): limb = intera_interface.Limb(side) tip_name = 'right_hand' try: gripper = intera_interface.Gripper(side + '_gripper') except: has_gripper = False rospy.loginfo("The electric gripper is not detected on the robot.") else: has_gripper = True joints = limb.joint_names() def set_j(limb, joint_name, delta): current_position = limb.joint_angle(joint_name) joint_command = {joint_name: current_position + delta} limb.set_joint_positions(joint_command) def set_l(limb, cartesian_axis, distance): # traj_options = TrajectoryOptions() # traj_options.interpolation_type = 'CARTESIAN' # traj = MotionTrajectory(trajectory_options = traj_options, limb = limb) # wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.2, # max_joint_accel=0.2) # waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb) endpoint_state = limb.tip_state(tip_name) if endpoint_state is None: rospy.logerr('Endpoint state not found with tip name %s', tip_name) return None pose = endpoint_state.pose # print ('*********') # print (pose.position) if cartesian_axis == 'x': pose.position.x += distance elif cartesian_axis == 'y': pose.position.y += distance elif cartesian_axis == 'z': pose.position.z += distance # poseStamped = PoseStamped() # poseStamped.pose = pose # print (pose.position) print('*********') joints_command = limb.ik_request(pose) limb.set_joint_positions(joints_command) print(limb.has_collided()) print("*****") # print ("*****") # joint_angles = limb.joint_ordered_angles() # waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles) # traj.append_waypoint(waypoint.to_msg()) # result = traj.send_trajectory() # if result is None: # rospy.logerr('Trajectory FAILED to send') # # if result.result: # rospy.loginfo('Motion controller successfully finished the trajectory!') # else: # rospy.logerr('Motion controller failed to complete the trajectory with error %s', # result.errorId) # traj_options.interpolation_type = 'JOINT' # joint_angles = limb.joint_ordered_angles() # waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles) # traj.append_waypoint(waypoint.to_msg()) # traj.send_trajectory() def set_g(action): if has_gripper: if action == "close": gripper.close() elif action == "open": gripper.open() elif action == "calibrate": gripper.calibrate() bindings = { '1': (set_j, [limb, joints[0], 0.1], joints[0] + " increase"), 'q': (set_j, [limb, joints[0], -0.1], joints[0] + " decrease"), '2': (set_j, [limb, joints[1], 0.1], joints[1] + " increase"), 'w': (set_j, [limb, joints[1], -0.1], joints[1] + " decrease"), '3': (set_j, [limb, joints[2], 0.1], joints[2] + " increase"), 'e': (set_j, [limb, joints[2], -0.1], joints[2] + " decrease"), '4': (set_j, [limb, joints[3], 0.1], joints[3] + " increase"), 'r': (set_j, [limb, joints[3], -0.1], joints[3] + " decrease"), '5': (set_j, [limb, joints[4], 0.1], joints[4] + " increase"), 't': (set_j, [limb, joints[4], -0.1], joints[4] + " decrease"), '6': (set_j, [limb, joints[5], 0.1], joints[5] + " increase"), 'y': (set_j, [limb, joints[5], -0.1], joints[5] + " decrease"), '7': (set_j, [limb, joints[6], 0.1], joints[6] + " increase"), 'u': (set_j, [limb, joints[6], -0.1], joints[6] + " decrease"), 'a': (set_l, [limb, "x", +0.01], " x" + " increase"), 'z': (set_l, [limb, "x", -0.01], " x" + " decrease"), 's': (set_l, [limb, "y", +0.01], " y" + " increase"), 'x': (set_l, [limb, "y", -0.01], " y" + " decrease"), 'd': (set_l, [limb, "z", +0.01], " z" + " increase"), 'c': (set_l, [limb, "z", -0.01], " z" + " decrease"), } if has_gripper: bindings.update({ '8': (set_g, "close", side + " gripper close"), 'i': (set_g, "open", side + " gripper open"), '9': (set_g, "calibrate", side + " gripper calibrate") }) done = False print("Controlling joints. Press ? for help, Esc to quit.") time_tamp = None time_label = None while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c == '\\': set_speed = input('请输入设定的速度,按下Enter确认。\n') limb.set_joint_position_speed(set_speed) elif c in bindings: cmd = bindings[c] if c == '8' or c == 'i' or c == '9': cmd[0](cmd[1]) print("command: %s" % (cmd[2], )) else: #expand binding to something like "set_j(right, 'j0', 0.1)" cmd[0](*cmd[1]) print("command: %s" % (cmd[2], )) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) time_tamp = rospy.get_time() else: if (time_tamp is not None): if time_tamp != time_label: if (rospy.get_time() - time_tamp) > 1: print('当前姿态:') print(limb.tip_state(tip_name).pose) # print (limb.tip_state(tip_name).pose.position) # print (limb.tip_state(tip_name).pose.quaternion) # print (limb.endpoint_pose()) # print (limb.endpoint_pose()['position']) # print (limb.endpoint_pose()['orientation']) print('当前关节角:') print(limb.joint_angles()) print('\n') time_label = time_tamp
def map_keyboard(side): limb = intera_interface.Limb(side) try: gripper = intera_interface.Gripper(side + '_gripper') except: has_gripper = False rospy.loginfo("The electric gripper is not detected on the robot.") else: has_gripper = True joints = limb.joint_names() def set_j(limb, joint_name, delta): current_position = limb.joint_angle(joint_name) joint_command = {joint_name: current_position + delta} limb.set_joint_positions(joint_command) def set_g(action): if has_gripper: if action == "close": gripper.close() elif action == "open": gripper.open() elif action == "calibrate": gripper.calibrate() bindings = { '1': (set_j, [limb, joints[0], 0.1], joints[0] + " increase"), 'q': (set_j, [limb, joints[0], -0.1], joints[0] + " decrease"), '2': (set_j, [limb, joints[1], 0.1], joints[1] + " increase"), 'w': (set_j, [limb, joints[1], -0.1], joints[1] + " decrease"), '3': (set_j, [limb, joints[2], 0.1], joints[2] + " increase"), 'e': (set_j, [limb, joints[2], -0.1], joints[2] + " decrease"), '4': (set_j, [limb, joints[3], 0.1], joints[3] + " increase"), 'r': (set_j, [limb, joints[3], -0.1], joints[3] + " decrease"), '5': (set_j, [limb, joints[4], 0.1], joints[4] + " increase"), 't': (set_j, [limb, joints[4], -0.1], joints[4] + " decrease"), '6': (set_j, [limb, joints[5], 0.1], joints[5] + " increase"), 'y': (set_j, [limb, joints[5], -0.1], joints[5] + " decrease"), '7': (set_j, [limb, joints[6], 0.1], joints[6] + " increase"), 'u': (set_j, [limb, joints[6], -0.1], joints[6] + " decrease") } if has_gripper: bindings.update({ '8': (set_g, "close", side + " gripper close"), 'i': (set_g, "open", side + " gripper open"), '9': (set_g, "calibrate", side + " gripper calibrate") }) #user set joint angles theta = {} theta['right_s0'] = np.float32(raw_input("Enter joint1: ")) theta['right_s1'] = np.float32(raw_input("Enter joint2: ")) theta['right_e0'] = np.float32(raw_input("Enter joint3: ")) theta['right_e1'] = np.float32(raw_input("Enter joint4: ")) theta['right_w0'] = np.float32(raw_input("Enter joint5: ")) theta['right_w1'] = np.float32(raw_input("Enter joint6: ")) theta['right_w2'] = np.float32(raw_input("Enter joint7: ")) while (limb.joint_angle(joints[0]) - theta['right_s0'] < 0.1 or limb.joint_angle(joints[1]) - theta['right_s1'] < 0.1 or limb.joint_angle(joints[2]) - theta['right_e0'] < 0.1 or limb.joint_angle(joints[3]) - theta['right_e1'] < 0.1 or limb.joint_angle(joints[4]) - theta['right_w0'] < 0.1 or limb.joint_angle(joints[5]) - theta['right_w1'] < 0.1 or limb.joint_angle(joints[6]) - theta['right_w2'] < 0.1): limb.set_joint_positions(theta) done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] if c == '8' or c == 'i' or c == '9': cmd[0](cmd[1]) print("command: %s" % (cmd[2], )) else: #expand binding to something like "set_j(right, 'j0', 0.1)" cmd[0](*cmd[1]) print("command: %s" % (cmd[2], )) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def main(): parser = argparse.ArgumentParser() parser.add_argument( "-p", "--log_dir", type=str, default='../logs/test/manual_push', help="Path to the parent directory where the produced trajectories are stored") args = parser.parse_args(rospy.myargv()[1:]) log_dir = args.log_path rospy.init_node('push_control', anonymous=True) default_value = 0.1 a = default_value repeats = 0 goal_no = 1 # Reset publisher for the topic resetting the observations (used in order to change goals) reset_publisher = rospy.Publisher('pushing/reset', Bool, queue_size=30) print('Setting up the robot...') # Controller controller = Control() while not rospy.is_shutdown(): # Only continue with the script if the observations are ready done = False while not done and not rospy.is_shutdown(): observation, c_observation = controller.get_observations() if (observation is not None and c_observation is not None): sys.stdout.write('\r **Observations ready**.') sys.stdout.flush() done = True else: sys.stdout.write('\rWaiting for observation initialisation...') sys.stdout.flush() controller.go_to_start() # Ask the user when to start the control loop done = False while not done and not rospy.is_shutdown(): sys.stdout.write('\r **Ready. Press any key to start, f to quit**.') sys.stdout.flush() c = intera_external_devices.getch() if c in ['\x1b', '\x03', 'f']: rospy.signal_shutdown("Shutting Down...") return elif c: done = True log_path = '{}/goal{}/trajectory_log_{}.csv'.format(log_dir, goal_no,repeats) log_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), log_path) log_list = ["step", "time", "cmd_eef_vx", "cmd_eef_vy", "goal_x", "goal_y", "goal_z", "eef_x", "eef_y", "eef_z", "eef_vx", "eef_vy", "eef_vz", "object_x", "object_y", "object_z", "object_vx", "object_vy", "object_vz", "z_angle", "obs_0", "obs_1", "obs_2", "obs_3", "obs_4", "obs_5", "obs_6", "obs_7", "obs_8", "obs_9", ] logger = Logger(log_list, log_path) # Set control rate rate = rospy.Rate(10) # Time and iteration count i = 0 start = rospy.Time.now() while not rospy.is_shutdown(): # Record the time and the current observations elapsed = rospy.Time.now() - start observation, c_observation = controller.get_observations() # Set the action action = [a, 0.0] joint_vels = controller.get_control(action) controller._right_arm.set_joint_velocities(joint_vels) # Log observations elapsed_sec = elapsed.to_sec() action_in_base = controller.base_rot_in_eef.dot(np.concatenate([action, [0.0]]))[:2] goal_pos_in_base, eef_pos_in_base, object_pos_in_base, \ eef_vel_in_base, object_vel_in_base, z_angle = _convert_message_to_array(controller, c_observation) logger.log(i, elapsed_sec, action_in_base[0], action_in_base[1], goal_pos_in_base[0], goal_pos_in_base[1], goal_pos_in_base[2], eef_pos_in_base[0], eef_pos_in_base[1], eef_pos_in_base[2], eef_vel_in_base[0], eef_vel_in_base[1], eef_vel_in_base[2], object_pos_in_base[0], object_pos_in_base[1], object_pos_in_base[2], object_vel_in_base[0], object_vel_in_base[1], object_vel_in_base[2], z_angle, observation[0], observation[1], observation[2], observation[3], observation[4], observation[5], observation[6], observation[7], observation[8], observation[9] ) ## Check that trajectory not stopped c = intera_external_devices.getch() if c in ['\x1b', '\x03']: rospy.signal_shutdown("Shutting Down...") # Change the action if above 2.5 sec if (elapsed_sec >2.5): a = 0.0 if(elapsed_sec > 3.0): a = default_value break i += 1 rate.sleep() controller.set_neutral() done = False print('Episode finished, press f or esc to quit or r to restart, q to change goal position') while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c in ['\x1b', '\x03', 'f']: done = True rospy.signal_shutdown("Shutting Down...") elif c in ['r']: repeats += 1 done = True elif c in ['q']: # publish reset reset_message = Bool() reset_message.data = True reset_publisher.publish(reset_message) # controller reset repeats = 0 goal_no += 1 controller.reset() done = True
def map_keyboard(limb): # initialize interfaces print("Getting robot state...") rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state() try: gripper = intera_interface.Gripper(limb) except ValueError: rospy.logerr("Could not detect a gripper attached to the robot.") return def clean_shutdown(): print("Exiting example.") rospy.on_shutdown(clean_shutdown) def offset_position(offset): current = gripper.get_position() gripper.set_position(current + offset) def offset_holding(offset): current = gripper.get_force() gripper.set_holding_force(current + offset) num_steps = 10.0 thirty_percent_velocity = 0.3 * ( gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) + gripper.MIN_VELOCITY bindings = { # key: (function, args, description) 'r': (gripper.reboot, [], "reboot"), 'c': (gripper.calibrate, [], "calibrate"), 'q': (gripper.close, [], "close"), 'o': (gripper.open, [], "open"), '+': (gripper.set_velocity, [gripper.MAX_VELOCITY], "set 100% velocity"), '-': (gripper.set_velocity, [thirty_percent_velocity], "set 30% velocity"), 's': (gripper.stop, [], "stop"), 'h': (offset_holding, [-(gripper.MAX_FORCE / num_steps)], "decrease holding force"), 'j': (offset_holding, [gripper.MAX_FORCE / num_steps], "increase holding force"), 'u': (offset_position, [-(gripper.MAX_POSITION / num_steps)], "decrease position"), 'i': (offset_position, [gripper.MAX_POSITION / num_steps], "increase position"), } done = False rospy.loginfo("Enabling robot...") rs.enable() print("Controlling grippers. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True elif c in bindings: cmd = bindings[c] cmd[0](*cmd[1]) print("command: %s" % (cmd[2], )) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) # force shutdown call if caught by key handler rospy.signal_shutdown("Example finished.")
def main(): parser = argparse.ArgumentParser() parser.add_argument( "-p", "--log_path", type=str, default='../logs/internal_dynamics_calibration/trajectories.pckl', help="Path where the trajectories produced are stored") args = parser.parse_args(rospy.myargv()[1:]) log_path = args.log_path rospy.init_node('calibrate_internal_dynamics', anonymous=True) # Controller controller = Control() done = False print('Ready. Press any key to start.') while not done and not rospy.is_shutdown(): c = intera_external_devices.getch() if c: done = True # Set control rate rate = rospy.Rate(10) trajectories_gathered = [] # Time and iteration count for policy_id, policy_params in enumerate(_POLICIES): if (rospy.is_shutdown()): break trajectories_gathered.append([]) controller.set_neutral() time.sleep(2) controller.set_policy_type(policy_params['policy_type']) controller.set_period_amplitude(policy_params['period'], policy_params['amplitude']) controller.set_line_policy(policy_params['line_policy']) steps = policy_params['steps'] start = rospy.Time.now() for i in range(steps): if (rospy.is_shutdown()): break elapsed = rospy.Time.now() - start # Get observation eef_pose_in_base = controller._right_arm.endpoint_pose() eef_pos_in_base = np.array([ eef_pose_in_base['position'].x, eef_pose_in_base['position'].y, eef_pose_in_base['position'].z, ]) eef_quat_in_base = np.array([ eef_pose_in_base['orientation'].x, eef_pose_in_base['orientation'].y, eef_pose_in_base['orientation'].z, eef_pose_in_base['orientation'].w, ]) eef_rot_in_base = T.quat2mat(eef_quat_in_base) eef_vel_in_eef = controller._right_arm.endpoint_velocity() eef_vel_in_eef = np.array([ eef_vel_in_eef['linear'].x, eef_vel_in_eef['linear'].y, eef_vel_in_eef['linear'].z, ]) eef_vel_in_base = eef_rot_in_base.dot(eef_vel_in_eef) obs_joint_pos = controller._right_arm.joint_angles() obs_joint_pos = np.array([ obs_joint_pos['right_j0'], obs_joint_pos['right_j1'], obs_joint_pos['right_j2'], obs_joint_pos['right_j3'], obs_joint_pos['right_j4'], obs_joint_pos['right_j5'], obs_joint_pos['right_j6'] ]) obs_joint_vels = controller._right_arm.joint_velocities() obs_joint_vels = np.array([ obs_joint_vels['right_j0'], obs_joint_vels['right_j1'], obs_joint_vels['right_j2'], obs_joint_vels['right_j3'], obs_joint_vels['right_j4'], obs_joint_vels['right_j5'], obs_joint_vels['right_j6'], ]) # Find action action = controller.get_policy(elapsed.to_sec(), theshold_time=steps * (5. / 60.)) # Get control if (controller.policy_type == 'joint'): joint_vels = dict([('right_j{}'.format(idx), action[idx]) for idx in range(7)]) else: joint_vels = controller.get_control(action) # Send contgrol to robot controller._right_arm.set_joint_velocities(joint_vels) # Record trajectories_gathered[-1].append([ eef_pos_in_base, eef_vel_in_base, obs_joint_pos, obs_joint_vels ]) rate.sleep() #pickle results log_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), log_path) pickle.dump(trajectories_gathered, open(os.path.abspath(log_path), 'wb')) rospy.signal_shutdown('Done')