names=module_names,
hrdf_file=hrdf_file)
arm.load_gains(gains_file)
# Add the gripper
gripper_family = arm_family
gripper_name = 'gripperSpool'
gripper_group = lookup.get_group_from_names([gripper_family], [gripper_name])
while gripper_group is None:
print('Looking for gripper...')
sleep(1)
gripper_group = lookup.get_group_from_names([gripper_family], [gripper_name])
gripper = arm_api.Gripper(gripper_group, -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()
"""
# 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()
# 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)
# 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)