Joint Position Joystick - Code Walkthrough

From sdk-wiki
Jump to: navigation, search


This example demonstrates the usage of the position controller to move the limbs to various positions via the Joystick interface. The main() function enables the robot and invokes the map_keyboard() function. It is at this function where the Joystick's key presses are mapped to individual joints and the commands are sent to the position controller periodically. Pressing the left or the right bumper button on the joystick shifts the joint being controlled in the corresponding limb to the next one. For instance, if the joint being controlled initially in the left limb is left_so, then pressing the left bumper button would update the currently controlled joint as left_s1. This is performed in the rotate() function.
Interfaces -

  • Limb.joint_names()
  • Limb.joint_angle(<string>)
  • Limb.set_joint_positions(<double>)

Code Walkthrough

Now, let's break down the code.

33 import argparse
35 import rospy
37 import baxter_interface
38 import baxter_external_devices
40 from baxter_interface import CHECK_VERSION

This imports the baxter interface for accessing the limb and the gripper class. The baxter_external_devices is imported to use its getch function, that captures the key presses on the joystick. The CHECK_VERSION is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.

43 def rotate(l):
44     """
45     Rotates a list left.
47     @param l: the list
48     """
49     if len(l):
50         v = l[0]
51         l[:-1] = l[1:]
52         l[-1] = v

This moves all the elements in the list to one position to their left. So the element at 0th position moves to the nth position, element at nth position moves to (n-1)th position, and so on.

55 def set_j(cmd, limb, joints, index, delta):
56     """
57     Set the selected joint to current pos + delta.
59     @param cmd: the joint command dictionary
60     @param limb: the limb to get the pos from
61     @param joints: a list of joint names
62     @param index: the index in the list of names
63     @param delta: delta to update the joint by
65     joint/index is to make this work in the bindings.
66     """
67     joint = joints[index]
68     cmd[joint] = delta + limb.joint_angle(joint)

joint_angle() method returns the current angle of the corresponding joint. This function builds the dictionary that is to be passed as a joint command. The last line assigns the new position which has an offset of delta from the current position.

71 def map_joystick(joystick):
72     """
73     Maps joystick input to joint position commands.
75     @param joystick: an instance of a Joystick
76     """
77     left = baxter_interface.Limb('left')
78     right = baxter_interface.Limb('right')
79     grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
80     grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
81     lcmd = {}
82     rcmd = {}

Two instances of the Limb class are created for each of Baxter's limbs. Similarly two instances of the Gripper class are created for the left and right grippers. The software version is checked for compatibility with the local version. lcmd and rcmd are the two dictionaries which would hold the set of commands from the below bindings to be executed on the left and right limbs respectively.

85     lj = left.joint_names()
86     rj = right.joint_names()
88     #abbreviations
89     jhi = lambda s: joystick.stick_value(s) > 0
90     jlo = lambda s: joystick.stick_value(s) < 0
91     bdn = joystick.button_down
92     bup = joystick.button_up

The joint_names() method returns a list of joints associated with that limb. The stick_value() method returns the deadbanded, scaled and offset value of the axis. The abbreviations for all these functions are created to be used within the bindings dictionary below.

 94     def print_help(bindings_list):
 95         print("Press Ctrl-C to quit.")
 96         for bindings in bindings_list:
 97             for (test, _cmd, doc) in bindings:
 98                 if callable(doc):
 99                     doc = doc()
100                 print("%s: %s" % (str(test[1][0]), doc))

The information about the joystick buttons and the corresponding joints are displayed. This information is available in bindings_list as below. The doc refers to the last element of the tuple and it is checked if it is a callable function. So, if there is a lambda function that evaluates and returns a string, then that function is called. Finally, the results are displayed.

102     bindings_list = []
103     bindings = (
104         ((bdn, ['rightTrigger']),
105          (grip_left.close,  []), "left gripper close"),
106         ((bup, ['rightTrigger']),
107          (,   []), "left gripper open"),
108         ((bdn, ['leftTrigger']),
109          (grip_right.close, []), "right gripper close"),
110         ((bup, ['leftTrigger']),
111          (,  []), "right gripper open"),
112         ((jlo, ['leftStickHorz']),
113          (set_j, [rcmd, right, rj, 0,  0.1]), lambda: "right inc " + rj[0]),
114         ((jhi, ['leftStickHorz']),
115          (set_j, [rcmd, right, rj, 0, -0.1]), lambda: "right dec " + rj[0]),
116         ((jlo, ['rightStickHorz']),
117          (set_j, [lcmd, left,  lj, 0,  0.1]), lambda: "left inc " + lj[0]),
118         ((jhi, ['rightStickHorz']),
119          (set_j, [lcmd, left,  lj, 0, -0.1]), lambda: "left dec " + lj[0]),
120         ((jlo, ['leftStickVert']),
121          (set_j, [rcmd, right, rj, 1,  0.1]), lambda: "right inc " + rj[1]),
122         ((jhi, ['leftStickVert']),
123          (set_j, [rcmd, right, rj, 1, -0.1]), lambda: "right dec " + rj[1]),
124         ((jlo, ['rightStickVert']),
125          (set_j, [lcmd, left,  lj, 1,  0.1]), lambda: "left inc " + lj[1]),
126         ((jhi, ['rightStickVert']),
127          (set_j, [lcmd, left,  lj, 1, -0.1]), lambda: "left dec " + lj[1]),
128         ((bdn, ['rightBumper']),
129          (rotate, [lj]), "left: cycle joint"),
130         ((bdn, ['leftBumper']),
131          (rotate, [rj]), "right: cycle joint"),
132         ((bdn, ['btnRight']),
133          (grip_left.calibrate, []), "left calibrate"),
134         ((bdn, ['btnLeft']),
135          (grip_right.calibrate, []), "right calibrate"),
136         ((bdn, ['function1']),
137          (print_help, [bindings_list]), "help"),
138         ((bdn, ['function2']),
139          (print_help, [bindings_list]), "help"),
140         )
141     bindings_list.append(bindings)

