gripper_keyboard.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: keyboard
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_keyboard():
00044     # initialize interfaces
00045     print("Getting robot state... ")
00046     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00047     init_state = rs.state().enabled
00048     left = baxter_interface.Gripper('left', CHECK_VERSION)
00049     right = baxter_interface.Gripper('right', CHECK_VERSION)
00050 
00051     def clean_shutdown():
00052         if not init_state:
00053             print("Disabling robot...")
00054             rs.disable()
00055         print("Exiting example.")
00056     rospy.on_shutdown(clean_shutdown)
00057 
00058     def capability_warning(gripper, cmd):
00059         msg = ("%s %s - not capable of '%s' command" %
00060                (gripper.name, gripper.type(), cmd))
00061         rospy.logwarn(msg)
00062 
00063     def offset_position(gripper, offset):
00064         if gripper.type() != 'electric':
00065             capability_warning(gripper, 'command_position')
00066             return
00067         current = gripper.position()
00068         gripper.command_position(current + offset)
00069 
00070     def offset_holding(gripper, offset):
00071         if gripper.type() != 'electric':
00072             capability_warning(gripper, 'set_holding_force')
00073             return
00074         current = gripper.parameters()['holding_force']
00075         gripper.set_holding_force(current + offset)
00076 
00077     def offset_moving(gripper, offset):
00078         if gripper.type() != 'electric':
00079             capability_warning(gripper, 'set_moving_force')
00080             return
00081         current = gripper.parameters()['moving_force']
00082         gripper.set_moving_force(current + offset)
00083 
00084     def offset_velocity(gripper, offset):
00085         if gripper.type() != 'electric':
00086             capability_warning(gripper, 'set_velocity')
00087             return
00088         current = gripper.parameters()['velocity']
00089         gripper.set_velocity(current + offset)
00090 
00091     def offset_dead_band(gripper, offset):
00092         if gripper.type() != 'electric':
00093             capability_warning(gripper, 'set_dead_band')
00094             return
00095         current = gripper.parameters()['dead_zone']
00096         gripper.set_dead_band(current + offset)
00097 
00098     bindings = {
00099     #   key: (function, args, description)
00100         'r': (left.reboot, [], "left: reboot"),
00101         'R': (right.reboot, [], "right: reboot"),
00102         'c': (left.calibrate, [], "left: calibrate"),
00103         'C': (right.calibrate, [], "right: calibrate"),
00104         'q': (left.close, [], "left: close"),
00105         'Q': (right.close, [], "right: close"),
00106         'w': (left.open, [], "left: open"),
00107         'W': (right.open, [], "right: open"),
00108         '[': (left.set_velocity, [100.0], "left:  set 100% velocity"),
00109         '{': (right.set_velocity, [100.0], "right:  set 100% velocity"),
00110         ']': (left.set_velocity, [30.0], "left:  set 30% velocity"),
00111         '}': (right.set_velocity, [30.0], "right:  set 30% velocity"),
00112         's': (left.stop, [], "left: stop"),
00113         'S': (right.stop, [], "right: stop"),
00114         'z': (offset_dead_band, [left, -1.0], "left:  decrease dead band"),
00115         'Z': (offset_dead_band, [right, -1.0], "right:  decrease dead band"),
00116         'x': (offset_dead_band, [left, 1.0], "left:  increase dead band"),
00117         'X': (offset_dead_band, [right, 1.0], "right:  increase dead band"),
00118         'f': (offset_moving, [left, -5.0], "left:  decrease moving force"),
00119         'F': (offset_moving, [right, -5.0], "right:  decrease moving force"),
00120         'g': (offset_moving, [left, 5.0], "left:  increase moving force"),
00121         'G': (offset_moving, [right, 5.0], "right:  increase moving force"),
00122         'h': (offset_holding, [left, -5.0], "left:  decrease holding force"),
00123         'H': (offset_holding, [right, -5.0], "right:  decrease holding force"),
00124         'j': (offset_holding, [left, 5.0], "left:  increase holding force"),
00125         'J': (offset_holding, [right, 5.0], "right:  increase holding force"),
00126         'v': (offset_velocity, [left, -5.0], "left:  decrease velocity"),
00127         'V': (offset_velocity, [right, -5.0], "right:  decrease velocity"),
00128         'b': (offset_velocity, [left, 5.0], "left:  increase velocity"),
00129         'B': (offset_velocity, [right, 5.0], "right:  increase velocity"),
00130         'u': (offset_position, [left, -15.0], "left:  decrease position"),
00131         'U': (offset_position, [right, -15.0], "right:  decrease position"),
00132         'i': (offset_position, [left, 15.0], "left:  increase position"),
00133         'I': (offset_position, [right, 15.0], "right:  increase position"),
00134     }
00135 
00136     done = False
00137     print("Enabling robot... ")
00138     rs.enable()
00139     print("Controlling grippers. Press ? for help, Esc to quit.")
00140     while not done and not rospy.is_shutdown():
00141         c = baxter_external_devices.getch()
00142         if c:
00143             if c in ['\x1b', '\x03']:
00144                 done = True
00145             elif c in bindings:
00146                 cmd = bindings[c]
00147                 cmd[0](*cmd[1])
00148                 print("command: %s" % (cmd[2],))
00149             else:
00150                 print("key bindings: ")
00151                 print("  Esc: Quit")
00152                 print("  ?: Help")
00153                 for key, val in sorted(bindings.items(),
00154                                        key=lambda x: x[1][2]):
00155                     print("  %s: %s" % (key, val[2]))
00156     # force shutdown call if caught by key handler
00157     rospy.signal_shutdown("Example finished.")
00158 
00159 
00160 def main():
00161     """RSDK Gripper Example: Keyboard Control
00162 
00163     Use your dev machine's keyboard to control and configure
00164     Baxter's grippers.
00165 
00166     Run this example to command various gripper movements while
00167     adjusting gripper parameters, including calibration, velocity,
00168     and force. Uses the baxter_interface.Gripper class and the
00169     helper function, baxter_external_devices.getch.
00170     """
00171     epilog = """
00172 See help inside the example with the '?' key for key bindings.
00173     """
00174     arg_fmt = argparse.RawDescriptionHelpFormatter
00175     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00176                                      description=main.__doc__,
00177                                      epilog=epilog)
00178     parser.parse_args(rospy.myargv()[1:])
00179 
00180     print("Initializing node... ")
00181     rospy.init_node("rsdk_gripper_keyboard")
00182 
00183     map_keyboard()
00184 
00185 
00186 if __name__ == '__main__':
00187     main()


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