Introduction
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
34
35 import rospy
36
37 import baxter_interface
38 import baxter_external_devices
39
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.
46
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.
58
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
64
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.
74
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()
87
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 (grip_left.open, []), "left gripper open"),
108 ((bdn, ['leftTrigger']),
109 (grip_right.close, []), "right gripper close"),
110 ((bup, ['leftTrigger']),
111 (grip_right.open, []), "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
166
167 Use a game controller to control the angular joint positions
168 of Baxter's arms.
169
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.
174
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:])
195
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
211
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)
218
219 print("Enabling robot... ")
220 rs.enable()
221
222 map_joystick(joystick)
223 print("Done.")
224
225
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.
