def main(): rospy.init_node("release_cup") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled left = baxter_interface.Gripper('left', CHECK_VERSION) # right = baxter_interface.Gripper('right', CHECK_VERSION) rs.enable() rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState, get_present_state, left, queue_size=1) gripper_pub = rospy.Publisher('cup_taken', Bool, queue_size=10) done = False while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True if c == 'w': left.open() elif c == 'q': left.close()
def map_keyboard(): bindings = { # key: (function, args, description) 'a': (move_end_effector, ['right', 'Y', 0.1], "Y increase"), 'd': (move_end_effector, ['right', 'Y', -0.1], "Y decrease"), 'w': (move_end_effector, ['right', 'X', 0.1], "x increase"), 's': (move_end_effector, ['right', 'X', -0.1], "X decrease"), } done = False print("Controlling robot. Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 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 adjust_yn(): def yes(): done = True rospy.signal_shutdown("Shutting Down") def no(): main_menu() bindings = { # key: (function, args, description) 'y': (yes,[], "Let Robot Continue"), 'n': (no, [], "Enter Control Mode"), } done = False while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Shutting Down.") elif c in bindings: cmd = bindings[c] cmd[0](*cmd[1]) else: print("key bindings: ") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def map_keyboard(): bindings = { # key: (function, args, description) 'a': (move_end_effector, ['right', 'Y', 0.1], "Y increase"), 'd': (move_end_effector, ['right', 'Y', -0.1], "Y decrease"), 'w': (move_end_effector, ['right', 'X', 0.1], "x increase"), 's': (move_end_effector, ['right', 'X', -0.1], "X decrease"), } done = False print("Controlling robot. Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 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 grids_location_recorder(self): done_flag = False #x_counter = 0 #y_counter = 0 counter = 0 self.GridLocations = [] print "Enter Keyboard Command:" while not done_flag and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: if c in ['q']: print "Grid Location Recorder Stops" done_flag = True elif c in ['r']: cur_pose = self.current_poses[self.arm] self.GridLocations.append(cur_pose) # will be 9 in total print "Current Pose", cur_pose print len(self.GridLocations ), " number of grid location recorded" elif c in ['u']: pass #rospy.sleep(0.1) print "Saving Grid Location Data" self.save_grid_locations()
def rotation_keyboard(): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.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 complete(): done = True rospy.signal_shutdown("Shutting Down") bindings = { # key: (function, args, description) 'k': (set_j, [left, lj[6], 0.1], "left wrist increase"), 'l': (set_j, [left, lj[6], -0.1], "left wrist decrease"), 'a': (set_j, [right, rj[6], 0.1], "right wrist increase"), 's': (set_j, [right, rj[6], -0.1], "right wrist decrease"), 'd': (complete,[], "Quit"), 'm': (main_menu,[], "Return To Main Menu"), } done = False print("Controlling Rotational Orientation") print("key bindings: ") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) #os.system('rosrun baxter_examples xdisplay_image.py --file ~/Pictures/key_bindings.jpg') while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Program finished.") elif c in bindings: cmd = bindings[c] #expand binding to something like "set_j(right, 's0', 0.1)" cmd[0](*cmd[1]) print("command: %s" % ( [2],)) else: print("key bindings: ") print(" d: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def adjust_anchor(anchor): old_point = anchor['position'] point = Point(x=old_point.x, y=old_point.y, z=old_point.z) anchor['position'] = point old_color = anchor['color'] anchor['color'] = (0, 1, 0, 1) static_pub.publish("/alignment_markers", create_marker_array(anchor)) STEP = 0.01 print( "Move the green dot so that it is in the same position relative to the robot model as the actual marker on the physical" ) print("robot. Use the following keyboard to adjust the dot:") print("") print(" w/s - Up/Down") print(" a/d - Left/Right") print(" q/e - Forward/Backward") print(" 1 - Set step size to 10cm") print(" 2 - Set step size to 1cm") print(" 3 - Set step size to 1mm") print(" Esc - Cancel") print(" Enter - Save") while True: c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: anchor['position'] = old_point break elif c == 'w': point.y = point.y - STEP elif c == 's': point.y = point.y + STEP elif c == 'a': point.x = point.x - STEP elif c == 'd': point.x = point.x + STEP elif c == 'q': point.z = point.z + STEP elif c == 'e': point.z = point.z - STEP elif c == '1': STEP = 0.1 print("Step set to 10cm.") elif c == '2': STEP = 0.01 print("Step set to 1cm.") elif c == '3': STEP = 0.001 print("Step set to 1mm.") elif c in ['\x0d']: break anchor['color'] = old_color
def map_keyboard(): left = baxter_interface.Limb('left') # right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) # grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() # rj = right.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) bindings = { # key: (function, args, description) '9': (set_j, [left, lj[0], 0.1], "left_s0 increase"), '6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"), '8': (set_j, [left, lj[1], 0.1], "left_s1 increase"), '7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"), 'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"), 'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"), 'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"), 'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"), 'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"), 'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"), 'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"), 'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"), '.': (set_j, [left, lj[6], 0.1], "left_w2 increase"), 'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"), ',': (grip_left.close, [], "left: gripper close"), 'm': (grip_left.open, [], "left: gripper open"), '/': (grip_left.calibrate, [], "left: gripper calibrate") } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 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 map_keyboard(): printHelper() rospy.loginfo('press ? to print help') while not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: rospy.signal_shutdown("Finished.Exiting...") return checkCommand(c)
def map_keyboard(): printHelper() rospy.loginfo('press ? to print help') while not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: rospy.signal_shutdown("Finished.Exiting...") return checkCommand(c)
def print_endpoint_effort(self): limb = baxter_interface.Limb(self.limb_side) done = False while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() wrench = limb.endpoint_effort() print('Force (x,y,z): ' + str(wrench['force'])) time.sleep(0.5) if c: if c in ['\x1b', '\x03']: #check if ESC of CTRL-C pressed done = True
def main_menu(): def complete(): done = True rospy.signal_shutdown("Shutting Down") def rotate(): rotation_keyboard() def xpos(): xpos = raw_input("Enter x adjustment (positive or negative): ") xpos ='x: '+xpos msg_publisher(xpos) def ypos(): ypos = raw_input("Enter y adjustment (positive or negative): ") ypos ='y: '+ypos msg_publisher(ypos) def zpos(): zpos = raw_input("Enter z adjustment (positive or negative): ") zpos ='z: '+zpos msg_publisher(zpos) bindings = { # key: (function, args, description) 'd': (complete,[], "Quit"), 'r': (rotate,[], "Adjust Rotational Orientation"), 'x': (xpos,[],"Input X Position Adjustment"), 'y': (ypos,[],"Input Y Position Adjustment"), 'z': (zpos,[],"Input Z Position Adjustment"), } print("Main Menu for Control Mode") print("key bindings: ") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) print(" ?: Help") done = False while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Shutting Down.") elif c in bindings: cmd = bindings[c] cmd[0](*cmd[1]) else: print("key bindings: ") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def map_keyboard(): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.joint_names() bhc = BaxterHapticControl('left', 0) bindings = { # key: (function, args, description) 'k': (bhc._velocity, [0, 0, 10, 0, 0, 0], "Velocity Controller Activated"), 'l': (bhc._velocity, [10, 0, 0, 0, 0, 0], "Velocity Controller Activated"), 'x': (bhc._delta_x_position, [0.1], "Pos X Increasing"), 'c': (bhc._delta_x_position, [-0.1], "Pos X Decreasing"), 'j': (bhc._determinant, [], "Determinant"), 'f': (bhc._set_point, [ 0.75, 0.15, -0.129, [[-0.9987068, -0.0499725, 0.0093573], [-0.0498289, 0.9986452, 0.0149960], [-0.0100940, 0.0145103, -0.9998438]] ], "Setting Pose"), 'd': (bhc._starting_position, [], "Starting Pose"), 'e': (bhc._get_current_endpose, [], "Current EE Pose"), 'm': (bhc._create_planned_trajectory, [], "Created Trajectory"), 'r': (bhc._neutral_position, [], "Resetting") } done = False print("Controlling joints. Press ? for help, Esc to quit.") rate = rospy.Rate(1000) #Controls rate of the loop below. while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 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 map_keyboard(self): # initialize interfaces print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled limb_0 = baxter_interface.Gripper(self.limb_name, CHECK_VERSION) done = False print("Enabling robot... ") rs.enable() print("Controlling grippers. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True elif c == 'a': rospy.loginfo("%s is pressed" %c) self.move((0,-1,0),0.05) elif c == 'd': rospy.loginfo("%s is pressed" %c) self.move((0,1,0),0.05) elif c == 'w': rospy.loginfo("%s is pressed" %c) self.move((-1,0,0),0.05) elif c == 's': rospy.loginfo("%s is pressed" %c) self.move((1,0,0),0.05) elif c == 'z': rospy.loginfo("%s is pressed" %c) self.move((0,0,-1),0.05) elif c == 'x': rospy.loginfo("%s is pressed" %c) self.move((0,0,1),0.05) else: print("Not implement it yet...") # cmd = self.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(self.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("Move.py finished.")
def execute(self, userdata): if self._init: self.move_pose = copy.deepcopy(userdata.pick_object_pose) self._init = False def set_pose(direction, delta): move_pose = copy.deepcopy(self.move_pose) move_scale = self.move_scale if direction == 'f': move_scale = move_scale + 1 if move_scale == 11: move_scale = 10 print move_scale elif direction == 'b': move_scale = move_scale - 1 if move_scale == 0: move_scale = 1 print move_scale elif direction == 'x': move_pose.position.x = move_pose.position.x + delta * move_scale elif direction == 'y': move_pose.position.y = move_pose.position.y + delta * move_scale elif direction == 'z': move_pose.position.z = move_pose.position.z + delta * move_scale self.move_pose = copy.deepcopy(move_pose) userdata.move_pose = copy.deepcopy(move_pose) self.move_scale = move_scale bindings = { '2': (set_pose, ['x', -0.01], "x decrease 0.01!"), '8': (set_pose, ['x', 0.01], "x increase 0.01!"), '4': (set_pose, ['y', -0.01], "y decrease 0.01!"), '6': (set_pose, ['y', 0.01], "y increase 0.01!"), 's': (set_pose, ['z', -0.01], "z decrease 0.01!"), 'w': (set_pose, ['z', 0.01], "z increase 0.01!"), '1': (set_pose, ['f', 1], "Multiply scale * increase 1!"), '3': (set_pose, ['b', 1], "Multiply scale * decrease 1!") } done = False while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: if c in bindings: cmd = bindings[c] cmd[0](*cmd[1]) print("command: %s" % cmd[2]) done = True return 'Succeed'
def handle_keyboard(engine, mgame): 'Funcion for allowing us to interface with processes via the keyboard' while not rospy.is_shutdown(): # Get keyboard input only when it changes (-1 input facilitates this) c = baxter_external_devices.getch(-1) if c: if c == 'c': engine._clapDetected('right') if c == 'x': engine._clapDetected('left') if c == 's': mgame.waiting = False if c == 'f': os._exit(0) if c == 'q': mgame.stop()
def talker(): # function menu print("Use 1-6 to control the increment of left arm") print("Use Control + (1-6) to control the negative increment of left arm") print("Use q-y to control the right arm") print("Use Control + (q-y) to control the negative increment of right arm") #initialize node and topic pub = rospy.Publisher('ik', String, queue_size=10) rospy.init_node('ik_talker', anonymous=True) # mapping from key to function bindings = { 'left_arm_increment': ['1', '2', '3', '4', '5', '6'], 'left_arm_decrement': ['!', '@', '#', '$', '%', '^'], 'right_arm_increment': ['q', 'w', 'e', 'r', 't', 'y'], 'right_arm_decrement': ['Q', 'W', 'E', 'R', 'T', 'Y'] } done = False message = '' while not done and not rospy.is_shutdown(): c = baxter_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['left_arm_increment']: message = "left_joint_increse " + c elif c in bindings['left_arm_decrement']: message = "left_joint_decrese " + c elif c in bindings['right_arm_increment']: message = "right_joint_increase " + c elif c in bindings['right_arm_decrement']: message = "right_joint_decrease " + c # test begin elif c == 'z': message = "left limb " + c # test end else: rospy.loginfo('key error!') if message != '': #publish message to topic rospy.loginfo(message) pub.publish(message)
def map_keyboard(): pub = rospy.Publisher("end_effector_command", String, queue_size=10) keyboard_binding = {} keyboard_binding["w"] = "up" keyboard_binding["s"] = "down" keyboard_binding["a"] = "left" keyboard_binding["d"] = "right" keyboard_binding["q"] = "backward" keyboard_binding["e"] = "forward" keyboard_binding["f"] = "switch limb to " keyboard_binding["z"] = "orientation_x" keyboard_binding[" "] = "keep" keyboard_binding["k"] = "further" keyboard_binding["l"] = "closer" limb = ["right", "left"] current_limb = 0 command = list() rate = rospy.Rate(6) print limb[current_limb] + " limb under control..." while not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: rospy.signal_shutdown("Finished.Exiting...") if (c in keyboard_binding.keys()): if (c == "f"): current_limb = 1 - current_limb print "control switch to " + limb[current_limb] pub.publish( String(keyboard_binding[c] + limb[current_limb])) else: print "sending command: " + limb[ current_limb] + " limb " + keyboard_binding[c] #pub.publish(String(keyboard_binding[c])) command = list() for s in range(0, 1): command.append(keyboard_binding[c]) else: print "invalid command: " + c #print limb[current_limb] + " limb under control..." # new control type if (len(command)): pub.publish(String(command.pop())) rate.sleep()
def map_keyboard(): pub = rospy.Publisher("end_effector_command", String, queue_size=10) keyboard_binding = {} keyboard_binding["w"] = "up" keyboard_binding["s"] = "down" keyboard_binding["a"] = "left" keyboard_binding["d"] = "right" keyboard_binding["q"] = "backward" keyboard_binding["e"] = "forward" keyboard_binding["f"] = "switch limb to " keyboard_binding["z"] = "orientation_x" keyboard_binding[" "] = "keep" keyboard_binding["k"] = "further" keyboard_binding["l"] = "closer" limb = ["right", "left"] current_limb = 0 command = list() rate = rospy.Rate(6) rospy.loginfo(limb[current_limb] + " limb under control...") while not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: # catch Esc or ctrl-c if c in ["\x1b", "\x03"]: rospy.signal_shutdown("Finished.Exiting...") if c in keyboard_binding.keys(): if c == "f": current_limb = 1 - current_limb rospy.loginfo("control switch to " + limb[current_limb]) pub.publish(String(keyboard_binding[c] + limb[current_limb])) else: rospy.loginfo("sending command: " + limb[current_limb] + " limb " + keyboard_binding[c]) # pub.publish(String(keyboard_binding[c])) command = list() for s in range(0, 1): command.append(keyboard_binding[c]) else: rospy.loginfo("invalid command: " + c) # print limb[current_limb] + " limb under control..." # new control type if len(command): pub.publish(String(command.pop())) rate.sleep()
def map_keyboard(): pub = rospy.Publisher("end_effector_command", String, queue_size=10) keyboard_binding = {} keyboard_binding["w"] = "up" keyboard_binding["s"] = "down" keyboard_binding["a"] = "left" keyboard_binding["d"] = "right" keyboard_binding["q"] = "backward" keyboard_binding["e"] = "forward" keyboard_binding["f"] = "switch limb " keyboard_binding["z"] = "orientation_x" keyboard_binding[" "] = "keep" keyboard_binding["k"] = "further" keyboard_binding["l"] = "closer" limb = ["right", "left"] current_limb = 0 rospy.loginfo(limb[current_limb] + " limb under control...") rospy.loginfo('press ? to print help') while not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: rospy.signal_shutdown("Finished.Exiting...") return if c == '?': printHelper(keyboard_binding) if (c in keyboard_binding.keys()): if (c == "f"): current_limb = 1 - current_limb rospy.loginfo("control switch to " + limb[current_limb]) pub.publish( String(keyboard_binding[c] + limb[current_limb])) else: rospy.loginfo("sending command: " + limb[current_limb] + " limb " + keyboard_binding[c]) pub.publish(String(keyboard_binding[c])) else: rospy.loginfo("invalid command: " + c) rospy.loginfo('press ? to print help')
def select_from_list(lst, allow_cancel=True): if allow_cancel: print("Select one (Esc to cancel)-") else: print("Select one-") for i in range(len(lst)): print("\t" + str(i) + ": " + str(lst[i])) while True: c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: if allow_cancel: return -1 elif c >= '0' and c <= '9': print("Selected " + str(lst[int(c)])) return int(c) else: rospy.sleep(0.001)
def map_keyboard(): pub = rospy.Publisher("end_effector_command", String, queue_size=10); keyboard_binding = {} keyboard_binding["w"] = "up" keyboard_binding["s"] = "down" keyboard_binding["a"] = "left" keyboard_binding["d"] = "right" keyboard_binding["q"] = "backward" keyboard_binding["e"] = "forward" keyboard_binding["f"] = "switch limb " keyboard_binding["z"] = "orientation_x" keyboard_binding[" "] = "keep" keyboard_binding["k"] = "further" keyboard_binding["l"] = "closer" limb = ["right", "left"] current_limb = 0 rospy.loginfo(limb[current_limb] + " limb under control...") rospy.loginfo('press ? to print help') while not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: rospy.signal_shutdown("Finished.Exiting...") return if c == '?': printHelper(keyboard_binding) if(c in keyboard_binding.keys()): if (c == "f"): current_limb = 1 - current_limb rospy.loginfo("control switch to " + limb[current_limb]) pub.publish(String(keyboard_binding[c] + limb[current_limb])) else: rospy.loginfo("sending command: " + limb[current_limb] + " limb " + keyboard_binding[c]) pub.publish(String(keyboard_binding[c])) else: rospy.loginfo("invalid command: " + c) rospy.loginfo('press ? to print help')
def map_keyboard(): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.joint_names() pnp = PickAndPlace('left', 0) 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) bindings = { # key: (function, args, description) '9': (handle_turtle_pose, [], "increase z"), } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 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 map_keyboard(self): # initialize interfaces print("Getting robot state... ") rs = baxter_intercontrollerface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled limb_0 = baxter_interface.Gripper(self.limb_name, CHECK_VERSION) done = False print("Enabling robot... ") rs.enable() print("Controlling grippers. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True elif c == 'a': rospy.loginfo("%s is pressed" % c) self.move((0, -1, 0), 0.05) elif c == 'd': rospy.loginfo("%s is pressed" % c) self.move((0, 1, 0), 0.05) elif c == 'w': rospy.loginfo("%s is pressed" % c) self.move((-1, 0, 0), 0.05) elif c == 's': rospy.loginfo("%s is pressed" % c) self.move((1, 0, 0), 0.05) elif c == 'z': rospy.loginfo("%s is pressed" % c) self.move((0, 0, -1), 0.05) elif c == 'x': rospy.loginfo("%s is pressed" % c) self.move((0, 0, 1), 0.05) else: print("Not implement it yet...") rospy.signal_shutdown("Move.py finished.")
def map_keyboard(self): # initialize interfaces print("Getting robot state... ") rs = baxter_intercontrollerface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled limb_0 = baxter_interface.Gripper(self.limb_name, CHECK_VERSION) done = False print("Enabling robot... ") rs.enable() print("Controlling grippers. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True elif c == 'a': rospy.loginfo("%s is pressed" %c) self.move((0,-1,0),0.05) elif c == 'd': rospy.loginfo("%s is pressed" %c) self.move((0,1,0),0.05) elif c == 'w': rospy.loginfo("%s is pressed" %c) self.move((-1,0,0),0.05) elif c == 's': rospy.loginfo("%s is pressed" %c) self.move((1,0,0),0.05) elif c == 'z': rospy.loginfo("%s is pressed" %c) self.move((0,0,-1),0.05) elif c == 'x': rospy.loginfo("%s is pressed" %c) self.move((0,0,1),0.05) else: print("Not implement it yet...") rospy.signal_shutdown("Move.py finished.")
def ask(self): done = False printed_objects = None while not done and not rospy.is_shutdown(): if printed_objects != self.objects: printed_objects = self.objects print "Which object?" for i, o in enumerate(self.objects): print "%d.)" % (i + 1), o c = baxter_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True else: try: idx = int(c) except ValueError: idx = 0 if idx != None and idx > 0 and idx <= len(self.objects): print "Sending", self.objects[idx - 1] self.command_publisher.publish(self.objects[idx - 1])
def ask(self): done = False printed_objects = None while not done and not rospy.is_shutdown(): if printed_objects != self.objects: printed_objects = self.objects print "Which object?" for i, o in enumerate(self.objects): print "%d.)" % (i + 1), o c = baxter_external_devices.getch() if c: if c in ['\x1b', '\x03']: done = True else: try: idx = int(c) except ValueError: idx = 0 if idx != None and idx > 0 and idx <= len(self.objects): print "Sending", self.objects[idx - 1] self.command_publisher.publish(self.objects[idx - 1])
def input_control(speed, accuracy): """ Takes user input to move the limbs """ left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.joint_names() # Set the speeds for the limbs to move left.set_joint_position_speed(speed) right.set_joint_position_speed(speed) done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c == '?': """ TODO FILL THIS OUT!!!! """ print("Enter joints and their values in corresponding order: ") 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 map_keyboard(): left = baxter_interface.Limb('left') # left = BaxterArm('left') right = BaxterArm('right') kin = baxter_kinematics('left') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.joint_names() leftdes = [] leftacc = [] lefterr = [] delta = 0.075 #was 0.05 gain = 1. def IK(limb, movement): # current_limb= limb.angles() # joint_command = {joint_name: current_position + delta} # current_pos, curr_ori = limb.ee_pose() # leftacc = np.append(current_pos,quaternion.as_euler_angles(curr_ori)) # print leftacc.shape joint_names = limb.joint_names() # curr_q = limb.joint_angles() def to_list(ls): return [ls[n] for n in joint_names] curr_q = np.asarray(to_list(limb.joint_angles())).reshape([7, 1]) # print curr_q.shape, 'shape' # print 'ee pose', limb.endpoint_pose() pos = np.array(limb.endpoint_pose()['position']) #ori = limb.endpoint_pose()['orientation'] #pose = np.append(pos, quaternion.as_euler_angles(quaternion.quaternion(ori.w,ori.x,ori.y,ori.z))).reshape([6,1]) #print pose # print pos, ori # sldfjsl # raw_input() JT = kin.jacobian_transpose() lefterr = np.append(movement, [0, 0, 0]).reshape([6, 1]) #lefterr = np.array(movement).reshape([3,1]) # leftdes = leftacc+lefterr # print leftacc, lefterr, leftdes #JT = np.linalg.pinv(limb.jacobian()) # JT = limb.jacobian().T des_q = curr_q + gain * JT.dot(lefterr) #des_q = JT.dot(lefterr + pose) print des_q.shape, 'shape2' q_dict = {} for i in range(len(joint_names)): q_dict[joint_names[i]] = des_q[i, :][0] # print joint_names[i], q_dict[joint_names[i]] # dsflasjkds # joint_command = current_limb + JT.dot(lefterr) # joint_command = JT.dot(leftdes) # print joint_command # print JT.dot(lefterr) # limb.exec_position_cmd(joint_command) # joint_command = {joint_name: curr_q + JT.dot(lefterr)} # print q_dict limb.move_to_joint_positions(q_dict) #print (pos[1]-np.array(limb.endpoint_pose()['position'])[1])/delta 0.91 for x, 0.82 for z print(pos[2] - np.array(limb.endpoint_pose()['position'])[2]) / delta bindings = { # key: (function, args, description) # 's': (IK, [left, [delta,delta/100,delta/100] ], "xinc"), #'d': (IK, [left, [-delta,-delta/100,-delta/100] ], "xdec"), 's': (IK, [left, [delta * 0.8, 0, 0]], "xinc"), 'd': (IK, [left, [-delta * 0.8, 0, 0]], "xdec"), 'w': (IK, [left, [0, delta * 0.5, 0]], "yinc"), 'e': (IK, [left, [0, -delta * 0.5, 0]], "ydec"), #'w': (IK, [left, [-delta/20,delta,-delta/20] ], "yinc"), # 'e': (IK, [left, [delta/20,-delta,delta/20] ], "ydec"), 'x': (IK, [left, [0, 0, delta]], "zinc"), 'c': (IK, [left, [0, 0, -delta]], "zdec"), 'k': (IK, [left, [0, 0, 0]], "nothing"), 'l': (IK, [left, [delta / 100, delta, delta / 100]], "allincy"), ';': (IK, [left, [-delta, -delta, -delta]], "alldec"), #'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"), 'b': (grip_left.close, [], "left: gripper close"), 'n': (grip_left.open, [], "left: gripper open"), 'm': (grip_left.calibrate, [], "left: gripper calibrate") #comma here? } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 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 chose_configuration(): # initialize interfaces print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled left = baxter_interface.Gripper('left', CHECK_VERSION) right = baxter_interface.Gripper('right', CHECK_VERSION) def clean_shutdown(): global out_file if not init_state: print("Disabling robot...") rs.disable() out_file.close() print("Exiting manipulations script.") rospy.on_shutdown(clean_shutdown) def move_left_flat_up(): global out_file out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_center.rec") right.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_left_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_left_flat.rec") out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_pushrotate_left_flat_up\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_pushrotate_left_flat_up.rec") out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_left_up\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_left_up.rec") right.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_center.rec") newConfig( a,b,d,f,e,h ) def move_left_up_flat(): global out_file out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_center.rec") right.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_left_up\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_left_up.rec") out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_pullrotate_left_up_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_pullrotate_left_up_flat.rec") out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_left_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_left_flat.rec") right.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_center.rec") newConfig( b,f,h,a,d,e ) def move_left_flat_down(): global out_file out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_center.rec") right.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_left_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_left_flat.rec") left.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_rotate_left_flat_down\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_rotate_left_flat_down.rec") left.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_left_down\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_left_down.rec") right.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_center.rec") newConfig( a,d,e,c,i,g ) def move_left_down_flat(): global out_file out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_center.rec") right.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_left_down\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_left_down.rec") left.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_rotate_left_down_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_rotate_left_down_flat.rec") left.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_left_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_left_flat.rec") right.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_center.rec") newConfig( c,i,g,a,d,e ) def move_right_flat_up(): global out_file out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_center.rec") left.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_right_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_right_flat.rec") out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_pushrotate_right_flat_up\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_pushrotate_right_flat_up.rec") out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_right_up\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_right_up.rec") left.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_center.rec") newConfig( a,b,c,d,f,i ) def move_right_up_flat(): global out_file out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_center.rec") left.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_right_up\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_right_up.rec") out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_pullrotate_right_up_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_pullrotate_right_up_flat.rec") out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_right_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_right_flat.rec") left.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_center.rec") newConfig( d,f,i,a,b,c ) def move_right_flat_down(): global out_file out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_center.rec") left.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_right_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_right_flat.rec") right.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_rotate_right_flat_down\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_rotate_right_flat_down.rec") right.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_right_down\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_right_down.rec") left.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_center.rec") newConfig( a,b,c,e,h,g ) def move_right_down_flat(): global out_file out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_reach_center\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_reach_center.rec") left.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_reach_right_down\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_reach_right_down.rec") right.close([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_rotate_right_down_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_rotate_right_down_flat.rec") right.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting right_leave_right_flat\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/right_leave_right_flat.rec") left.open([]) out_file.write("Time: "+ str(rospy.Time.now()) + ": starting left_leave_cente\n") os.system("rosrun baxter_examples joint_trajectory_file_playback_modified.py -f /home/berto/ros_ws/src/articulated_object_manipulation/actions/left_leave_center.rec") newConfig( e,h,g,a,b,c ) configurations_from_a = { # key: (function, args, description) 'b': (move_left_flat_up, [], "b"), 'c': (move_left_flat_down, [], "c"), 'd': (move_right_flat_up, [], "d"), 'e': (move_right_flat_down, [], "e"), } configurations_from_b = { # key: (function, args, description) 'a': (move_left_up_flat, [], "a"), 'f': (move_right_flat_up, [], "f"), 'h': (move_right_flat_down, [], "h"), } configurations_from_c = { # key: (function, args, description) 'a': (move_left_down_flat, [], "a"), 'g': (move_right_flat_down, [], "g"), 'i': (move_right_flat_up, [], "i"), } configurations_from_d = { # key: (function, args, description) 'a': (move_right_up_flat, [], "a"), 'f': (move_left_flat_up, [], "f"), 'i': (move_left_flat_down, [], "i"), } configurations_from_e = { # key: (function, args, description) 'a': (move_right_down_flat, [], "a"), 'g': (move_left_flat_down, [], "g"), 'h': (move_left_flat_up, [], "h"), } configurations_from_f = { # key: (function, args, description) 'b': (move_right_up_flat, [], "b"), 'd': (move_left_up_flat, [], "d"), } configurations_from_g = { # key: (function, args, description) 'c': (move_right_down_flat, [], "c"), 'e': (move_left_down_flat, [], "e"), } configurations_from_h = { # key: (function, args, description) 'b': (move_right_down_flat, [], "b"), 'e': (move_left_up_flat, [], "e"), } configurations_from_i = { # key: (function, args, description) 'c': (move_right_up_flat, [], "c"), 'd': (move_left_down_flat, [], "d"), } done = False print("Enabling robot... ") rs.enable() print("=================================================================================") print("Current configuration is 'a', insert the goal configuration or") print("press ? to see the reachable configurations, Esc to quit.") print("=================================================================================") while not done and not rospy.is_shutdown(): first_input = baxter_external_devices.getch() if first_input: if current_configuration == a: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_a: cmd = configurations_from_a[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_a.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) elif current_configuration == b: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_b: cmd = configurations_from_b[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_b.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) elif current_configuration == c: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_c: cmd = configurations_from_c[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_c.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) elif current_configuration == d: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_d: cmd = configurations_from_d[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_d.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) elif current_configuration == e: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_e: cmd = configurations_from_e[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_e.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) elif current_configuration == f: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_f: cmd = configurations_from_f[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_f.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) elif current_configuration == g: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_g: cmd = configurations_from_g[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_g.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) elif current_configuration == h: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_h: cmd = configurations_from_h[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_h.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) elif current_configuration == i: if first_input in ['\x1b', '\x03']: done = True elif first_input in configurations_from_i: cmd = configurations_from_i[first_input] print("Move to configuration: %s" % (cmd[2],)) cmd[0](*cmd[1]) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(configurations_from_i.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2])) # force shutdown call if caught by key handler rospy.signal_shutdown("Manipulations finished.")
def map_keyboard(): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) #These are from PyKDL and are needed for the Jacobian left_kin = baxter_kinematics('left') right_kin = baxter_kinematics('right') #Connect with IK service right_iksvc, right_ns = ik_command.connect_service('right') left_iksvc, left_ns = ik_command.connect_service('left') def command_jacobian(side, direction): #UNUSED if side == 'left': limb = left kin = left_kin elif side == 'right': limb = right kin = right_kin else: raise Exception("Got wrong side in command_jacobian") # current is the current position of end effector current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) current_p = current_q.reshape((7, 1)) current = np.zeros((6, 1)) quaternion_to_euler(current_p, current) # We need to convert the direction from the world frame to the hand frame # Rotate direction by the inverse of the rotation matrix from the angular pose in current R = tf.transformations.quaternion_matrix(current_p[3:7].flatten().tolist()) R_inv = tf.transformations.inverse_matrix(R) print R_inv[0:3, 0:3] direction = np.array(direction).reshape((6, 1)) print direction[0:3] dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3]) print dir_rot #direction[0:3] = dir_rot # Goal is the goal position, found by adding the requested direction from the user goal = current + direction current_angles = np.array([limb.joint_angles()[name] for name in limb.joint_names()]) # Get the Jacobian inverse and solve for the necessary change in joint angles jacobian_inv = kin.jacobian_transpose() delta = jacobian_inv*(goal-current) commands = current_angles.flatten() + delta.flatten() # Command the joints command_list = commands.tolist() joint_command = dict(zip(limb.joint_names(), command_list[0])) limb.set_joint_positions(joint_command) def command_ik(side, direction): """Use the Rethink IK service to figure out a desired joint position""" if side == 'left': limb = left iksvc = left_iksvc ns = left_ns else: limb = right iksvc = right_iksvc ns = right_ns current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) direction = np.array(direction) desired_p = current_p + direction ik_command.service_request(iksvc, desired_p, side) zeros = [0]*4 inc = 0.1 bindings = { 'q': (command_ik, ['right', [inc, 0, 0]+zeros], "increase right x"), 'a': (command_ik, ['right', [-inc, 0, 0]+zeros], "decrease right x"), 'w': (command_ik, ['right', [0, inc, 0]+zeros], "increase right y"), 's': (command_ik, ['right', [0, -inc, 0]+zeros], "decrease right y"), 'e': (command_ik, ['right', [0, 0, inc]+zeros], "increase right z"), 'd': (command_ik, ['right', [0, 0, -inc]+zeros], "decrease right z"), 'u': (command_ik, ['left', [inc, 0, 0]+zeros], "increase left x"), 'j': (command_ik, ['left', [-inc, 0, 0]+zeros], "decrease left x"), 'i': (command_ik, ['left', [0, inc, 0]+zeros], "increase left y"), 'k': (command_ik, ['left', [0, -inc, 0]+zeros], "decrease left y"), 'o': (command_ik, ['left', [0, 0, inc]+zeros], "increase left z"), 'l': (command_ik, ['left', [0, 0, -inc]+zeros], "decrease left z"), 'z': (grip_right.close, [], "right: gripper close"), 'x': (grip_right.open, [], "right: gripper open"), 'c': (grip_right.calibrate, [], "right: gripper calibrate") } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "command_jacobian(left, [0.1, 0, 0])" 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 map_keyboard(): global desired_angles, inc, angle_inc left = baxter_interface.Limb('left') grip_left = baxter_interface.Gripper('left', CHECK_VERSION)qqqqqqqqqqqa #These are from PyKDL and are needed for the Jacobian left_kin = baxter_kinematics('left') #Connect with IK service left_iksvc, left_ns = ik_command.connect_service('left') #Initialize desired angles from current angles init_orient = left.endpoint_pose()['orientation'] quat = [0,0,0,0] quat[3] = init_orient.w quat[0] = init_orient.x quat[1] = init_orient.y quat[2] = init_orient.z desired_angles = tf.transformations.euler_from_quaternion(quat) #print init_orient def command_jacobian(side, direction): #UNUSED if side == 'left': limb = left kin = left_kin else: raise Exception("Got wrong side in command_jacobian") # current is the current position of end effector current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) current_p = current_q.reshape((7, 1)) current = np.zeros((6, 1)) quaternion_to_euler(current_p, current) # We need to convert the direction from the world frame to the hand frame # Rotate direction by the inverse of the rotation matrix from the angular pose in current R = tf.transformations.quaternion_matrix(current_p[3:7].flatten().tolist()) R_inv = tf.transformations.inverse_matrix(R) print R_inv[0:3, 0:3] direction = np.array(direction).reshape((6, 1)) print direction[0:3] dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3]) print dir_rot #direction[0:3] = dir_rot # Goal is the goal position, found by adding the requested direction from the user goal = current + direction current_angles = np.array([limb.joint_angles()[name] for name in limb.joint_names()]) # Get the Jacobian inverse and solve for the necessary change in joint angles jacobian_inv = kin.jacobian_transpose() delta = jacobian_inv*(goal-current) commands = current_angles.flatten() + delta.flatten() # Command the joints command_list = commands.tolist() joint_command = dict(zip(limb.joint_names(), command_list[0])) limb.set_joint_positions(joint_command) def command_ik(side, direction): """Use the Rethink IK service to figure out a desired joint position""" if side == 'left': limb = left iksvc = left_iksvc ns = left_ns current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) direction = np.array(direction) print direction desired_p = current_p + direction ik_command.service_request(iksvc, desired_p, side) zeros = [0]*4 def update_angles(change_angles): global desired_angles current_angles = np.array(desired_angles) change_angles = np.array(change_angles) #Note that adding np arrays does element-wise summation, but adding python lists concatenates them new_angles = current_angles + change_angles #Check that the angles are in bounds, angles are in radians #for value in new_angles: #Wrap crossing zero towards 2*pi # if value < 0: # value = (2* np.pi) + value #Wrap crossing 2*pi towards zero # if value > (2 * np.pi): # value = value - (2*np.pi) #Convert to a python array and store in the desired angles desired_angles = new_angles.tolist() #Convert the desired angles to a quaternion quat = tf.transformations.quaternion_from_euler(desired_angles[0], desired_angles[1], desired_angles[2]) #This only does the left side, because the scooter only has a left arm limb = left current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) current_p[3:7] = quat ik_command.service_request(left_iksvc, current_p, "left") def update_inc(isSmall = True): global inc, angle_inc if isSmall: inc = 0.1 angle_inc = 0.1 else: inc = 0.5 angle_inc = 0.2 def toggle_gripper(): if grip_left.position() > 95: #gripper is open, so close it. Can be confused by a big object, though. print "Closing gripper" grip_left.close() elif grip_left.gripping() or grip_left.position() < 5: print "Opening gripper" #gripper is closed on something, or on nothing grip_left.open() else: print "Unknown state, opening gripper" #Unknown state, so open the gripper grip_left.open() done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): #This had to be moved into the loop so the increment could be rebound when #the -/= keys are used to change the increment between small and large bindings = { 'u': (command_ik, ['left', [inc, 0, 0]+zeros], "increase left x"), 'j': (command_ik, ['left', [-inc, 0, 0]+zeros], "decrease left x"), 'i': (command_ik, ['left', [0, inc, 0]+zeros], "increase left y"), 'k': (command_ik, ['left', [0, -inc, 0]+zeros], "decrease left y"), 'o': (command_ik, ['left', [0, 0, inc]+zeros], "increase left z"), 'l': (command_ik, ['left', [0, 0, -inc]+zeros], "decrease left z"), 'q': (update_angles, [[angle_inc, 0, 0]], "increase left roll"), 'a': (update_angles, [[-angle_inc, 0, 0]], "decrease left roll"), 'w': (update_angles, [[0,angle_inc,0]], "increase left pitch"), 's': (update_angles, [[0,-angle_inc,0]], "decrease left pitch"), 'e': (update_angles, [[0,0,angle_inc]], "increase left yaw"), 'd': (update_angles, [[0,0,-angle_inc]], "decrease left yaw"), '-': (update_inc, [True], "small motion increments"), '=': (update_inc, [False], "large motion increments"), ' ': (toggle_gripper, [], "toggle gripper state") } c = baxter_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] #expand binding to something like "command_jacobian(left, [0.1, 0, 0])" cmd[0](*cmd[1]) print("command: %s with params %s" % (cmd[2], cmd[1])) 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 run_control(joystick): # Initialise parameters kbOnly = True if joystick == None else False print('kbOnly: ', kbOnly) #abbreviations if kbOnly == False: jhi = lambda s: joystick.stick_value(s) > 0 jlo = lambda s: joystick.stick_value(s) < 0 bdn = joystick.button_down bup = joystick.button_up zeros = [0] * 4 initial_inc = 0.03 inc = initial_inc delta = 0.01 incU = 0.05 incL = 0.01 syncErrThresh = 0.01 vrepZdelta = 1.08220972 # Service to call functions in V-rep (e.g. set/get joint positions) vrepScriptFunc = rospy.ServiceProxy('vrep/simRosCallScriptFunction', simRosCallScriptFunction) # Get robot interface robot = RobotArm(vrepScriptFunc) robot.updPos('left') # Get appropriate bindings kb_b = js_b = False def gen_js_bindings(inc): bindings = ( ((bdn, ['btnUp']), (robot.grip_l_close, []), "left gripper"), ((jhi, ['rightTrigger']), (inc_delta, [inc, delta, incU, incL]), "Speed increased"), ((jhi, ['leftTrigger']), (inc_delta, [inc, -delta, incU, incL]), "Speed decreased"), ((bdn, ['leftBumper']), (robot.pos_to_ori, ['down']), "Switched to orientation"), ((bup, ['leftBumper']), (robot.pos_to_ori, ['up']), "Switched to position"), ((bdn, ['rightBumper']), (robot.world_to_ee, ['ee']), "Switched to tool frame"), ((bup, ['rightBumper']), (robot.world_to_ee, ['world']), "Switched to world frame"), ((jlo, ['leftStickHorz']), (robot.command_ik, ['left', [0, -inc, 0]]), lambda: "left y dec "), ((jhi, ['leftStickHorz']), (robot.command_ik, ['left', [0, inc, 0]]), lambda: "left y inc "), ((jlo, ['leftStickVert']), (robot.command_ik, ['left', [-inc, 0, 0]]), lambda: "left x dec "), ((jhi, ['leftStickVert']), (robot.command_ik, ['left', [inc, 0, 0]]), lambda: "left x inc "), ((jlo, ['rightStickVert']), (robot.command_ik, ['left', [0, 0, -inc]]), lambda: "left z dec "), ((jhi, ['rightStickVert']), (robot.command_ik, ['left', [0, 0, inc]]), lambda: "left z inc "), ((bdn, ['function1']), (print_js_bindings, [js_b]), "help"), ((bdn, ['function2']), (print_js_bindings, [js_b]), "help"), ) return bindings def gen_kb_bindings(inc): bindings = { '9': (inc_delta, [inc, delta, incU, incL], "Speed increased"), '0': (inc_delta, [inc, -delta, incU, incL], "Speed decreased"), 'i': (robot.pos_to_ori, [], "Switch position/orientation"), # 'p': (robot.pos_to_ori, ['up'], "Switched to position"), 'o': (robot.world_to_ee, [], "Switch world/tool frame"), # 'i': (robot.world_to_ee, ['world'], "Switched to world frame"), 'w': (robot.command_ik, ['left', [inc, 0, 0]], "increase left x"), 's': (robot.command_ik, ['left', [-inc, 0, 0]], "decrease left x"), 'a': (robot.command_ik, ['left', [0, inc, 0]], "increase left y"), 'd': (robot.command_ik, ['left', [0, -inc, 0]], "decrease left y"), 'q': (robot.command_ik, ['left', [0, 0, inc]], "increase left z"), 'e': (robot.command_ik, ['left', [0, 0, -inc]], "decrease left z"), 'p': (robot.grip_l_close, [], "Gripper open/close"), 'r': (robot.recordPos, ['left'], "Recorded position"), # 'l': (robot.grip_l_open, [], "left: gripper open"), # 'c': (grip_left.calibrate, [], "left: gripper calibrate") } return bindings if kbOnly == False: js_b = [] bindings = gen_js_bindings(inc) js_b.append(bindings) kb_b = gen_kb_bindings(inc) # Print bindings if kbOnly == False: print_js_bindings(js_b) else: print_kb_bindings(kb_b) # Setup data recording endpoint_state = EndpointState def on_endpoint_states(msg, endpoint_state): endpoint_state.header = msg.header endpoint_state.pose = msg.pose endpoint_state.twist = msg.twist endpoint_state.wrench = msg.wrench recording = False done_recording = False def record_data(filename): # Subscribe to endpoint state # _cartesian_state_sub = rospy.Subscriber( # '/robot/limb/left/endpoint_state', # EndpointState, # on_endpoint_states, # endpoint_state, # queue_size=1, # tcp_nodelay=True) # bag = rosbag.Bag(filename, 'w') def terminate_process_and_children(s): list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE) list_output = list_cmd.stdout.read() retcode = list_cmd.wait() assert retcode == 0, "List command returned %d" % retcode for str in list_output.split("\n"): if (str.startswith(s)): os.system("rosnode kill " + str) # while not done_recording: # if recording==True: # print('Endpoint actual: ',endpoint_state) # bag.write('endpoint_state',endpoint_state) rt = rospy.Rate(100) while not recording: rt.sleep() p = subprocess.Popen( 'rosbag record -j -O test.bag /robot/limb/left/endpoint_state ' '/cameras/head_camera/image', stdin=subprocess.PIPE, shell=True, cwd='/home/talha/test_ws/src/baxter_ik/scripts/') # print 'Finishing recording' # bag.close() while not done_recording: rt.sleep() print 'Finishing recording' terminate_process_and_children("/record") # thread.start_new_thread(record_data, ('test.bag',)) # Move to start position move_to_start_pos(robot, syncErrThresh) vrepScriptFunc.call('set_ik_mode@Baxter_leftArm_target', 1, [], [], [], '') rate = rospy.Rate(100) # while loop while not rospy.is_shutdown(): btnPressed = False autoMove = False # inputControl() if kbOnly == True: c = baxter_external_devices.getch(1) if c: # print('Pressed: ',c) recording = True #catch Esc or ctrl-c if c in ['\x1b', '\x03']: # terminate_process_and_children(p) rospy.signal_shutdown("Example finished.") if c == '\r': done_recording = True if c == 'f': autoMove = True # Keyboard control if c in kb_b: btnPressed = True cmd = kb_b[c] if cmd[0] == inc_delta: inc = cmd[0](*cmd[1]) kb_b = gen_kb_bindings(inc) elif cmd[0] == robot.pos_to_ori: kb_b = gen_kb_bindings(initial_inc) cmd[0](*cmd[1]) else: cmd[0](*cmd[1]) # print("command: %s" % (cmd[2],)) else: c = baxter_external_devices.getch() if c == '\r': done_recording = True # Joystick control for (test, cmd, doc) in bindings: if test[0](*test[1]): recording = True if cmd[0] == inc_delta: inc = cmd[0](*cmd[1]) bindings = gen_js_bindings(inc) js_b[0] = bindings elif cmd[0] == robot.pos_to_ori: bindings = gen_js_bindings(initial_inc) js_b[0] = bindings cmd[0](*cmd[1]) else: cmd[0](*cmd[1]) if callable(doc): print(doc()) else: print(doc) if autoMove == True: wp1, wp2, wp3 = auto_move_generate_wp(robot, robot.recordedPose) print('Waypoint 1:', wp1) robot.auto_ik('left', wp1, vrepZdelta) resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target', 1, [], [], [], '') # Get resulting joint positions from V-rep resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target', 1, [], [], [], '') # Wrap and send joint positions to Baxter limb_joints = dict( zip(robot.left._joint_names['left'], resp.outputFloats)) robot.left.move_to_joint_positions(limb_joints) robot.auto_ik('left', wp2, vrepZdelta) resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target', 1, [], [], [], '') # Get resulting joint positions from V-rep resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target', 1, [], [], [], '') # Wrap and send joint positions to Baxter limb_joints = dict( zip(robot.left._joint_names['left'], resp.outputFloats)) robot.left.move_to_joint_positions(limb_joints) robot.auto_ik('left', wp3, vrepZdelta) resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target', 1, [], [], [], '') # Get resulting joint positions from V-rep resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target', 1, [], [], [], '') # Wrap and send joint positions to Baxter limb_joints = dict( zip(robot.left._joint_names['left'], resp.outputFloats)) robot.left.move_to_joint_positions(limb_joints) # Check if IK was a success in V-rep resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target', 1, [], [], [], '') # print 'Ik Success: ', resp.outputInts # If successful, receive & send joints to Baxter if resp.outputInts[0] == -1 and btnPressed == True: # Get resulting joint positions from V-rep resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target', 1, [], [], [], '') # Get Baxter joints curr_jnts = np.array([ robot.left.joint_angles()[name] for name in robot.left.joint_names() ]) # Compare joints norm = np.linalg.norm(curr_jnts) err = math.fabs( np.linalg.norm(np.array(resp.outputFloats) - curr_jnts)) err = err / norm # print('Error: ',err) # limb_joints = dict(zip(robot.left._joint_names['left'], resp.outputFloats)) # robot.left.set_joint_positions(limb_joints) # syncPos(left, vrepScriptFunc) if err > abs(syncErrThresh): # Wrap and send joint positions to Baxter limb_joints = dict( zip(robot.left._joint_names['left'], resp.outputFloats)) robot.left.set_joint_positions(limb_joints) # print('V-rep Joints: ',limb_joints) # resp = vrepScriptFunc.call('get_pose@Baxter_leftArm_target',1,[],[],[],'') # print 'V-rep pose: ',resp.outputFloats # print('---------------------------------------------------------------------------------'/0) # print 'Finish pose: ',current_p # print('Actual joints: ', left.joint_angles()) # print('---------------------------------------------------------------------------------') else: print('Synching') left_arm_sync(robot, syncErrThresh) vrepScriptFunc.call('set_ik_mode@Baxter_leftArm_target', 1, [], [], [], '') rate.sleep()
def get_char(): """Gets the character typed in the terminal. Useful for keyboard control.""" return baxter_external_devices.getch()
def gauge_obj_center(init_pos, vis_pub, obj_height, obj_radius_x, obj_radius_y): center = np.array(init_pos) # 1 mm DELTA = 0.001 X_NEG = np.array([-DELTA, 0, 0]) X_POS = np.array([DELTA, 0, 0]) Y_NEG = np.array([0, -DELTA, 0]) Y_POS = np.array([0, DELTA, 0]) Z_NEG = np.array([0, 0, -DELTA]) Z_POS = np.array([0, 0, DELTA]) done = False # A dictionary, mapping from a character to a tuple. # First element in tuple is a function, second is arguments to be # unpacked and passed to the function, third is the string to display # to user. bindings = { # key: (function, args, description) 'h': (X_NEG, 'Move X by -DELTA'), 'l': (X_POS, 'Move X by +DELTA'), 'j': (Y_NEG, 'Move Y by -DELTA'), 'k': (Y_POS, 'Move Y by +DELTA'), 'm': (Z_NEG, 'Move Z by -DELTA'), 'i': (Z_POS, 'Move Z by +DELTA'), } ##### # Main UI loop ##### print('Welcome to interactive keyboard UI for gauging object center.') print('Press ? for help, Esc to quit.') print('') while not done and not rospy.is_shutdown(): # From baxter_examples joint_position_keyboard.py c = baxter_external_devices.getch() if c: # Catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True print ('Done. Returning object center %f %f %f' % \ (center[0], center[1], center[2])) break elif c in bindings: cmd = bindings[c] print("command: %s" % (cmd[1], )) # Expand binding to something like "set_j(right, 's0', 0.1)" center = move_delta(center, vis_pub, cmd[0], obj_height, obj_radius_x, obj_radius_y) print('New center: %f %f %f' % (center[0], center[1], center[2])) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[0]): print(" %s: %s" % (key, val[1])) return center
right_pose = PoseStamped() right_pose.header = hdr right_pose.pose.position.x = 0.3 right_pose.pose.position.y = -0.7 right_pose.pose.position.z = 0.2 right_pose.pose.orientation.x = 0 right_pose.pose.orientation.y = 1 right_pose.pose.orientation.z = 0 right_pose.pose.orientation.w = 0 done = False while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True # rospy.signal_shutdown("Example finished.") elif c == 'l': print "move left arm" move_to_pose(left_pose = left_pose, timeout = 4) elif c == 'r': print "move right arm" move_to_pose(right_pose = right_pose, timeout = 4) elif c == 's': request_object()
def map_keyboard(): # left = baxter_interface.Limb('left') print "this" left = BaxterArm('left') right = BaxterArm('right') kin = baxter_kinematics('left') config = ALL_CONFIGS['position_baxter'] ctrlr = OSPositionController(left, config) ctrlr.set_active(True) grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.joint_names() leftdes = [] leftacc = [] lefterr = [] delta = 0.05 #was 0.05 gain = 1. def IK(limb, movement): current_pos, curr_ori = limb.ee_pose() goal_pos = current_pos + movement goal_ori = curr_ori ctrlr.set_goal(goal_pos=goal_pos, goal_ori=goal_ori, orientation_ctrl=True) lin_error, ang_error, success, time_elapsed = ctrlr.wait_until_goal_reached( timeout=1.0) #print (pos[1]-np.array(limb.endpoint_pose()['position'])[1])/delta 0.91 for x, 0.82 for z # print (pos[2]-np.array(limb.endpoint_pose()['position'])[2])/delta bindings = { # key: (function, args, description) 's': (IK, [left, [delta, 0, 0]], "xinc"), 'd': (IK, [left, [-delta, 0, 0]], "xdec"), 'w': (IK, [left, [0, delta, 0]], "yinc"), 'e': (IK, [left, [0, -delta, 0]], "ydec"), 'x': (IK, [left, [0, 0, delta]], "zinc"), 'c': (IK, [left, [0, 0, -delta]], "zdec"), #'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"), 'b': (grip_left.close, [], "left: gripper close"), 'n': (grip_left.open, [], "left: gripper open"), 'm': (grip_left.calibrate, [], "left: gripper calibrate") #comma here? } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 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 __move(self, wp, accuracy=1.0): rate = rospy.Rate(10.0) if self.left_arm_enabled: if wp.lgrip < 50: self.left_grip.close() else: self.left_grip.open() if self.right_arm_enabled: if wp.rgrip < 50: self.right_grip.close() else: self.right_grip.open() done = False if self.left_arm_enabled: self.left_arm.set_joint_position_speed( max(min(Script.MAX_VEL * wp.vel / 100.0, 1.0), 0.0)) #default 0.3, range 0.0 - 1.0 if self.right_arm_enabled: self.right_arm.set_joint_position_speed( max(min(Script.MAX_VEL * wp.vel / 100.0, 1.0), 0.0)) #default 0.3, range 0.0 - 1.0 dd = [9999.0, 9999.0, 9999.0, 9999.0, 9999.0] while not done: if self.cancel_move: break if self.left_arm_enabled: self.left_arm.set_joint_positions(wp.ljpos) if self.right_arm_enabled: self.right_arm.set_joint_positions(wp.rjpos) dist = 0.0 if self.left_arm_enabled: for j in self.left_arm.joint_names(): dist = dist + (wp.ljpos[j] - self.left_arm.joint_angle( j)) * (wp.ljpos[j] - self.left_arm.joint_angle(j)) if self.right_arm_enabled: for j in self.right_arm.joint_names(): dist = dist + (wp.rjpos[j] - self.right_arm.joint_angle( j)) * (wp.rjpos[j] - self.right_arm.joint_angle(j)) dist = math.sqrt(dist) ss = 0.0 for d in dd: ss = ss + math.fabs(d - dist) if ss / (wp.vel / 100.0) < 0.1 / accuracy: done = True elif rospy.is_shutdown(): print("ROS shutdown, cancelled movement") done = True elif baxter_external_devices.getch() in ['\x1b', '\x03']: print("stopped") done = True else: dd.append(dist) del dd[0] rate.sleep() if self.left_arm_enabled: self.left_arm.set_joint_position_speed(0.3) if self.right_arm_enabled: self.right_arm.set_joint_position_speed(0.3)
def map_keyboard(): left = baxter_interface.Limb("left") right = baxter_interface.Limb("right") grip_left = baxter_interface.Gripper("left") grip_right = baxter_interface.Gripper("right") lj = left.joint_names() rj = right.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) bindings = { # key: (function, args, description) "9": (set_j, [left, lj[0], 0.1], "left_s0 increase"), "6": (set_j, [left, lj[0], -0.1], "left_s0 decrease"), "8": (set_j, [left, lj[1], 0.1], "left_s1 increase"), "7": (set_j, [left, lj[1], -0.1], "left_s1 decrease"), "o": (set_j, [left, lj[2], 0.1], "left_e0 increase"), "y": (set_j, [left, lj[2], -0.1], "left_e0 decrease"), "i": (set_j, [left, lj[3], 0.1], "left_e1 increase"), "u": (set_j, [left, lj[3], -0.1], "left_e1 decrease"), "l": (set_j, [left, lj[4], 0.1], "left_w0 increase"), "h": (set_j, [left, lj[4], -0.1], "left_w0 decrease"), "k": (set_j, [left, lj[5], 0.1], "left_w1 increase"), "j": (set_j, [left, lj[5], -0.1], "left_w1 decrease"), ".": (set_j, [left, lj[6], 0.1], "left_w2 increase"), "n": (set_j, [left, lj[6], -0.1], "left_w2 decrease"), ",": (grip_left.close, [], "left: gripper close"), "m": (grip_left.open, [], "left: gripper open"), "/": (grip_left.calibrate, [], "left: gripper calibrate"), "4": (set_j, [right, rj[0], 0.1], "right_s0 increase"), "1": (set_j, [right, rj[0], -0.1], "right_s0 decrease"), "3": (set_j, [right, rj[1], 0.1], "right_s1 increase"), "2": (set_j, [right, rj[1], -0.1], "right_s1 decrease"), "r": (set_j, [right, rj[2], 0.1], "right_e0 increase"), "q": (set_j, [right, rj[2], -0.1], "right_e0 decrease"), "e": (set_j, [right, rj[3], 0.1], "right_e1 increase"), "w": (set_j, [right, rj[3], -0.1], "right_e1 decrease"), "f": (set_j, [right, rj[4], 0.1], "right_w0 increase"), "a": (set_j, [right, rj[4], -0.1], "right_w0 decrease"), "d": (set_j, [right, rj[5], 0.1], "right_w1 increase"), "s": (set_j, [right, rj[5], -0.1], "right_w1 decrease"), "v": (set_j, [right, rj[6], 0.1], "right_w2 increase"), "z": (set_j, [right, rj[6], -0.1], "right_w2 decrease"), "c": (grip_right.close, [], "right: gripper close"), "x": (grip_right.open, [], "right: gripper open"), "b": (grip_right.calibrate, [], "right: gripper calibrate"), } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] # expand binding to something like "set_j(right, 's0', 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 map_keyboard(): # initialize interfaces print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled left = baxter_interface.Gripper('left', CHECK_VERSION) right = baxter_interface.Gripper('right', CHECK_VERSION) def clean_shutdown(): if not init_state: print("Disabling robot...") rs.disable() print("Exiting example.") rospy.on_shutdown(clean_shutdown) def capability_warning(gripper, cmd): msg = ("%s %s - not capable of '%s' command" % (gripper.name, gripper.type(), cmd)) rospy.logwarn(msg) def offset_position(gripper, offset): if gripper.type() != 'electric': capability_warning(gripper, 'command_position') return current = gripper.position() gripper.command_position(current + offset) def offset_holding(gripper, offset): if gripper.type() != 'electric': capability_warning(gripper, 'set_holding_force') return current = gripper.parameters()['holding_force'] gripper.set_holding_force(current + offset) def offset_moving(gripper, offset): if gripper.type() != 'electric': capability_warning(gripper, 'set_moving_force') return current = gripper.parameters()['moving_force'] gripper.set_moving_force(current + offset) def offset_velocity(gripper, offset): if gripper.type() != 'electric': capability_warning(gripper, 'set_velocity') return current = gripper.parameters()['velocity'] gripper.set_velocity(current + offset) def offset_dead_band(gripper, offset): if gripper.type() != 'electric': capability_warning(gripper, 'set_dead_band') return current = gripper.parameters()['dead_zone'] gripper.set_dead_band(current + offset) bindings = { # key: (function, args, description) 'r': (left.reboot, [], "left: reboot"), 'R': (right.reboot, [], "right: reboot"), 'c': (left.calibrate, [], "left: calibrate"), 'C': (right.calibrate, [], "right: calibrate"), 'q': (left.close, [], "left: close"), 'Q': (right.close, [], "right: close"), 'w': (left.open, [], "left: open"), 'W': (right.open, [], "right: open"), '[': (left.set_velocity, [100.0], "left: set 100% velocity"), '{': (right.set_velocity, [100.0], "right: set 100% velocity"), ']': (left.set_velocity, [30.0], "left: set 30% velocity"), '}': (right.set_velocity, [30.0], "right: set 30% velocity"), 's': (left.stop, [], "left: stop"), 'S': (right.stop, [], "right: stop"), 'z': (offset_dead_band, [left, -1.0], "left: decrease dead band"), 'Z': (offset_dead_band, [right, -1.0], "right: decrease dead band"), 'x': (offset_dead_band, [left, 1.0], "left: increase dead band"), 'X': (offset_dead_band, [right, 1.0], "right: increase dead band"), 'f': (offset_moving, [left, -5.0], "left: decrease moving force"), 'F': (offset_moving, [right, -5.0], "right: decrease moving force"), 'g': (offset_moving, [left, 5.0], "left: increase moving force"), 'G': (offset_moving, [right, 5.0], "right: increase moving force"), 'h': (offset_holding, [left, -5.0], "left: decrease holding force"), 'H': (offset_holding, [right, -5.0], "right: decrease holding force"), 'j': (offset_holding, [left, 5.0], "left: increase holding force"), 'J': (offset_holding, [right, 5.0], "right: increase holding force"), 'v': (offset_velocity, [left, -5.0], "left: decrease velocity"), 'V': (offset_velocity, [right, -5.0], "right: decrease velocity"), 'b': (offset_velocity, [left, 5.0], "left: increase velocity"), 'B': (offset_velocity, [right, 5.0], "right: increase velocity"), 'u': (offset_position, [left, -15.0], "left: decrease position"), 'U': (offset_position, [right, -15.0], "right: decrease position"), 'i': (offset_position, [left, 15.0], "left: increase position"), 'I': (offset_position, [right, 15.0], "right: increase position"), } done = False print("Enabling robot... ") rs.enable() print("Controlling grippers. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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 run_control(joystick): # Initialise parameters kbOnly = True if joystick==None else False print('kbOnly: ', kbOnly) #abbreviations if kbOnly == False: jhi = lambda s: joystick.stick_value(s) > 0 jlo = lambda s: joystick.stick_value(s) < 0 bdn = joystick.button_down bup = joystick.button_up zeros = [0]*4 initial_inc = 0.03 inc = initial_inc delta = 0.01 incU = 0.02 incL = 0.01 syncErrThresh = 0.003 vrepZdelta = 1.08220972 # Service to call functions in V-rep (e.g. set/get joint positions) vrepScriptFunc = rospy.ServiceProxy('vrep/simRosCallScriptFunction', simRosCallScriptFunction) # Get robot interface robot = RobotArm(vrepScriptFunc) robot.updPos('left') # Get appropriate bindings kb_b= js_b = False def gen_js_bindings(inc): bindings = ( ((bdn, ['btnUp']), (robot.grip_l_close, []), "left gripper"), ((jhi, ['rightTrigger']), (inc_delta, [inc, delta, incU, incL]), "Speed increased"), ((jhi, ['leftTrigger']), (inc_delta, [inc, -delta, incU, incL]), "Speed decreased"), ((bdn, ['leftBumper']), (robot.pos_to_ori, ['down']), "Switched to orientation"), ((bup, ['leftBumper']), (robot.pos_to_ori, ['up']), "Switched to position"), ((bdn, ['rightBumper']), (robot.world_to_ee, ['ee']), "Switched to tool frame"), ((bup, ['rightBumper']), (robot.world_to_ee, ['world']), "Switched to world frame"), ((jlo, ['leftStickHorz']), (robot.command_ik, ['left', [0, -inc, 0]]), lambda: "left y dec "), ((jhi, ['leftStickHorz']), (robot.command_ik, ['left', [0, inc, 0]]), lambda: "left y inc "), ((jlo, ['leftStickVert']), (robot.command_ik, ['left', [-inc, 0, 0]]), lambda: "left x dec "), ((jhi, ['leftStickVert']), (robot.command_ik, ['left', [inc, 0, 0]]), lambda: "left x inc "), ((jlo, ['rightStickVert']), (robot.command_ik, ['left', [0, 0, -inc]]), lambda: "left z dec "), ((jhi, ['rightStickVert']), (robot.command_ik, ['left', [0, 0, inc]]), lambda: "left z inc "), ((bdn, ['function1']), (print_js_bindings, [js_b]), "help"), ((bdn, ['function2']), (print_js_bindings, [js_b]), "help"), ) return bindings def gen_kb_bindings(inc): bindings = { '9': (inc_delta, [inc, delta, incU, incL], "Speed increased"), '0': (inc_delta, [inc, -delta, incU, incL], "Speed decreased"), 'i': (robot.pos_to_ori, [], "Switch position/orientation"), # 'o': (robot.world_to_ee, [], "Switch world/tool frame"), 'w': (robot.command_ik, ['left', [inc, 0, 0]], "increase left x"), 's': (robot.command_ik, ['left', [-inc, 0, 0]], "decrease left x"), 'a': (robot.command_ik, ['left', [0, inc, 0]], "increase left y"), 'd': (robot.command_ik, ['left', [0, -inc, 0]], "decrease left y"), 'q': (robot.command_ik, ['left', [0, 0, inc]], "increase left z"), 'e': (robot.command_ik, ['left', [0, 0, -inc]], "decrease left z"), 'p': (robot.grip_l_close, [], "Gripper open/close"), # 'l': (robot.grip_l_open, [], "left: gripper open"), # 'c': (grip_left.calibrate, [], "left: gripper calibrate") } return bindings if kbOnly == False: js_b = [] bindings = gen_js_bindings(inc) js_b.append(bindings) kb_b = gen_kb_bindings(inc) # Print bindings if kbOnly == False: print_js_bindings(js_b) else: print_kb_bindings(kb_b) # Setup data recording endpoint_state=EndpointState() def on_endpoint_states(msg,endpoint_state): endpoint_state.header= msg.header endpoint_state.pose= msg.pose endpoint_state.twist= msg.twist endpoint_state.wrench= msg.wrench recording=False done_recording=False def record_data(filename): # Subscribe to endpoint state # _cartesian_state_sub = rospy.Subscriber( # '/robot/limb/left/endpoint_state', # EndpointState, # on_endpoint_states, # endpoint_state, # queue_size=1, # tcp_nodelay=True) # bag = rosbag.Bag(filename, 'w') def terminate_process_and_children(s): list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE) list_output = list_cmd.stdout.read() retcode = list_cmd.wait() assert retcode == 0, "List command returned %d" % retcode for str in list_output.split("\n"): if (str.startswith(s)): os.system("rosnode kill " + str) # while not done_recording: # if recording==True: # print('Endpoint actual: ',endpoint_state) # bag.write('endpoint_state',endpoint_state) rt = rospy.Rate(100) while not recording: rt.sleep() p = subprocess.Popen('rosbag record -j -O 5.bag /robot/limb/left/endpoint_state ' '/camera1/rgb/image_color /camera2/rgb/image_color ', # '/cameras/left_hand_camera/image', stdin=subprocess.PIPE,shell=True, cwd='/home/domenico/talha_ws/src/baxter_ik/data/id_7/') # /home/talha/test_ws/src/baxter_ik/scripts/ # print 'Finishing recording' # bag.close() while not done_recording: rt.sleep() print 'Finishing recording' terminate_process_and_children("/record") # thread.start_new_thread(record_data, ('test.bag',)) then = rospy.get_rostime() # Move to start position move_to_start_pos(robot,syncErrThresh) vrepScriptFunc.call('set_ik_mode@Baxter_leftArm_target',1,[],[],[],'') prevPosVrep = None rate = rospy.Rate(100) # while loop while not rospy.is_shutdown(): # inputControl() if kbOnly == True: c = baxter_external_devices.getch(-1) if c: # print('Pressed: ',c) recording=True #catch Esc or ctrl-c if c in ['\x1b', '\x03']: # terminate_process_and_children(p) rospy.signal_shutdown("Example finished.") if c == '\r': done_recording=True # Keyboard control if c in kb_b: cmd = kb_b[c] if cmd[0] == inc_delta: inc = cmd[0](*cmd[1]) kb_b = gen_kb_bindings(inc) elif cmd[0] == robot.pos_to_ori: # kb_b = gen_kb_bindings(initial_inc) cmd[0](*cmd[1]) else: cmd[0](*cmd[1]) # print("command: %s" % (cmd[2],)) else: # Joystick control for (test, cmd, doc) in bindings: then = rospy.get_rostime() if test[0](*test[1]): recording=True if cmd[0] == inc_delta: inc = cmd[0](*cmd[1]) bindings = gen_js_bindings(inc) js_b[0] = bindings elif cmd[0] == robot.pos_to_ori: # bindings = gen_js_bindings(initial_inc) # js_b[0] = bindings cmd[0](*cmd[1]) else: cmd[0](*cmd[1]) if callable(doc): print(doc()) else: print(doc) # Check if IK was a success in V-rep resp = vrepScriptFunc.call('ikSuccess@Baxter_leftArm_target',1,[],[],[],'') # print 'Ik Success: ', resp.outputInts # If successful, receive & send joints to Baxter if resp.outputInts[0] == -1: # Get resulting joint positions from V-rep resp = vrepScriptFunc.call('get_jnt_pos@Baxter_leftArm_target',1,[],[],[],'') if prevPosVrep is None: prevPosVrep = [0,0,0,0,0,0,0] err2 = np.linalg.norm(np.array(resp.outputFloats) - np.array(prevPosVrep)) # print(resp.outputFloats) # print(prevPosVrep) prevPosVrep = copy.deepcopy(resp.outputFloats) # Get Baxter joints curr_jnts = np.array([robot.left.joint_angles()[name] for name in robot.left.joint_names()]) # Compare joints norm = np.linalg.norm(curr_jnts) err = math.fabs(np.linalg.norm(np.array(resp.outputFloats)-curr_jnts)) err = err/norm # print('Error: ',err) # print('ErrorVrep: ', err2) # limb_joints = dict(zip(robot.left._joint_names['left'], resp.outputFloats)) # robot.left.set_joint_positions(limb_joints) # robot.syncPos('left') if err > abs(syncErrThresh): if err2 > 0.0001: # print('Position updated') # Wrap and send joint positions to Baxter limb_joints = dict(zip(robot.left._joint_names['left'], resp.outputFloats)) diff = rospy.get_rostime() - then # print ('Time taken:', diff.to_sec()) robot.left.set_joint_positions(limb_joints) curr_jnts = np.array([robot.left.joint_angles()[name] for name in robot.left.joint_names()]) jnt_err = np.zeros((7, 1)) for x in range(curr_jnts.size): jnt_err[x] = curr_jnts[x] - resp.outputFloats[x] # print('Joint difference: ', jnt_err.tolist()) # print('V-rep Joints: ',limb_joints) # resp = vrepScriptFunc.call('get_pose@Baxter_leftArm_target',1,[],[],[],'') # print 'V-rep pose: ',resp.outputFloats # print('---------------------------------------------------------------------------------'/0) # print 'Finish pose: ',current_p # print('Actual joints: ', robot.left.joint_angles()) # print('---------------------------------------------------------------------------------') rate.sleep()
def map_keyboard(): #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: ")) def judge(dict): if (abs(limb.joint_angle(joints[0])-theta['right_s0'])>0.1 or abs(limb.joint_angle(joints[1])-theta['right_s1'])>0.1 or abs(limb.joint_angle(joints[2])-theta['right_e0'])>0.1 or abs(limb.joint_angle(joints[3])-theta['right_e1'])>0.1 or abs(limb.joint_angle(joints[4])-theta['right_w0'])>0.1 or abs(limb.joint_angle(joints[5])-theta['right_w1'])>0.1 or abs(limb.joint_angle(joints[6])-theta['right_w2'])>0.1): return True else: return False while(judge(theta)): limb.set_joint_positions(theta) time.sleep(0.01) limb = intera_interface.Limb(side) left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.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) bindings = { # key: (function, args, description) '9': (set_j, [left, lj[0], 0.1], "left_s0 increase"), '6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"), '8': (set_j, [left, lj[1], 0.1], "left_s1 increase"), '7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"), 'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"), 'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"), 'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"), 'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"), 'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"), 'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"), 'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"), 'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"), '.': (set_j, [left, lj[6], 0.1], "left_w2 increase"), 'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"), ',': (grip_left.close, [], "left: gripper close"), 'm': (grip_left.open, [], "left: gripper open"), '/': (grip_left.calibrate, [], "left: gripper calibrate"), '4': (set_j, [right, rj[0], 0.1], "right_s0 increase"), '1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"), '3': (set_j, [right, rj[1], 0.1], "right_s1 increase"), '2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"), 'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"), 'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"), 'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"), 'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"), 'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"), 'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"), 'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"), 's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"), 'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"), 'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"), 'c': (grip_right.close, [], "right: gripper close"), 'x': (grip_right.open, [], "right: gripper open"), 'b': (grip_right.calibrate, [], "right: gripper calibrate"), } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 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 map_keyboard(): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.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) bindings = { # key: (function, args, description) '9': (set_j, [left, lj[0], 0.005], "left_s0 increase"), '6': (set_j, [left, lj[0], -0.005], "left_s0 decrease"), '8': (set_j, [left, lj[1], 0.005], "left_s1 increase"), '7': (set_j, [left, lj[1], -0.005], "left_s1 decrease"), 'o': (set_j, [left, lj[2], 0.005], "left_e0 increase"), 'y': (set_j, [left, lj[2], -0.005], "left_e0 decrease"), 'i': (set_j, [left, lj[3], 0.005], "left_e1 increase"), 'u': (set_j, [left, lj[3], -0.005], "left_e1 decrease"), 'l': (set_j, [left, lj[4], 0.005], "left_w0 increase"), 'h': (set_j, [left, lj[4], -0.005], "left_w0 decrease"), 'k': (set_j, [left, lj[5], 0.005], "left_w1 increase"), 'j': (set_j, [left, lj[5], -0.005], "left_w1 decrease"), '.': (set_j, [left, lj[6], 0.005], "left_w2 increase"), 'n': (set_j, [left, lj[6], -0.005], "left_w2 decrease"), ',': (grip_left.close, [], "left: gripper close"), 'm': (grip_left.open, [], "left: gripper open"), '/': (grip_left.calibrate, [], "left: gripper calibrate"), '4': (set_j, [right, rj[0], 0.005], "right_s0 increase"), '1': (set_j, [right, rj[0], -0.005], "right_s0 decrease"), '3': (set_j, [right, rj[1], 0.005], "right_s1 increase"), '2': (set_j, [right, rj[1], -0.005], "right_s1 decrease"), 'r': (set_j, [right, rj[2], 0.005], "right_e0 increase"), 'q': (set_j, [right, rj[2], -0.005], "right_e0 decrease"), 'e': (set_j, [right, rj[3], 0.005], "right_e1 increase"), 'w': (set_j, [right, rj[3], -0.005], "right_e1 decrease"), 'f': (set_j, [right, rj[4], 0.005], "right_w0 increase"), 'a': (set_j, [right, rj[4], -0.005], "right_w0 decrease"), 'd': (set_j, [right, rj[5], 0.005], "right_w1 increase"), 's': (set_j, [right, rj[5], -0.005], "right_w1 decrease"), 'v': (set_j, [right, rj[6], 0.005], "right_w2 increase"), 'z': (set_j, [right, rj[6], -0.005], "right_w2 decrease"), 'c': (grip_right.close, [], "right: gripper close"), 'x': (grip_right.open, [], "right: gripper open"), 'b': (grip_right.calibrate, [], "right: gripper calibrate"), } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "set_j(right, 's0', 0.005)" 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 map_keyboard(): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) #These are from PyKDL and are needed for the Jacobian left_kin = baxter_kinematics('left') right_kin = baxter_kinematics('right') #Connect with IK service right_iksvc, right_ns = ik_command.connect_service('right') left_iksvc, left_ns = ik_command.connect_service('left') def command_jacobian(side, direction): #UNUSED if side == 'left': limb = left kin = left_kin elif side == 'right': limb = right kin = right_kin else: raise Exception("Got wrong side in command_jacobian") # current is the current position of end effector current_p = np.array(limb.endpoint_pose()['position'] + limb.endpoint_pose()['orientation']) current_p = current_q.reshape((7, 1)) current = np.zeros((6, 1)) quaternion_to_euler(current_p, current) # We need to convert the direction from the world frame to the hand frame # Rotate direction by the inverse of the rotation matrix from the angular pose in current R = tf.transformations.quaternion_matrix( current_p[3:7].flatten().tolist()) R_inv = tf.transformations.inverse_matrix(R) print R_inv[0:3, 0:3] direction = np.array(direction).reshape((6, 1)) print direction[0:3] dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3]) print dir_rot #direction[0:3] = dir_rot # Goal is the goal position, found by adding the requested direction from the user goal = current + direction current_angles = np.array( [limb.joint_angles()[name] for name in limb.joint_names()]) # Get the Jacobian inverse and solve for the necessary change in joint angles jacobian_inv = kin.jacobian_transpose() delta = jacobian_inv * (goal - current) commands = current_angles.flatten() + delta.flatten() # Command the joints command_list = commands.tolist() joint_command = dict(zip(limb.joint_names(), command_list[0])) limb.set_joint_positions(joint_command) def command_ik(side, direction): """Use the Rethink IK service to figure out a desired joint position""" if side == 'left': limb = left iksvc = left_iksvc ns = left_ns else: limb = right iksvc = right_iksvc ns = right_ns current_p = np.array(limb.endpoint_pose()['position'] + limb.endpoint_pose()['orientation']) direction = np.array(direction) desired_p = current_p + direction ik_command.service_request(iksvc, desired_p, side, False) zeros = [0] * 4 inc = 0.1 bindings = { 'q': (command_ik, ['right', [inc, 0, 0] + zeros], "increase right x"), 'a': (command_ik, ['right', [-inc, 0, 0] + zeros], "decrease right x"), 'w': (command_ik, ['right', [0, inc, 0] + zeros], "increase right y"), 's': (command_ik, ['right', [0, -inc, 0] + zeros], "decrease right y"), 'e': (command_ik, ['right', [0, 0, inc] + zeros], "increase right z"), 'd': (command_ik, ['right', [0, 0, -inc] + zeros], "decrease right z"), 'u': (command_ik, ['left', [inc, 0, 0] + zeros], "increase left x"), 'j': (command_ik, ['left', [-inc, 0, 0] + zeros], "decrease left x"), 'i': (command_ik, ['left', [0, inc, 0] + zeros], "increase left y"), 'k': (command_ik, ['left', [0, -inc, 0] + zeros], "decrease left y"), 'o': (command_ik, ['left', [0, 0, inc] + zeros], "increase left z"), 'l': (command_ik, ['left', [0, 0, -inc] + zeros], "decrease left z"), 'z': (grip_right.close, [], "right: gripper close"), 'x': (grip_right.open, [], "right: gripper open"), 'c': (grip_right.calibrate, [], "right: gripper calibrate") } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_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] #expand binding to something like "command_jacobian(left, [0.1, 0, 0])" 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]))
self.left_arm.move_to_joint_positions(l_angles) self.right_arm.move_to_joint_positions(r_angles) if __name__ == '__main__': print("Welcome.") rospy.init_node("rethink_rsdk_joint_velocity") #print("Getting Baxter's state... ") rs = baxter_interface.RobotEnable() print("Attempting to enable Baxter") rs.enable() task_completer = Mover('both') task_completer.move() print("Completing tasks. Press Esc to quit.") done = False while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True else: print("Press Esc to quit.") print("All tasks completed.") print("Disabling Baxter... ") rs.disable() print("done.")
def run(self): #print "Recording Grids Location..." #self.grids_location_recorder() #print "Recording Grid Roi..." #self.grids_roi_recorder() cur_arm = 'left' step = 0.004 print "Input: " while not rospy.is_shutdown(): c = baxter_external_devices.getch(1.0) if c: if c in ['w']: pose = self.current_poses['left'] x = pose.pose.position.x y = pose.pose.position.y z = -0.0247 #pose.pose.position.z ox = 0.0 oy = 1.0 oz = 0.0 ow = 0.0 print " " print "Get w" x = x + step cur_arm = 'left' elif c in ['z']: pose = self.current_poses['left'] x = pose.pose.position.x y = pose.pose.position.y z = -0.0247 #pose.pose.position.z ox = 0.0 oy = 1.0 oz = 0.0 ow = 0.0 print " " print "Get z" x = x - step cur_arm = 'left' elif c in ['a']: pose = self.current_poses['left'] x = pose.pose.position.x y = pose.pose.position.y z = -0.0247 #pose.pose.position.z ox = 0.0 oy = 1.0 oz = 0.0 ow = 0.0 print " " print "Get a" y = y + step cur_arm = 'left' elif c in ['s']: pose = self.current_poses['left'] x = pose.pose.position.x y = pose.pose.position.y z = -0.0247 #pose.pose.position.z ox = 0.0 oy = 1.0 oz = 0.0 ow = 0.0 print " " print "Get s" y = y - step cur_arm = 'left' if c in ['i']: pose = self.current_poses['right'] x = pose.pose.position.x y = pose.pose.position.y z = -0.0247 #pose.pose.position.z ox = 0.0 oy = 1.0 oz = 0.0 ow = 0.0 print " " print "Get i" x = x + step cur_arm = 'right' elif c in ['m']: pose = self.current_poses['right'] x = pose.pose.position.x y = pose.pose.position.y z = -0.0247 #pose.pose.position.z ox = 0.0 oy = 1.0 oz = 0.0 ow = 0.0 print " " print "Get m" x = x - step cur_arm = 'right' elif c in ['j']: pose = self.current_poses['right'] x = pose.pose.position.x y = pose.pose.position.y z = -0.0247 #pose.pose.position.z ox = 0.0 oy = 1.0 oz = 0.0 ow = 0.0 print " " print "Get j" y = y + step cur_arm = 'right' elif c in ['k']: pose = self.current_poses['right'] x = pose.pose.position.x y = pose.pose.position.y z = -0.0247 #pose.pose.position.z ox = 0.0 oy = 1.0 oz = 0.0 ow = 0.0 y = y - step cur_arm = 'right' print " " print "Get k" else: rospy.sleep(0.1) continue msg_string = cur_arm + ':move_to:' + \ str(x) + \ ',' + \ str(y) + \ ',' + \ str(z) + \ ',' + \ str(ox) + \ ',' + \ str(oy) + \ ',' + \ str(oz) + \ ',' + \ str(ow) self.ArmCmdPub.publish(msg_string) rospy.sleep(0.1)