diff --git a/kits/arm/gui_TeachAndRepeat.py b/kits/arm/gui_TeachAndRepeat.py new file mode 100644 index 00000000..a8f80469 --- /dev/null +++ b/kits/arm/gui_TeachAndRepeat.py @@ -0,0 +1,324 @@ +''' +APPLE's DEMO SCRIPTS - HEBI 6DOF ARM EASY Teach and Repeat + +Works with latest Python API +''' + +# Importing the needed libraries + +import hebi +import numpy as np +from tkinter import * # Tkinter is the built in PYTHON GUI +from time import sleep +from hebi import arm as arm_api +from hebi.util import create_mobile_io + +lookup = hebi.Lookup() +# Give the Lookup process 2 seconds to discover modules +sleep(2) + +# Print all the modules on the network +print('Modules found on network:') +for entry in lookup.entrylist: + print(f'{entry.family} | {entry.name}') + +# Arm Setup +arm_family = "Arm" +module_names = ['J1_base', 'J2_shoulder', 'J3_elbow', 'J4_wrist1', 'J5_wrist2', 'J6_wrist3'] +hrdf_file = "hrdf/A-2085-06G.hrdf" +gains_file = "gains/A-2085-06.xml" # make sure this file in the same folder as this program + +arm = arm_api.create([arm_family], + names=module_names, + hrdf_file=hrdf_file) +arm.load_gains(gains_file) + + +# Add the gripper +gripper_family = arm_family +gripper_name = 'gripperSpool' +gripper = arm_api.Gripper(lookup.get_group_from_names([gripper_family], [gripper_name]), -5, 1) +gripper.load_gains("gains/gripper_spool_gains.xml") +arm.set_end_effector(gripper) + + +# Create mobileIO object +phone_name = "mobileIO" +print('Waiting for Mobile IO device to come online...') +m = create_mobile_io(lookup, arm_family, phone_name) +if m is None: + raise RuntimeError("Could not find Mobile IO device") +m.set_led_color("blue") # as we start in grav comp +m.clear_text() # Clear any garbage on screen +m.update() + +# Demo Variables +abort_flag = False +pending_goal = False +run_mode = "training" +goal = arm_api.Goal(arm.size) + +# Print Instructions +instructions = """B1 - Add waypoint (stop) +B2 - Add waypoint (stop) and toggle the gripper +B3 - Add waypoint (flow) +B5 - Toggle training/playback +B6 - Clear waypoints +A3 - Up/down for longer/shorter time to waypoint +B8 - Quit +""" +print(instructions) +m.set_text(instructions) + +####################### +## Main Control Loop ## +####################### + +# Set Resolution of the canvas +resx = 800 +resy = 600 + + +def init(data): + data.margin = 20 # Spacing for every new waypoint + data.timerDelay = 5 + + # Create a list of Buttons to be drawn on the canvas + data.buttonList = ["B1 \n Add Stop Waypoint ", + "B2 \n Add Stop \n Toggle the gripper", + "B3 \n Add Flow Waypoint", + "B5 \n Toggle \n Training/Playback", + "B6 \n Clear Waypoints", + "B8 \n Quit"] + data.buttonRows = len(data.buttonList) + data.buttonColors =["yellow"] * data.buttonRows + + data.buttonPress = np.zeros((data.buttonRows,1)) + + # Sizing values for the buttons + data.buttonwidth = 150 + data.buttonheight = 75 + + data.buttonPos = createButtonPos(data) + + # Initializing list to keep track of waypoints + data.run_mode = "training" + data.goal = np.empty((0,3), dtype=np.float64) + #data.goal = np.zeros((1,3), dtype=np.float64) + data.time = np.array([]) + data.gripper = np.array([]) + + +def mousePressed(event, data): + buttonCheck(event,data) + +def keyPressed(event, data): + pass + +def createButtonPos(data): + + # Creating the list of rectangle coords + buttonRows = len(data.buttonList) + pointsDefRect = 4 + + buttonPos = np.zeros((buttonRows,pointsDefRect)) + # Displaying all the buttons on the screen + for row in range(buttonRows): + (x1,y1) = (3*resx/4, (data.margin) + ((data.margin+data.buttonheight) * row)) + (x2,y2) = (3*resx/4 + data.buttonwidth, (data.margin + data.buttonheight) + (data.margin + data.buttonheight)*row) + buttonPos[row] = [x1,y1,x2,y2] + + + + return buttonPos + +def buttonCheck(event,data): + mx = event.x + my = event.y + for row in range(data.buttonRows): + (x1,y1,x2,y2) = data.buttonPos[row] + if (x1 < mx < x2) and (y1 < my < y2): + data.buttonPress[row] = 1 + + +def timerFired(data): + #updatewaypoints(data) + arm.update() + + if not m.update(): + print("Failed to get feedback from MobileIO") + + slider3 = m.get_axis_state(3) + + # Set all the button colors to yellow + data.buttonColors =["yellow"]*len(data.buttonList) + + if m.get_button_diff(8) == 3 or data.buttonPress[5] == 1: # "ToOn" + m.set_led_color("transparent") + m.clear_text() + print("Shutting Down the Program") + data.buttonColors[5] = "blue" + abort_flag = True + quit() + + # Keep Button 5 as blue when the code is in playback mode + if data.run_mode == "playback": + data.buttonColors[3] = "blue" + + if data.run_mode == "training": + # B1 add waypoint (stop) + if m.get_button_diff(1) == 3 or data.buttonPress[0] == 1: # "ToOn" + print("Stop waypoint added") + goal.add_waypoint(t=slider3 + 3.0, position=arm.last_feedback.position, aux=gripper.state, velocity=[0]*arm.size) + data.time = np.append(data.time, slider3 + 3.0) + data.gripper = np.append(data.gripper, 0) + data.goal=np.append(data.goal, [arm.FK(arm.last_feedback.position)], axis = 0) + data.buttonColors[0] = "blue" + + # B2 add waypoint (stop) and toggle the gripper + if m.get_button_diff(2) == 3 or data.buttonPress[1] == 1: # "ToOn" + # Add 2 waypoints to allow the gripper to open or close + print("Stop waypoint added and gripper toggled") + position = arm.last_feedback.position + goal.add_waypoint(t=slider3 + 3.0, position=position, aux=gripper.state, velocity=[0]*arm.size) + gripper.toggle() + goal.add_waypoint(t=2.0, position=position, aux=gripper.state, velocity=[0]*arm.size) + + data.time = np.append(data.time, slider3 + 5.0) + data.gripper = np.append(data.gripper, 1) + data.goal=np.append(data.goal, [arm.FK(arm.last_feedback.position)], axis = 0) + data.buttonColors[1] = "blue" + + # B3 add waypoint (flow) + if m.get_button_diff(3) == 3 or data.buttonPress[2] == 1: # "ToOn" + print("Flow waypoint added") + goal.add_waypoint(t=slider3 + 3.0, position=arm.last_feedback.position, aux=gripper.state) + data.time = np.append(data.time, slider3 + 3.0) + data.gripper = np.append(data.gripper, 0) + data.goal=np.append(data.goal, [arm.FK(arm.last_feedback.position)], axis = 0) + data.buttonColors[2] = "blue" + + # B5 toggle training/playback + if m.get_button_diff(5) == 3 or data.buttonPress[3] == 1: # "ToOn" + # Check for more than 2 waypoints + if goal.waypoint_count > 1: + print("Transitioning to playback mode") + data.run_mode = "playback" + m.set_led_color("green") + arm.set_goal(goal) + else: + print("At least two waypoints are needed") + data.buttonColors[3] = "blue" + + # B6 clear waypoints + if m.get_button_diff(6) == 3 or data.buttonPress[4] == 1: # "ToOn" + print("Waypoints cleared") + goal.clear() + data.goal = np.zeros((1,3), dtype=np.float64) + data.time = np.array([]) + data.gripper = np.array([]) + data.buttonColors[4] = "blue" + + elif data.run_mode == "playback": + # B5 toggle training/playback + if m.get_button_diff(5) == 3 or data.buttonPress[3] == 1: # "ToOn" + print("Transitioning to training mode") + data.run_mode = "training" + m.set_led_color("blue") + arm.cancel_goal() + + # replay through the path again once the goal has been reached + if arm.at_goal: + arm.set_goal(goal) + data.buttonPress = np.zeros((data.buttonRows,1)) #Reset the buttonClick + arm.send() + +def drawText(canvas,data): + + # Draws First Row Text + canvas.create_text(5, data.margin, text="Waypoints",anchor= SW) + canvas.create_text(150, data.margin, text="Joints",anchor= SW) + canvas.create_text(300, data.margin, text="Time",anchor= SW) + canvas.create_text(400, data.margin, text="Gripper",anchor= SW) + + # Draw Button Text + buttonRows = len(data.buttonList) + + # Displaying all the buttons on the screen + for row in range(buttonRows): + (x1,y1,x2,y2) = data.buttonPos[row] + + canvas.create_rectangle(x1,y1,x2,y2, + fill= data.buttonColors[row]) + canvas.create_text(x1+(x2 - x1)/2, + y1+(y2 - y1)/2, + text= data.buttonList[row], justify = CENTER) + # Adding Waypoints information + + # Draw added goal positions + numWayPoints = len(data.time) + for row in range(numWayPoints): + + canvas.create_text(5, data.margin * (row+2), text="%d" % int(row + 1),anchor= SW) + goalList = data.goal[row] + goalList = np.around(goalList, 2) + goalStr = str(goalList) + canvas.create_text(150, data.margin * (row+2), text= goalStr,anchor= SW) + canvas.create_text(300, data.margin * (row+2), text="%0.1f" % float(data.time[row]),anchor= SW) + canvas.create_text(400, data.margin * (row+2), text="%d" % data.gripper[row] ,anchor= SW) + +def redrawAll(canvas, data): + drawText(canvas, data) + +def run(width = resx, height = resy): + def redrawAllWrapper(canvas, data): + canvas.delete(ALL) + redrawAll(canvas, data) + canvas.update() + def mousePressedWrapper(event, canvas, data): + mousePressed(event, data) + redrawAllWrapper(canvas, data) + + def keyPressedWrapper(event, canvas, data): + keyPressed(event, data) + redrawAllWrapper(canvas, data) + + def timerFiredWrapper(canvas, data): + abort_flag = timerFired(data) + if abort_flag == True: + print("Timer Fire Wrapper Abort") + redrawAllWrapper(canvas, data) + # pause, then call timerFired again + canvas.after(data.timerDelay, timerFiredWrapper, canvas, data) + # Set up data and call init + return abort_flag + class Struct(object): pass + data = Struct() + data.width = width + data.height = height + data.timerDelay = 5 # milliseconds + init(data) + # create the root and the canvas + root = Tk() + canvas = Canvas(root, width=data.width, height=data.height) + canvas.pack() # Drawing the canvas + + root.bind("", lambda event: + mousePressedWrapper(event, canvas, data)) + root.bind("", lambda event: + keyPressedWrapper(event, canvas, data)) + abort_flag = timerFiredWrapper(canvas, data) + if abort_flag == True: + root.destroy() + # and launch the app + # Adjust size + root.geometry("800x600") + + # set minimum window size value + root.minsize(resx, resy) + + # set maximum window size value + root.maxsize(resx, resy) + root.mainloop() # blocks until window is closed + +run(resx,resy)