joint_position_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 Joint Position 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 rotate(l):
00044     """
00045     Rotates a list left.
00046 
00047     @param l: the list
00048     """
00049     if len(l):
00050         v = l[0]
00051         l[:-1] = l[1:]
00052         l[-1] = v
00053 
00054 
00055 def set_j(cmd, limb, joints, index, delta):
00056     """
00057     Set the selected joint to current pos + delta.
00058 
00059     @param cmd: the joint command dictionary
00060     @param limb: the limb to get the pos from
00061     @param joints: a list of joint names
00062     @param index: the index in the list of names
00063     @param delta: delta to update the joint by
00064 
00065     joint/index is to make this work in the bindings.
00066     """
00067     joint = joints[index]
00068     cmd[joint] = delta + limb.joint_angle(joint)
00069 
00070 
00071 def map_joystick(joystick):
00072     """
00073     Maps joystick input to joint position commands.
00074 
00075     @param joystick: an instance of a Joystick
00076     """
00077     left = baxter_interface.Limb('left')
00078     right = baxter_interface.Limb('right')
00079     grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
00080     grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
00081     lcmd = {}
00082     rcmd = {}
00083 
00084     #available joints
00085     lj = left.joint_names()
00086     rj = right.joint_names()
00087 
00088     #abbreviations
00089     jhi = lambda s: joystick.stick_value(s) > 0
00090     jlo = lambda s: joystick.stick_value(s) < 0
00091     bdn = joystick.button_down
00092     bup = joystick.button_up
00093 
00094     def print_help(bindings_list):
00095         print("Press Ctrl-C to quit.")
00096         for bindings in bindings_list:
00097             for (test, _cmd, doc) in bindings:
00098                 if callable(doc):
00099                     doc = doc()
00100                 print("%s: %s" % (str(test[1][0]), doc))
00101 
00102     bindings_list = []
00103     bindings = (
00104         ((bdn, ['rightTrigger']),
00105          (grip_left.close,  []), "left gripper close"),
00106         ((bup, ['rightTrigger']),
00107          (grip_left.open,   []), "left gripper open"),
00108         ((bdn, ['leftTrigger']),
00109          (grip_right.close, []), "right gripper close"),
00110         ((bup, ['leftTrigger']),
00111          (grip_right.open,  []), "right gripper open"),
00112         ((jlo, ['leftStickHorz']),
00113          (set_j, [rcmd, right, rj, 0,  0.1]), lambda: "right inc " + rj[0]),
00114         ((jhi, ['leftStickHorz']),
00115          (set_j, [rcmd, right, rj, 0, -0.1]), lambda: "right dec " + rj[0]),
00116         ((jlo, ['rightStickHorz']),
00117          (set_j, [lcmd, left,  lj, 0,  0.1]), lambda: "left inc " + lj[0]),
00118         ((jhi, ['rightStickHorz']),
00119          (set_j, [lcmd, left,  lj, 0, -0.1]), lambda: "left dec " + lj[0]),
00120         ((jlo, ['leftStickVert']),
00121          (set_j, [rcmd, right, rj, 1,  0.1]), lambda: "right inc " + rj[1]),
00122         ((jhi, ['leftStickVert']),
00123          (set_j, [rcmd, right, rj, 1, -0.1]), lambda: "right dec " + rj[1]),
00124         ((jlo, ['rightStickVert']),
00125          (set_j, [lcmd, left,  lj, 1,  0.1]), lambda: "left inc " + lj[1]),
00126         ((jhi, ['rightStickVert']),
00127          (set_j, [lcmd, left,  lj, 1, -0.1]), lambda: "left dec " + lj[1]),
00128         ((bdn, ['rightBumper']),
00129          (rotate, [lj]), "left: cycle joint"),
00130         ((bdn, ['leftBumper']),
00131          (rotate, [rj]), "right: cycle joint"),
00132         ((bdn, ['btnRight']),
00133          (grip_left.calibrate, []), "left calibrate"),
00134         ((bdn, ['btnLeft']),
00135          (grip_right.calibrate, []), "right calibrate"),
00136         ((bdn, ['function1']),
00137          (print_help, [bindings_list]), "help"),
00138         ((bdn, ['function2']),
00139          (print_help, [bindings_list]), "help"),
00140         )
00141     bindings_list.append(bindings)
00142 
00143     rate = rospy.Rate(100)
00144     print_help(bindings_list)
00145     print("Press Ctrl-C to stop. ")
00146     while not rospy.is_shutdown():
00147         for (test, cmd, doc) in bindings:
00148             if test[0](*test[1]):
00149                 cmd[0](*cmd[1])
00150                 if callable(doc):
00151                     print(doc())
00152                 else:
00153                     print(doc)
00154         if len(lcmd):
00155             left.set_joint_positions(lcmd)
00156             lcmd.clear()
00157         if len(rcmd):
00158             right.set_joint_positions(rcmd)
00159             rcmd.clear()
00160         rate.sleep()
00161     return False
00162 
00163 
00164 def main():
00165     """RSDK Joint Position Example: Joystick Control
00166 
00167     Use a game controller to control the angular joint positions
00168     of Baxter's arms.
00169 
00170     Attach a game controller to your dev machine and run this
00171     example along with the ROS joy_node to control the position
00172     of each joint in Baxter's arms using the joysticks. Be sure to
00173     provide your *joystick* type to setup appropriate key mappings.
00174 
00175     Each stick axis maps to a joint angle; which joints are currently
00176     controlled can be incremented by using the mapped increment buttons.
00177     Ex:
00178       (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
00179     """
00180     epilog = """
00181 See help inside the example with the "Start" button for controller
00182 key bindings.
00183     """
00184     arg_fmt = argparse.RawDescriptionHelpFormatter
00185     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00186                                      description=main.__doc__,
00187                                      epilog=epilog)
00188     required = parser.add_argument_group('required arguments')
00189     required.add_argument(
00190         '-j', '--joystick', required=True,
00191         choices=['xbox', 'logitech', 'ps3'],
00192         help='specify the type of joystick to use'
00193     )
00194     args = parser.parse_args(rospy.myargv()[1:])
00195 
00196     joystick = None
00197     if args.joystick == 'xbox':
00198         joystick = baxter_external_devices.joystick.XboxController()
00199     elif args.joystick == 'logitech':
00200         joystick = baxter_external_devices.joystick.LogitechController()
00201     elif args.joystick == 'ps3':
00202         joystick = baxter_external_devices.joystick.PS3Controller()
00203     else:
00204         parser.error("Unsupported joystick type '%s'" % (args.joystick))
00205 
00206     print("Initializing node... ")
00207     rospy.init_node("rsdk_joint_position_joystick")
00208     print("Getting robot state... ")
00209     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00210     init_state = rs.state().enabled
00211 
00212     def clean_shutdown():
00213         print("\nExiting example.")
00214         if not init_state:
00215             print("Disabling robot...")
00216             rs.disable()
00217     rospy.on_shutdown(clean_shutdown)
00218 
00219     print("Enabling robot... ")
00220     rs.enable()
00221 
00222     map_joystick(joystick)
00223     print("Done.")
00224 
00225 
00226 if __name__ == '__main__':
00227     main()


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