Introduction
This example demonstrates the usage of the position controller to move the joints to various positions via the keyboard interface. The main()
function enables the robot and invokes the map_keyboard()
function. It is at this function where the keyboard's keys are mapped to individual joints and the commands are sent to the position controller, periodically.
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 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.
41 def map_keyboard():
42 left = baxter_interface.Limb('left')
43 right = baxter_interface.Limb('right')
44 grip_left = baxter_interface.Gripper('left')
45 grip_right = baxter_interface.Gripper('right')
46 lj = left.joint_names()
47 rj = right.joint_names()
Two instances of the Limb class are created for each of Baxter's limbs. Similarly two instances of the Gripper class for the left and right grippers are created. joint_names()
method returns an array of joints in that limb.
51 def set_j(limb, joint_name, delta):
52 current_position = limb.joint_angle(joint_name)
53 joint_command = {joint_name: current_position + delta}
54 limb.set_joint_positions(joint_command)
The set_j()
function is invoked whenever a valid key press occurs. The limb
refers to the limb instance of Baxter's limbs for the corresponding side. 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_positions()
method publishes the joint commands to the position controller.
56 bindings = {
57 # key: (function, args, description)
58 '9': (set_j, [left, lj[0], 0.1], "left_s0 increase"),
59 '6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"),
60 '8': (set_j, [left, lj[1], 0.1], "left_s1 increase"),
61 '7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"),
62 'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"),
63 'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"),
64 'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"),
65 'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"),
66 'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"),
67 'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"),
68 'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"),
69 'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"),
70 '.': (set_j, [left, lj[6], 0.1], "left_w2 increase"),
71 'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
72 ',': (grip_left.close, [], "left: gripper close"),
73 'm': (grip_left.open, [], "left: gripper open"),
74 '/': (grip_left.calibrate, [], "left: gripper calibrate"),
75
76 '4': (set_j, [right, rj[0], 0.1], "right_s0 increase"),
77 '1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"),
78 '3': (set_j, [right, rj[1], 0.1], "right_s1 increase"),
79 '2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"),
80 'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"),
81 'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"),
82 'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"),
83 'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"),
84 'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"),
85 'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"),
86 'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"),
87 's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"),
88 'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"),
89 'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"),
90 'c': (grip_right.close, [], "right: gripper close"),
91 'x': (grip_right.open, [], "right: gripper open"),
92 'b': (grip_right.calibrate, [], "right: gripper calibrate"),
93 }
The bindings
is a dictionary that holds the set of characters in the keyboard and their corresponding joints.
94 done = False
95 print("Controlling joints. Press ? for help, Esc to quit.")
96 while not done and not rospy.is_shutdown():
97 c = baxter_external_devices.getch()
98 if c:
99 #catch Esc or ctrl-c
100 if c in ['\x1b', '\x03']:
101 done = True
102 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.
103 elif c in bindings:
104 cmd = bindings[c]
105 #expand binding to something like "set_j(right, 's0', 0.1)"
106 cmd[0](*cmd[1])
107 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.
108 else:
109 print("key bindings: ")
110 print(" Esc: Quit")
111 print(" ?: Help")
112 for key, val in sorted(bindings.items(),
113 key=lambda x: x[1][2]):
114 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.
117 def main():
118 """RSDK Joint Position Example: Keyboard Control
119
120 Use your dev machine's keyboard to control joint positions.
121
122 Each key corresponds to increasing or decreasing the angle
123 of a joint on one of Baxter's arms. Each arm is represented
124 by one side of the keyboard and inner/outer key pairings
125 on each row for each joint.
126 """
127 epilog = """
128 See help inside the example with the '?' key for key bindings.
129 """
130 arg_fmt = argparse.RawDescriptionHelpFormatter
131 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
132 description=main.__doc__,
133 epilog=epilog)
134 parser.parse_args(rospy.myargv()[1:])
135
136 print("Initializing node... ")
137 rospy.init_node("rsdk_joint_position_keyboard")
138 print("Getting robot state... ")
139 rs = baxter_interface.RobotEnable(CHECK_VERSION)
140 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 Baxter's initial state. This is to make sure that Baxter is sent back to the same state after the program exits.
142 def clean_shutdown():
143 print("\nExiting example...")
144 if not init_state:
145 print("Disabling robot...")
146 rs.disable()
147 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.
149 print("Enabling robot... ")
150 rs.enable()
151
152 map_keyboard()
153 print("Done.")
154
155
156 if __name__ == '__main__':
157 main()
The robot is enabled before the programs execution. It is important to note that Baxter should be enabled before trying to move its joints. map_keyboard()
function captures the key input and moves the joint as explained above.