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)