# mobileIO setup phone_name = "mobileIO" lookup = hebi.Lookup() sleep(2) # Setup 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.update() # Demo Variables abort_flag = False run_mode = "softstart" goal = arm_api.Goal(arm.size) home_position = [0, np.pi / 3, 2 * np.pi / 3, 5 * np.pi / 6, -np.pi / 2, 0] # Command the softstart to the home position softstart = arm_api.Goal(arm.size) softstart.add_waypoint(t=4, position=home_position) arm.update() arm.set_goal(softstart) arm.send() # Get the cartesian position and rotation matrix @ home position xyz_home = np.zeros(3) rot_home = np.zeros((3, 3)) # arm.FK(home_position, xyz_home, ori_home) arm.FK(home_position, xyz_out=xyz_home, orientation_out=rot_home)
""" 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) # Printing 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) ####################### ## Main Control Loop ## #######################