gripper_joystick.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2014, 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 """
00031 Baxter RSDK Gripper Example: joystick
00032 """
00033 import argparse
00034 
00035 import rospy
00036 
00037 import baxter_interface
00038 import baxter_external_devices
00039 
00040 from baxter_interface import CHECK_VERSION
00041 
00042 
00043 def map_joystick(joystick):
00044     """
00045     maps joystick input to gripper commands
00046 
00047     @param joystick: an instance of a Joystick
00048     """
00049     # initialize interfaces
00050     print("Getting robot state... ")
00051     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00052     init_state = rs.state().enabled
00053     left = baxter_interface.Gripper('left', CHECK_VERSION)
00054     right = baxter_interface.Gripper('right', CHECK_VERSION)
00055 
00056     def clean_shutdown():
00057         print("\nExiting example...")
00058         if not init_state:
00059             print("Disabling robot...")
00060             rs.disable()
00061     rospy.on_shutdown(clean_shutdown)
00062 
00063     # decrease position dead_band
00064     left.set_dead_band(2.5)
00065     right.set_dead_band(2.5)
00066 
00067     # abbreviations
00068     jhi = lambda s: joystick.stick_value(s) > 0
00069     jlo = lambda s: joystick.stick_value(s) < 0
00070     bdn = joystick.button_down
00071     bup = joystick.button_up
00072 
00073     def print_help(bindings_list):
00074         print("Press Ctrl-C to quit.")
00075         for bindings in bindings_list:
00076             for (test, _cmd, doc) in bindings:
00077                 if callable(doc):
00078                     doc = doc()
00079                 print("%s: %s" % (str(test[1]), doc))
00080 
00081     def capability_warning(gripper, cmd):
00082         msg = ("%s %s - not capable of '%s' command" %
00083                (gripper.name, gripper.type(), cmd))
00084         print msg
00085 
00086     def offset_position(gripper, offset):
00087         if gripper.type() != 'electric':
00088             capability_warning(gripper, 'set_position')
00089             return
00090         current = gripper.position()
00091         gripper.command_position(current + offset)
00092 
00093     def offset_holding(gripper, offset):
00094         if gripper.type() != 'electric':
00095             capability_warning(gripper, 'set_holding_force')
00096             return
00097         current = gripper.parameters()['holding_force']
00098         gripper.set_holding_force(current + offset)
00099 
00100     def offset_velocity(gripper, offset):
00101         if gripper.type() != 'electric':
00102             capability_warning(gripper, 'set_velocity')
00103             return
00104         current = gripper.parameters()['velocity']
00105         gripper.set_velocity(current + offset)
00106 
00107     bindings_list = []
00108     bindings = (
00109         #(test, command, description)
00110         ((bdn, ['btnDown']), (left.reboot, []), "left: reboot"),
00111         ((bdn, ['btnLeft']), (right.reboot, []), "right: reboot"),
00112         ((bdn, ['btnRight']), (left.calibrate, []), "left: calibrate"),
00113         ((bdn, ['btnUp']), (right.calibrate, []), "right: calibrate"),
00114         ((bdn, ['rightTrigger']), (left.close, []), "left: close"),
00115         ((bdn, ['leftTrigger']), (right.close, []), "right: close"),
00116         ((bup, ['rightTrigger']), (left.open, []), "left: open (release)"),
00117         ((bup, ['leftTrigger']), (right.open, []), "right: open (release)"),
00118         ((bdn, ['rightBumper']), (left.stop, []), "left: stop"),
00119         ((bdn, ['leftBumper']), (right.stop, []), "right: stop"),
00120         ((jlo, ['rightStickHorz']), (offset_position, [left, -15.0]),
00121                                      "left:  decrease position"),
00122         ((jlo, ['leftStickHorz']), (offset_position, [right, -15.0]),
00123                                     "right:  decrease position"),
00124         ((jhi, ['rightStickHorz']), (offset_position, [left, 15.0]),
00125                                      "left:  increase position"),
00126         ((jhi, ['leftStickHorz']), (offset_position, [right, 15.0]),
00127                                      "right:  increase position"),
00128         ((jlo, ['rightStickVert']), (offset_holding, [left, -5.0]),
00129                                      "left:  decrease holding force"),
00130         ((jlo, ['leftStickVert']), (offset_holding, [right, -5.0]),
00131                                     "right:  decrease holding force"),
00132         ((jhi, ['rightStickVert']), (offset_holding, [left, 5.0]),
00133                                      "left:  increase holding force"),
00134         ((jhi, ['leftStickVert']), (offset_holding, [right, 5.0]),
00135                                     "right:  increase holding force"),
00136         ((bdn, ['dPadDown']), (offset_velocity, [left, -5.0]),
00137                                "left:  decrease velocity"),
00138         ((bdn, ['dPadLeft']), (offset_velocity, [right, -5.0]),
00139                                "right:  decrease velocity"),
00140         ((bdn, ['dPadRight']), (offset_velocity, [left, 5.0]),
00141                                 "left:  increase velocity"),
00142         ((bdn, ['dPadUp']), (offset_velocity, [right, 5.0]),
00143                              "right:  increase velocity"),
00144         ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
00145         ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
00146     )
00147     bindings_list.append(bindings)
00148 
00149     print("Enabling robot...")
00150     rs.enable()
00151     rate = rospy.Rate(100)
00152     print_help(bindings_list)
00153     print("Press <Start> button for help; Ctrl-C to stop...")
00154     while not rospy.is_shutdown():
00155         # test each joystick condition and call binding cmd if true
00156         for (test, cmd, doc) in bindings:
00157             if test[0](*test[1]):
00158                 cmd[0](*cmd[1])
00159                 print(doc)
00160         rate.sleep()
00161     rospy.signal_shutdown("Example finished.")
00162 
00163 
00164 def main():
00165     """RSDK Gripper Example: Joystick Control
00166 
00167     Use a game controller to control the grippers.
00168 
00169     Attach a game controller to your dev machine and run this
00170     example along with the ROS joy_node to control Baxter's
00171     grippers using the joysticks and buttons. Be sure to provide
00172     the *joystick* type you are using as an argument to setup
00173     appropriate key mappings.
00174 
00175     Uses the baxter_interface.Gripper class and the helper classes
00176     in baxter_external_devices.Joystick.
00177     """
00178     epilog = """
00179 See help inside the example with the "Start" button for controller
00180 key bindings.
00181     """
00182     arg_fmt = argparse.RawDescriptionHelpFormatter
00183     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00184                                      description=main.__doc__,
00185                                      epilog=epilog)
00186     required = parser.add_argument_group('required arguments')
00187     required.add_argument(
00188         '-j', '--joystick', required=True, choices=['xbox', 'logitech', 'ps3'],
00189         help='specify the type of joystick to use'
00190     )
00191     args = parser.parse_args(rospy.myargv()[1:])
00192 
00193     joystick = None
00194     if args.joystick == 'xbox':
00195         joystick = baxter_external_devices.joystick.XboxController()
00196     elif args.joystick == 'logitech':
00197         joystick = baxter_external_devices.joystick.LogitechController()
00198     elif args.joystick == 'ps3':
00199         joystick = baxter_external_devices.joystick.PS3Controller()
00200     else:
00201         # Should never reach this case with proper argparse usage
00202         parser.error("Unsupported joystick type '%s'" % (args.joystick))
00203 
00204     print("Initializing node... ")
00205     rospy.init_node("rsdk_gripper_joystick")
00206 
00207     map_joystick(joystick)
00208 
00209 
00210 if __name__ == '__main__':
00211     main()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Fri Oct 3 2014 16:37:39