The joint position example(keyboard, joystick) demonstrates basic joint position control. The key or button mapped to either increasing or decreasing the angle of a particular joint on Sawyer's arm.
Introduction
Use a game controller or your dev machine's keyboard to control the angular joint positions of Sawyer's arm. If you would like to follow along with the actual source code for these examples on GitHub, it can be found through link for joint position joystick and link for joint position keyboard.
Usage
Start the joint position keyboard example program, ex:
$ rosrun intera_examples joint_position_keyboard.py
Start the joint position joystick example program, ex:
$ rosrun intera_examples joint_position_joystick.py
Upon startup, you will be prompted with the following:
Initializing node...
Getting robot state...
Enabling robot...
[INFO] [WallTime: 1399575163.891211] Robot Enabled
Controlling joints. Press ? for help, Esc to quit.
Arguments
Important Arguments:
See the joint position example available arguments on the command line by passing:
$ rosrun intera_examples joint_position_keyboard.py -h
usage: joint_position_keyboard.py [-h] [-l LIMB]
RSDK Joint Position Example: Keyboard Control
Use your dev machine's keyboard to control joint positions.
Each key corresponds to increasing or decreasing the angle
of a joint on Sawyer's arm. The increasing and descreasing
are represented by number key and letter key next to the number.
optional arguments:
-h, --help show this help message and exit
-l LIMB, --limb LIMB
Limb on which to run the joint position keyboard example
For joint position joystick example:
$ rosrun intera_examples joint_position_joystick.py -h
usage: joint_position_joystick.py [-h] [-l LIMB] [-j JOYSTICK]
SDK Joint Position Example: Joystick Control
Use a game controller to control the angular joint positions
of Sawyer's arms.
Attach a game controller to your dev machine and run this
example along with the ROS joy_node to control the position
of each joint in Sawyer's arm using the joystick. Be sure to
provide your *joystick* type to setup appropriate key mappings.
Each stick axis maps to a joint angle; which joints are currently
controlled can be incremented by using the mapped increment buttons.
Ex:
(x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
required arguments:
-j, --joystick specify the type of joystick to use
optional arguments:
-h, --help show this help message and exit
-l LIMB, --limb LIMB
Limb on which to run the joint position joystick example
Joint Position Keyboard Code Walkthrough
Now, let's break down the code.
33 """
34 SDK Joint Position Example: keyboard
35 """
36 import argparse
37
38 import rospy
39
40 import intera_interface
41 import intera_external_devices
42
43 from intera_interface import CHECK_VERSION
This imports the intera interface for accessing the limb and the gripper class. The intera_external_devices is imported to use its getch function, that captures the key presses on the keyboard. 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 map_keyboard(side):
44 limb = intera_interface.Limb(side)
45
46 try:
47 gripper = intera_interface.Gripper(side)
48 except:
49 has_gripper = False
50 rospy.logerr("Could not initalize the gripper.")
51 else:
52 has_gripper = True
53
54 joints = limb.joint_names()
The instance of Limb class, limb and the instance of Gripper class, gripper are created. joint_names() method returns an array of joints of the limb.
56 def set_j(limb, joint_name, delta):
57 current_position = limb.joint_angle(joint_name)
58 joint_command = {joint_name: current_position + delta}
59 limb.set_joint_positions(joint_command)
60
61 def set_g(action):
62 if has_gripper:
63 if action == "close":
64 gripper.close()
65 elif action == "open":
66 gripper.open()
67 elif action == "calibrate":
68 gripper.calibrate()
The set_j() function is invoked whenever a valid key press occurs. The limb refers to the limb instance of Sawyer's limb. delta refers to the required displacement of the joint from its current position. The method joint_angle() returns the current position of that joint. Then the joint command message is updated for that corresponding joint to indicate the new position. set_joint_position() method publishes the joint commands to the position controller.
The set_g function calls gripper action function(open(), or close() or calibrate()) when corresponding action is provided.
70 bindings = {
71 '1': (set_j, [limb, joints[0], 0.1], joints[0]+" increase"),
72 'q': (set_j, [limb, joints[0], -0.1], joints[0]+" decrease"),
73 '2': (set_j, [limb, joints[1], 0.1], joints[1]+" increase"),
74 'w': (set_j, [limb, joints[1], -0.1], joints[1]+" decrease"),
75 '3': (set_j, [limb, joints[2], 0.1], joints[2]+" increase"),
76 'e': (set_j, [limb, joints[2], -0.1], joints[2]+" decrease"),
77 '4': (set_j, [limb, joints[3], 0.1], joints[3]+" increase"),
78 'r': (set_j, [limb, joints[3], -0.1], joints[3]+" decrease"),
79 '5': (set_j, [limb, joints[4], 0.1], joints[4]+" increase"),
80 't': (set_j, [limb, joints[4], -0.1], joints[4]+" decrease"),
81 '6': (set_j, [limb, joints[5], 0.1], joints[5]+" increase"),
82 'y': (set_j, [limb, joints[5], -0.1], joints[5]+" decrease"),
83 '7': (set_j, [limb, joints[6], 0.1], joints[6]+" increase"),
84 'u': (set_j, [limb, joints[6], -0.1], joints[6]+" decrease"),
85 '8': (set_g, "close", side+" gripper close"),
86 'i': (set_g, "open", side+" gripper open"),
87 '9': (set_g, "calibrate", side+" gripper calibrate")
88 }
The bindings is a dictionary that holds the set of characters in the keyboard and their corresponding joints.
89 done = False
90 print("Controlling joints. Press ? for help, Esc to quit.")
91 while not done and not rospy.is_shutdown():
92 c = intera_external_devices.getch()
93 if c:
94 #catch Esc or ctrl-c
95 if c in ['\x1b', '\x03']:
96 done = True
97 rospy.signal_shutdown("Example finished.")
The done variable captures whether "esc" or "ctrl-c" was hit. The while loop iterates as long as the "esc" or "ctrl-c" is hit or ros-shutdown signal is given. c captures the keyboard input. If "esc" or "ctrl-c" is given then done gets updated as true, and a shutdown signal will be sent.
98 elif c in bindings:
99 cmd = bindings[c]
100 if c == '8' or c == 'i' or c == '9':
101 cmd[0](cmd[1])
102 print("command: %s" % (cmd[2],))
103 else:
104 #expand binding to something like "set_j(right, 'j0', 0.1)"
105 cmd[0](*cmd[1])
106 print("command: %s" % (cmd[2],))
The user inputted key is checked whether it is in the bindings dictionary. bindings[c] returns the value of the key c from the dictionary. The 0th index of the value refers to the function to be called and the next index holds the arguments to be sent along with that function. cmd[2] is the description of the joint command from the dictionary above.
107 else:
108 print("key bindings: ")
109 print(" Esc: Quit")
110 print(" ?: Help")
111 for key, val in sorted(bindings.items(),
112 key=lambda x: x[1][2]):
113 print(" %s: %s" % (key, val[2]))
This case is executed when the key pressed is not a valid input. So, the key is sorted and is printed along with its corresponding description.
115 def main():
116 """RSDK Joint Position Example: Keyboard Control
117 Use your dev machine's keyboard to control joint positions.
118 Each key corresponds to increasing or decreasing the angle
119 of a joint on Sawyer's arm. The increasing and descreasing
120 are represented by number key and letter key next to the number.
121 """
122 epilog = """
123 See help inside the example with the '?' key for key bindings.
124 """
125 rp = intera_interface.RobotParams()
126 valid_limbs = rp.get_limb_names()
127 if not valid_limbs:
128 rp.log_message(("Cannot detect any limb parameters on this robot. "
129 "Exiting."), "ERROR")
130 return
131 arg_fmt = argparse.RawDescriptionHelpFormatter
132 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
133 description=main.__doc__,
134 epilog=epilog)
135 parser.add_argument(
136 "-l", "--limb", dest="limb", default=valid_limbs[0],
137 choices=valid_limbs,
138 help="Limb on which to run the joint position keyboard example"
139 )
140 args = parser.parse_args(rospy.myargv()[1:])
141
142 print("Initializing node... ")
143 rospy.init_node("sdk_joint_position_keyboard")
144 print("Getting robot state... ")
145 rs = intera_interface.RobotEnable(CHECK_VERSION)
146 init_state = rs.state().enabled
The node is initialized and then the software version is checked for compatiblity with the local version. init_state captures Sawyer's initial state. This is to make sure that Sawyer is sent back to the same state after the program exits.
150 def clean_shutdown():
151 print("\nExiting example.")
152 if not init_state:
153 print("Disabling robot...")
154 rs.disable()
155 rospy.on_shutdown(clean_shutdown)
As explained above, the robot is checked if it was initially disabled and if so, it is disabled upon the program's termination.
157 rospy.loginfo("Enabling robot...")
158 rs.enable()
159 map_keyboard(args.limb)
160 print("Done.")
161
162
163 if __name__ == '__main__':
164 main()
The robot is enabled before the programs execution. It is important to note that Sawyer should be auto-enabled before trying to move its joints. map_keyboard() function captures the key input and moves the joint as explained above.
Joint Position Joystick Code Walkthrough
Now, let's break down the code.
33 import argparse
34
35 import rospy
36
37 import intera_interface
38 import intera_external_devices
39
40 from intera_interface import CHECK_VERSION
This imports the intera interface for accessing the limb and the gripper class. The intera_external_devices is imported to use its joystick 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 @param l: the list
47 """
48 if len(l):
49 v = l[0]
50 l[:-1] = l[1:]
51 l[-1] = v
The rotate function rotates a list, it will be used in bindings. 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.
54 def set_j(cmd, limb, joints, index, delta):
55 """
56 Set the selected joint to current pos + delta.
57 @param cmd: the joint command dictionary
58 @param limb: the limb to get the pos from
59 @param joints: a list of joint names
60 @param index: the index in the list of names
61 @param delta: delta to update the joint by
62 joint/index is to make this work in the bindings.
63 """
64 joint = joints[index]
65 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.
69 def map_joystick(joystick, side):
70 """
71 Maps joystick input to joint position commands.
72 @param joystick: an instance of a Joystick
73 """
74 limb = intera_interface.Limb(side)
75 gripper = None
76 try:
77 gripper = intera_interface.Gripper(side)
78 except:
79 rospy.loginfo("Could not detect a connected electric gripper.")
The instance of Limb class and the instance of Gripper class are created. No gripper will raise could not detect gripper info.
82 def set_g(action):
83 if gripper is not None:
84 if action == "close":
85 gripper.close()
86 elif action == "open":
87 gripper.open()
88 elif action == "calibrate":
89 gripper.calibrate()
The set_g function calls gripper action function(open(), or close() or calibrate()) when corresponding action is provided.
91 limb_cmd = {}
92
93 #available joints
94 joints = limb.joint_names()
95
96 #abbreviations
97 jhi = lambda s: joystick.stick_value(s) > 0
98 jlo = lambda s: joystick.stick_value(s) < 0
99 bdn = joystick.button_down
100 bup = joystick.button_up
Create instance of joint names and abbreviations for joystick values, button up and button down. The joint_names() method returns a list of joints associated with the limb. The stick_value() method returns the dead-banded, scaled and offset value of the axis. The abbreviations for all these functions are created to be used within the bindings dictionary below.
102 def print_help(bindings_list):
103 print("Press Ctrl-C to quit.")
104 for bindings in bindings_list:
105 for (test, _cmd, doc) in bindings:
106 if callable(doc):
107 doc = doc()
108 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.
110 bindings_list = []
111 bindings = [
112 ((jlo, ['leftStickHorz']), (set_j, [limb_cmd, limb, joints, 0, 0.1]),
113 lambda: "Increase " + joints[0]),
114 ((jhi, ['leftStickHorz']), (set_j, [limb_cmd, limb, joints, 0, -0.1]),
115 lambda: "Decrease " + joints[0]),
116 ((jlo, ['leftStickVert']), (set_j, [limb_cmd, limb, joints, 1, 0.1]),
117 lambda: "Increase " + joints[1]),
118 ((jhi, ['leftStickVert']), (set_j, [limb_cmd, limb, joints, 1, -0.1]),
119 lambda: "Decrease " + joints[1]),
120 ((jlo, ['rightStickHorz']), (set_j, [limb_cmd, limb, joints, 2, 0.1]),
121 lambda: "Increase " + joints[2]),
122 ((jhi, ['rightStickHorz']), (set_j, [limb_cmd, limb, joints, 2, -0.1]),
123 lambda: "Decrease " + joints[2]),
124 ((jlo, ['rightStickVert']), (set_j, [limb_cmd, limb, joints, 3, 0.1]),
125 lambda: "Increase " + joints[3]),
126 ((jhi, ['rightStickVert']), (set_j, [limb_cmd, limb, joints, 3, -0.1]),
127 lambda: "Decrease " + joints[3]),
128 ((bdn, ['leftBumper']), (rotate, [joints]), side + ": cycle joint"),
129 ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
130 ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
131 ]
132 if gripper:
133 bindings.extend([
134 ((bdn, ['rightTrigger']), (set_g, ['close'], gripper), side + " gripper close"),
135 ((bup, ['rightTrigger']), (set_g, ['open'], gripper), side + " gripper open"),
136 ((bdn, ['btnLeft']), (set_g, ['calibrate'], gripper), "right calibrate")
137 ])
138 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.
140 rate = rospy.Rate(100)
141 print_help(bindings_list)
142 print("Press Ctrl-C to stop. ")
143 while not rospy.is_shutdown():
144 for (test, cmd, doc) in bindings:
145 if test[0](*test[1]):
146 cmd[0](*cmd[1])
147 if callable(doc):
148 print(doc())
149 else:
150 print(doc)
151 if len(limb_cmd):
152 limb.set_joint_positions(limb_cmd)
153 limb_cmd.clear()
154 rate.sleep()
155 return False
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 (gripper.close,[]) is parsed as above. For the first binding, the expression cmd[0](*cmd[1]) returns the function call gripper.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] and rj[1]. The dictionaries limb_cmd hold the joint commands for the limb. These dictionaries get populated when the set_j method is called by the previous section of code.
158 def main():
159 """SDK Joint Position Example: Joystick Control
160 Use a game controller to control the angular joint positions
161 of Sawyer's arms.
162 Attach a game controller to your dev machine and run this
163 example along with the ROS joy_node to control the position
164 of each joint in Sawyer's arm using the joystick. Be sure to
165 provide your *joystick* type to setup appropriate key mappings.
166 Each stick axis maps to a joint angle; which joints are currently
167 controlled can be incremented by using the mapped increment buttons.
168 Ex:
169 (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
170 """
171 epilog = """
172 See help inside the example with the "Start" button for controller
173 key bindings.
174 """
175 rp = intera_interface.RobotParams()
176 valid_limbs = rp.get_limb_names()
177 if not valid_limbs:
178 rp.log_message(("Cannot detect any limb parameters on this robot. "
179 "Exiting."), "ERROR")
180 return
181 arg_fmt = argparse.RawDescriptionHelpFormatter
182 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
183 description=main.__doc__,
184 epilog=epilog)
185 required = parser.add_argument_group('required arguments')
186 required.add_argument(
187 '-j', '--joystick', required=True,
188 choices=['xbox', 'logitech', 'ps3'],
189 help='specify the type of joystick to use'
190 )
191 parser.add_argument(
192 "-l", "--limb", dest="limb", default=valid_limbs[0],
193 choices=valid_limbs,
194 help="Limb on which to run the joint position joystick example"
195 )
196 args = parser.parse_args(rospy.myargv()[1:])
197
198 joystick = None
199 if args.joystick == 'xbox':
200 joystick = intera_external_devices.joystick.XboxController()
201 elif args.joystick == 'logitech':
202 joystick = intera_external_devices.joystick.LogitechController()
203 elif args.joystick == 'ps3':
204 joystick = intera_external_devices.joystick.PS3Controller()
205 else:
206 parser.error("Unsupported joystick type '%s'" % (args.joystick))
207
208 print("Initializing node... ")
209 rospy.init_node("sdk_joint_position_joystick")
210 print("Getting robot state... ")
211 rs = intera_interface.RobotEnable(CHECK_VERSION)
212 init_state = rs.state().enabled
The type of joystick input is captured as entered by the user and an instance of the corresponding interface is created. The node is initialized and the robot is enabled.
217 def clean_shutdown():
218 print("\nExiting example.")
219 if not init_state:
220 print("Disabling robot...")
221 rs.disable()
222 rospy.on_shutdown(clean_shutdown)
223
224 rospy.loginfo("Enabling robot...")
225 rs.enable()
226
227 map_joystick(joystick, args.limb)
228 print("Done.")
229
230
231 if __name__ == '__main__':
232 main()
The map_joystick() method performs the mapping and execution of the commands as explained above.
