gripper_cuff_control.py
Go to the documentation of this file.
00001 #!/usr/bin/python2
00002 
00003 # Copyright (c) 2013-2015, Rethink Robotics
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. Redistributions in binary form must reproduce the above copyright
00012 #    notice, this list of conditions and the following disclaimer in the
00013 #    documentation and/or other materials provided with the distribution.
00014 # 3. Neither the name of the Rethink Robotics nor the names of its
00015 #    contributors may be used to endorse or promote products derived from
00016 #    this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 import argparse
00031 import sys
00032 
00033 import rospy
00034 
00035 from baxter_interface import (
00036     DigitalIO,
00037     Gripper,
00038     Navigator,
00039     CHECK_VERSION,
00040 )
00041 
00042 
00043 class GripperConnect(object):
00044     """
00045     Connects wrist button presses to gripper open/close commands.
00046 
00047     Uses the DigitalIO Signal feature to make callbacks to connected
00048     action functions when the button values change.
00049     """
00050 
00051     def __init__(self, arm, lights=True):
00052         """
00053         @type arm: str
00054         @param arm: arm of gripper to control {left, right}
00055         @type lights: bool
00056         @param lights: if lights should activate on cuff grasp
00057         """
00058         self._arm = arm
00059         # inputs
00060         self._close_io = DigitalIO('%s_upper_button' % (arm,))  # 'dash' btn
00061         self._open_io = DigitalIO('%s_lower_button' % (arm,))   # 'circle' btn
00062         self._light_io = DigitalIO('%s_lower_cuff' % (arm,))    # cuff squeeze
00063         # outputs
00064         self._gripper = Gripper('%s' % (arm,), CHECK_VERSION)
00065         self._nav = Navigator('%s' % (arm,))
00066 
00067         # connect callback fns to signals
00068         if self._gripper.type() != 'custom':
00069             if not (self._gripper.calibrated() or
00070                     self._gripper.calibrate() == True):
00071                 rospy.logwarn("%s (%s) calibration failed.",
00072                               self._gripper.name.capitalize(),
00073                               self._gripper.type())
00074         else:
00075             msg = (("%s (%s) not capable of gripper commands."
00076                    " Running cuff-light connection only.") %
00077                    (self._gripper.name.capitalize(), self._gripper.type()))
00078             rospy.logwarn(msg)
00079 
00080         self._gripper.on_type_changed.connect(self._check_calibration)
00081         self._open_io.state_changed.connect(self._open_action)
00082         self._close_io.state_changed.connect(self._close_action)
00083 
00084         if lights:
00085             self._light_io.state_changed.connect(self._light_action)
00086 
00087         rospy.loginfo("%s Cuff Control initialized...",
00088                       self._gripper.name.capitalize())
00089 
00090     def _open_action(self, value):
00091         if value and self._is_grippable():
00092             rospy.logdebug("gripper open triggered")
00093             self._gripper.open()
00094 
00095     def _close_action(self, value):
00096         if value and self._is_grippable():
00097             rospy.logdebug("gripper close triggered")
00098             self._gripper.close()
00099 
00100     def _light_action(self, value):
00101         if value:
00102             rospy.logdebug("cuff grasp triggered")
00103         else:
00104             rospy.logdebug("cuff release triggered")
00105         self._nav.inner_led = value
00106         self._nav.outer_led = value
00107 
00108     def _check_calibration(self, value):
00109         if self._gripper.calibrated():
00110             return True
00111         elif value == 'electric':
00112             rospy.loginfo("calibrating %s...",
00113                           self._gripper.name.capitalize())
00114             return (self._gripper.calibrate() == True)
00115         else:
00116             return False
00117 
00118     def _is_grippable(self):
00119         return (self._gripper.calibrated() and self._gripper.ready())
00120 
00121 
00122 def main():
00123     """RSDK Gripper Button Control Example
00124 
00125     Connects cuff buttons to gripper open/close commands:
00126         'Circle' Button    - open gripper
00127         'Dash' Button      - close gripper
00128         Cuff 'Squeeze'     - turn on Nav lights
00129 
00130     Run this example in the background or in another terminal
00131     to be able to easily control the grippers by hand while
00132     using the robot. Can be run in parallel with other code.
00133     """
00134     arg_fmt = argparse.RawDescriptionHelpFormatter
00135     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00136                                      description=main.__doc__)
00137     parser.add_argument('-g', '--gripper', dest='gripper', default='both',
00138                         choices=['both', 'left', 'right'],
00139                         help='gripper limb to control (default: both)')
00140     parser.add_argument('-n', '--no-lights', dest='lights',
00141                         action='store_false',
00142                         help='do not trigger lights on cuff grasp')
00143     parser.add_argument('-v', '--verbose', dest='verbosity',
00144                         action='store_const', const=rospy.DEBUG,
00145                         default=rospy.INFO,
00146                         help='print debug statements')
00147     args = parser.parse_args(rospy.myargv()[1:])
00148 
00149     rospy.init_node('rsdk_gripper_cuff_control_%s' % (args.gripper,),
00150                     log_level=args.verbosity)
00151 
00152     arms = (args.gripper,) if args.gripper != 'both' else ('left', 'right')
00153     grip_ctrls = [GripperConnect(arm, args.lights) for arm in arms]
00154 
00155     print("Press cuff buttons to control grippers. Spinning...")
00156     rospy.spin()
00157     print("Gripper Button Control Finished.")
00158     return 0
00159 
00160 if __name__ == '__main__':
00161     sys.exit(main())


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Sat Jun 8 2019 20:16:27