Joint Position Keyboard - Code Walkthrough

From sdk-wiki
Revision as of 15:16, 29 July 2014 by Sdkuser (talk | contribs) (Protected "Joint Position Keyboard - Code Walkthrough": Setting Permissions for (almost all) Pages - editable and moveable by all logged in users ([Edit=Allow only autoconfirmed users] (indefinite) [Move=Allow only autoconfirmed users] (indefinite)))
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

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.