class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the DigitalIO Signal feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control {left, right}
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._close_io = DigitalIO('%s_upper_button' % (arm, )) # 'dash' btn
self._open_io = DigitalIO('%s_lower_button' % (arm, )) # 'circle' btn
self._light_io = DigitalIO('%s_lower_cuff' % (arm, )) # cuff squeeze
# outputs
self._gripper = Gripper('%s' % (arm, ), CHECK_VERSION)
self._nav = Navigator('%s' % (arm, ))
# connect callback fns to signals
if self._gripper.type() != 'custom':
if not (self._gripper.calibrated()
or self._gripper.calibrate() == True):
rospy.logwarn("%s (%s) calibration failed.",
self._gripper.name.capitalize(),
self._gripper.type())
else:
msg = (("%s (%s) not capable of gripper commands."
" Running cuff-light connection only.") %
(self._gripper.name.capitalize(), self._gripper.type()))
rospy.logwarn(msg)
self._gripper.on_type_changed.connect(self._check_calibration)
self._open_io.state_changed.connect(self._open_action)
self._close_io.state_changed.connect(self._close_action)
if lights:
self._light_io.state_changed.connect(self._light_action)
rospy.loginfo("%s Cuff Control initialized...",
self._gripper.name.capitalize())
def _open_action(self, value):
if value and self._is_grippable():
rospy.logdebug("gripper open triggered")
self._gripper.open()
def _close_action(self, value):
if value and self._is_grippable():
rospy.logdebug("gripper close triggered")
self._gripper.close()
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
self._nav.inner_led = value
self._nav.outer_led = value
def _check_calibration(self, value):
if self._gripper.calibrated():
return True
elif value == 'electric':
rospy.loginfo("calibrating %s...", self._gripper.name.capitalize())
return (self._gripper.calibrate() == True)
else:
return False
def _is_grippable(self):
return (self._gripper.calibrated() and self._gripper.ready())
class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the DigitalIO Signal feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control {left, right}
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._close_io = DigitalIO('%s_upper_button' % (arm,)) # 'dash' btn
self._open_io = DigitalIO('%s_lower_button' % (arm,)) # 'circle' btn
self._light_io = DigitalIO('%s_lower_cuff' % (arm,)) # cuff squeeze
# outputs
self._gripper = Gripper('%s' % (arm,), CHECK_VERSION)
self._nav = Navigator('%s' % (arm,))
# connect callback fns to signals
if self._gripper.type() != 'custom':
if not (self._gripper.calibrated() or
self._gripper.calibrate() == True):
rospy.logwarn("%s (%s) calibration failed.",
self._gripper.name.capitalize(),
self._gripper.type())
else:
msg = (("%s (%s) not capable of gripper commands."
" Running cuff-light connection only.") %
(self._gripper.name.capitalize(), self._gripper.type()))
rospy.logwarn(msg)
self._gripper.on_type_changed.connect(self._check_calibration)
self._open_io.state_changed.connect(self._open_action)
self._close_io.state_changed.connect(self._close_action)
if lights:
self._light_io.state_changed.connect(self._light_action)
rospy.loginfo("%s Cuff Control initialized...",
self._gripper.name.capitalize())
def _open_action(self, value):
if value and self._is_grippable():
rospy.logdebug("gripper open triggered")
self._gripper.open()
def _close_action(self, value):
if value and self._is_grippable():
rospy.logdebug("gripper close triggered")
self._gripper.close()
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
self._nav.inner_led = value
self._nav.outer_led = value
def _check_calibration(self, value):
if self._gripper.calibrated():
return True
elif value == 'electric':
rospy.loginfo("calibrating %s...",
self._gripper.name.capitalize())
return (self._gripper.calibrate() == True)
else:
return False
def _is_grippable(self):
return (self._gripper.calibrated() and self._gripper.ready())
class ArmController(object):
def __init__(self,
starting_poss=None,
push_thresh=10,
mode='one_arm',
arm_mode='first'):
"""
Initialises parameters and moves the arm to a neutral
position.
"""
self.push_thresh = push_thresh
self._right_neutral_pos = starting_poss[0]
self._left_neutral_pos = starting_poss[1]
self._mode = mode
self._arm_mode = arm_mode
rospy.loginfo("Creating interface and calibrating gripper")
self._right_limb = Limb('right')
self._left_limb = Limb('left')
self._right_gripper = Gripper('right', CHECK_VERSION)
self._right_gripper.calibrate()
self._left_gripper = Gripper('left', CHECK_VERSION)
self._left_gripper.calibrate()
self._is_right_fist_closed = False
self._is_left_fist_closed = False
rospy.loginfo("Moving to neutral position")
self.move_to_neutral()
rospy.loginfo("Initialising PoseGenerator")
self._pg = PoseGenerator(self._mode, self._arm_mode)
self._sub_right_gesture = rospy.Subscriber(
"/low_myo/gesture", UInt8, self._right_gesture_callback)
self._sub_left_gesture = rospy.Subscriber("/top_myo/gesture", UInt8,
self._left_gesture_callback)
self._last_data = None
self._pg.calibrate()
def move_to_neutral(self):
if self._mode == "one_arm":
self._right_limb.move_to_joint_positions(self._right_neutral_pos)
elif self._mode == "two_arms":
self._right_limb.move_to_joint_positions(self._right_neutral_pos)
self._left_limb.move_to_joint_positions(self._left_neutral_pos)
else:
raise ValueError("Mode %s is invalid!" % self._mode)
def is_right_pushing(self):
"""
Checks if any of the joints is under external stress. Returns
true if the maximum recorded stress above specified threshold.
"""
e = self._right_limb.joint_efforts()
max_effort = max([abs(e[i]) for i in e.keys()])
return max_effort > self.push_thresh
def is_left_pushing(self):
"""
Checks if any of the joints is under external stress. Returns
true if the maximum recorded stress above specified threshold.
"""
e = self._left_limb.joint_efforts()
max_effort = max([abs(e[i]) for i in e.keys()])
return max_effort > self.push_thresh
def _command_right_gripper(self):
"""
Reads state from Myo and opens/closes gripper as needed.
"""
if not self._right_gripper.ready():
return
if self._right_gripper.moving():
return
if self._is_right_fist_closed:
self._right_gripper.close()
else:
self._right_gripper.open()
def _command_left_gripper(self):
"""
Reads state from Myo and opens/closes gripper as needed.
"""
if not self._left_gripper.ready():
return
if self._left_gripper.moving():
return
if self._is_left_fist_closed:
self._left_gripper.close()
else:
self._left_gripper.open()
def step(self):
"""
Executes a step of the main routine.
Fist checks the status of the gripper and
"""
os.system('clear')
if self._mode == "one_arm":
return self.one_arm_step()
elif self._mode == "two_arms":
return self.two_arms_step()
else:
raise ValueError("Mode %s is invalid!" % self.mode)
def one_arm_step(self):
self._command_right_gripper()
pos = self._pg.generate_pose()
if pos is not None:
if not self.is_right_pushing():
self._right_limb.set_joint_positions(pos)
else:
rospy.logwarn("Arm is being pushed!")
else:
rospy.logwarn("Generated position is invalid")
def two_arms_step(self):
self._command_right_gripper()
self._command_left_gripper()
poss = self._pg.generate_pose()
if poss is not None:
if not self.is_right_pushing():
self._right_limb.set_joint_positions(poss[0])
if not self.is_left_pushing():
self._left_limb.set_joint_positions(poss[1])
else:
rospy.logwarn("Arm is being pushed!")
else:
rospy.logwarn("Generated position is invalid")
def _right_gesture_callback(self, data):
self._is_right_fist_closed = (data.data == 1)
def _left_gesture_callback(self, data):
self._is_left_fist_closed = (data.data != 0)