The first element of every tuple refers to the command that has to be executed. As indicated above set_j, bdn, bup, jhi and jlo refers to the function and their acronyms. The second tuple has a list that holds the list of arguments to be passed along with that function. The last element holds a string or a function that evaluates and forms a string to detail the joint under consideration.

143     rate = rospy.Rate(100)
144     print_help(bindings_list)
145     print("Press Ctrl-C to stop. ")
146     while not rospy.is_shutdown():
147         for (test, cmd, doc) in bindings:
148             if test[0](*test[1]):
149                 cmd[0](*cmd[1])
150                 if callable(doc):
151                     print(doc())
152                 else:
153                     print(doc)

The test tuple holds the function that needs to be called for a particular value to be tested. For instance, the first tuple holds the abbreviation bdn and the parameter rightTrigger that is passed along. This test function checks whether the joystick's button_down is pressed. If this is true then the cmd, that refers to the second tuple (grip_left.close,[]) is parsed as above. For the first binding, the expression cmd[0](*cmd[1]) returns the function call grip_left.close([]). The next line checks if the last tuple is a function (lambda) or not (string). Since the joints being controlled are updated dynamically, the lambda function is used to retrieve the current joints rj[0], lj[0], rj[1] and lj[1].

154         if len(lcmd):
155             left.set_joint_positions(lcmd)
156             lcmd.clear()
157         if len(rcmd):
158             right.set_joint_positions(rcmd)
159             rcmd.clear()
160         rate.sleep()
161     return False

The dictionaries lcmd and rcmd hold the joint commands for the left and right limbs. These dictionaries get populated when the set_j method is called by the previous section of code.

164 def main():
165     """RSDK Joint Position Example: Joystick Control
167     Use a game controller to control the angular joint positions
168     of Baxter's arms.
170     Attach a game controller to your dev machine and run this
171     example along with the ROS joy_node to control the position
172     of each joint in Baxter's arms using the joysticks. Be sure to
173     provide your *joystick* type to setup appropriate key mappings.
175     Each stick axis maps to a joint angle; which joints are currently
176     controlled can be incremented by using the mapped increment buttons.
177     Ex:
178       (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
179     """
180     epilog = """
181 See help inside the example with the "Start" button for controller
182 key bindings.
183     """
184     arg_fmt = argparse.RawDescriptionHelpFormatter
185     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
186                                      description=main.__doc__,
187                                      epilog=epilog)
188     required = parser.add_argument_group('required arguments')
189     required.add_argument(
190         '-j', '--joystick', required=True,
191         choices=['xbox', 'logitech', 'ps3'],
192         help='specify the type of joystick to use'
193     )
194     args = parser.parse_args(rospy.myargv()[1:])
196     joystick = None
197     if args.joystick == 'xbox':
198         joystick = baxter_external_devices.joystick.XboxController()
199     elif args.joystick == 'logitech':
200         joystick = baxter_external_devices.joystick.LogitechController()
201     elif args.joystick == 'ps3':
202         joystick = baxter_external_devices.joystick.PS3Controller()
203     else:
204         parser.error("Unsupported joystick type '%s'" % (args.joystick))

The type of joystick input is captured as entered by the user and an instance of the corresponding interface is created.

206     print("Initializing node... ")
207     rospy.init_node("rsdk_joint_position_joystick")
208     print("Getting robot state... ")
209     rs = baxter_interface.RobotEnable(CHECK_VERSION)
210     init_state = rs.state().enabled
212     def clean_shutdown():
213         print("\nExiting example.")
214         if not init_state:
215             print("Disabling robot...")
216             rs.disable()
217     rospy.on_shutdown(clean_shutdown)
219     print("Enabling robot... ")
220     rs.enable()
222     map_joystick(joystick)
223     print("Done.")
226 if __name__ == '__main__':
227     main()

The node is initialized and the robot is enabled. The map_joystick() method performs the mapping and execution of the commands as explained above.