joint_position_keyboard.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 """
00031 Baxter RSDK Joint Position 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     left = baxter_interface.Limb('left')
00045     right = baxter_interface.Limb('right')
00046     grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
00047     grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
00048     lj = left.joint_names()
00049     rj = right.joint_names()
00050 
00051     def set_j(limb, joint_name, delta):
00052         current_position = limb.joint_angle(joint_name)
00053         joint_command = {joint_name: current_position + delta}
00054         limb.set_joint_positions(joint_command)
00055 
00056     bindings = {
00057     #   key: (function, args, description)
00058         '9': (set_j, [left, lj[0], 0.1], "left_s0 increase"),
00059         '6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"),
00060         '8': (set_j, [left, lj[1], 0.1], "left_s1 increase"),
00061         '7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"),
00062         'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"),
00063         'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"),
00064         'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"),
00065         'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"),
00066         'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"),
00067         'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"),
00068         'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"),
00069         'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"),
00070         '.': (set_j, [left, lj[6], 0.1], "left_w2 increase"),
00071         'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
00072         ',': (grip_left.close, [], "left: gripper close"),
00073         'm': (grip_left.open, [], "left: gripper open"),
00074         '/': (grip_left.calibrate, [], "left: gripper calibrate"),
00075 
00076         '4': (set_j, [right, rj[0], 0.1], "right_s0 increase"),
00077         '1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"),
00078         '3': (set_j, [right, rj[1], 0.1], "right_s1 increase"),
00079         '2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"),
00080         'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"),
00081         'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"),
00082         'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"),
00083         'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"),
00084         'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"),
00085         'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"),
00086         'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"),
00087         's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"),
00088         'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"),
00089         'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"),
00090         'c': (grip_right.close, [], "right: gripper close"),
00091         'x': (grip_right.open, [], "right: gripper open"),
00092         'b': (grip_right.calibrate, [], "right: gripper calibrate"),
00093      }
00094     done = False
00095     print("Controlling joints. Press ? for help, Esc to quit.")
00096     while not done and not rospy.is_shutdown():
00097         c = baxter_external_devices.getch()
00098         if c:
00099             #catch Esc or ctrl-c
00100             if c in ['\x1b', '\x03']:
00101                 done = True
00102                 rospy.signal_shutdown("Example finished.")
00103             elif c in bindings:
00104                 cmd = bindings[c]
00105                 #expand binding to something like "set_j(right, 's0', 0.1)"
00106                 cmd[0](*cmd[1])
00107                 print("command: %s" % (cmd[2],))
00108             else:
00109                 print("key bindings: ")
00110                 print("  Esc: Quit")
00111                 print("  ?: Help")
00112                 for key, val in sorted(bindings.items(),
00113                                        key=lambda x: x[1][2]):
00114                     print("  %s: %s" % (key, val[2]))
00115 
00116 
00117 def main():
00118     """RSDK Joint Position Example: Keyboard Control
00119 
00120     Use your dev machine's keyboard to control joint positions.
00121 
00122     Each key corresponds to increasing or decreasing the angle
00123     of a joint on one of Baxter's arms. Each arm is represented
00124     by one side of the keyboard and inner/outer key pairings
00125     on each row for each joint.
00126     """
00127     epilog = """
00128 See help inside the example with the '?' key for key bindings.
00129     """
00130     arg_fmt = argparse.RawDescriptionHelpFormatter
00131     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00132                                      description=main.__doc__,
00133                                      epilog=epilog)
00134     parser.parse_args(rospy.myargv()[1:])
00135 
00136     print("Initializing node... ")
00137     rospy.init_node("rsdk_joint_position_keyboard")
00138     print("Getting robot state... ")
00139     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00140     init_state = rs.state().enabled
00141 
00142     def clean_shutdown():
00143         print("\nExiting example...")
00144         if not init_state:
00145             print("Disabling robot...")
00146             rs.disable()
00147     rospy.on_shutdown(clean_shutdown)
00148 
00149     print("Enabling robot... ")
00150     rs.enable()
00151 
00152     map_keyboard()
00153     print("Done.")
00154 
00155 
00156 if __name__ == '__main__':
00157     main()


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