Introduction
This example demonstrates the ability of baxter to move its limbs to previous recorded positions read from a file. The main() function captures the name of the file from which the joint positions are to be read. map_file() function parses the file and moves the joints to the specified positions.
Interfaces -
- Gripper.error()
- Gripper.reset()
- Gripper.calibrated()
- Gripper.type()
- Gripper.calibrate()
- Gripper.command_positions(<double>)
- Limb.move_to_joint_positions(<Joint command>)
- Limb.set_joint_positions(<Joint Command>)
Code Walkthrough
Now, let's break down the code.
33 import argparse
34 import sys
35
36 import rospy
37
38 import baxter_interface
39
40 from baxter_interface import CHECK_VERSION
This imports the baxter interface for accessing the limb and the gripper class. 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.
50 def clean_line(line, names):
51 """
52 Cleans a single line of recorded joint positions
53
54 @param line: the line described in a list to process
55 @param names: joint name keys
56 """
57 #convert the line of strings to a float or None
58 line = [try_float(x) for x in line.rstrip().split(',')]
59 #zip the values with the joint names
60 combined = zip(names[1:], line[1:])
61 #take out any tuples that have a none value
62 cleaned = [x for x in combined if x[1] is not None]
63 #convert it to a dictionary with only valid commands
64 command = dict(cleaned)
65 left_command = dict((key, command[key]) for key in command.keys()
66 if key[:-2] == 'left_')
67 right_command = dict((key, command[key]) for key in command.keys()
68 if key[:-2] == 'right_')
69 return (command, left_command, right_command, line)
This first line calls try_float() function to replace the passed list's contents with float and "none" values. Then, a tuple is constructed by zipping the names passed with the values and the variable line. This may contain valid values as well as none values . After removing the none values, a dictionary is constructed. This dictionary is then parsed to create the left and right joint commands.
72 def map_file(filename, loops=1):
73 """
74 Loops through csv file
75
76 @param filename: the file to play
77 @param loops: number of times to loop
78 values < 0 mean 'infinite'
79
80 Does not loop indefinitely, but only until the file is read
81 and processed. Reads each line, split up in columns and
82 formats each line into a controller command in the form of
83 name/value pairs. Names come from the column headers
84 first column is the time stamp
85 """
86 left = baxter_interface.Limb('left')
87 right = baxter_interface.Limb('right')
88 grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
89 grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
90 rate = rospy.Rate(1000)
Instances of the left and right, limb and gripper classes are created. The software version is checked for its compatiblity with the local version.
92 if grip_left.error():
93 grip_left.reset()
94 if grip_right.error():
95 grip_right.reset()
96 if (not grip_left.calibrated() and
97 grip_left.type() != 'custom'):
98 grip_left.calibrate()
99 if (not grip_right.calibrated() and
100 grip_right.type() != 'custom'):
101 grip_right.calibrate()
error() method returns if the gripper is in an error state. The possible cause of errors might be over/undervoltage, over/under current, motor faults, etc. calibrated() method indicates if the gripper is already calibrated, while calibrate() performs the actual calibration of the grippers. type() method returns if the gripper is a custom, electric or someother type. The calibration check is skipped if the gripper is a custom gripper.
103 print("Playing back: %s" % (filename,))
104 with open(filename, 'r') as f:
105 lines = f.readlines()
106 keys = lines[0].rstrip().split(',')
107
108 l = 0
109 # If specified, repeat the file playback 'loops' number of times
110 while loops < 1 or l < loops:
111 i = 0
112 l += 1
113 print("Moving to start position...")
114
115 _cmd, lcmd_start, rcmd_start, _raw = clean_line(lines[1], keys)
116 left.move_to_joint_positions(lcmd_start)
117 right.move_to_joint_positions(rcmd_start)
118 start_time = rospy.get_time()
After the file's contents are parsed as a string, they are split into individual elements using "," as the delimiter. The variable loops indicates the number of times the robot should move to the joint positions. Each line in lines hold the joint positions in the same order as they were recorded. keys holds the first joint position that was recorded. clean_line() function parses the string and returns them as joint commands, as explained above. The method move_to_joint_positions(), moves the joints to the commanded position. It is important to note that there is no trajectory planning involved here. Instead, this is passed onto a low pass filter and the intermediate positions between the start and the goal position are obtained. They are then published as a joint command message using the set_joint_positions() method.
119 for values in lines[1:]:
120 i += 1
121 loopstr = str(loops) if loops > 0 else "forever"
122 sys.stdout.write("\r Record %d of %d, loop %d of %s" %
123 (i, len(lines) - 1, l, loopstr))
124 sys.stdout.flush()
125
126 cmd, lcmd, rcmd, values = clean_line(values, keys)
lines[1:] holds all the joint positions other than the starting position. The lines are looped from the 1st position till the end, and the left and right joint commands are captured using the clean_line() function, as explained above.
128 while (rospy.get_time() - start_time) < values[0]:
129 if rospy.is_shutdown():
130 print("\n Aborting - ROS shutdown")
131 return False
132 if len(lcmd):
133 left.set_joint_positions(lcmd)
134 if len(rcmd):
135 right.set_joint_positions(rcmd)
136 if ('left_gripper' in cmd and
137 grip_left.type() != 'custom'):
138 grip_left.command_position(cmd['left_gripper'])
139 if ('right_gripper' in cmd and
140 grip_right.type() != 'custom'):
141 grip_right.command_position(cmd['right_gripper'])
142 rate.sleep()
143 print
144 return True
As discussed above set_joint_positions() method publishes the positions as a joint command and command_position() publishes the gripper commands. The left, right grippers are verified if they are not custom grippers.
147 def main():
148 """RSDK Joint Position Example: File Playback
149
150 Uses Joint Position Control mode to play back a series of
151 recorded joint and gripper positions.
152
153 Run the joint_recorder.py example first to create a recording
154 file for use with this example. This example uses position
155 control to replay the recorded positions in sequence.
156
157 Note: This version of the playback example simply drives the
158 joints towards the next position at each time stamp. Because
159 it uses Position Control it will not attempt to adjust the
160 movement speed to hit set points "on time".
161 """
162 epilog = """
163 Related examples:
164 joint_recorder.py; joint_trajectory_file_playback.py.
165 """
166 arg_fmt = argparse.RawDescriptionHelpFormatter
167 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
168 description=main.__doc__,
169 epilog=epilog)
170 parser.add_argument(
171 '-f', '--file', metavar='PATH', required=True,
172 help='path to input file'
173 )
174 parser.add_argument(
175 '-l', '--loops', type=int, default=1,
176 help='number of times to loop the input file. 0=infinite.'
177 )
178 args = parser.parse_args(rospy.myargv()[1:])
The path of the file from which the joint positions are to be played back is captured along with the number of times the playback should occur.
180 print("Initializing node... ")
181 rospy.init_node("rsdk_joint_position_file_playback")
182 print("Getting robot state... ")
183 rs = baxter_interface.RobotEnable(CHECK_VERSION)
184 init_state = rs.state().enabled
185
186 def clean_shutdown():
187 print("\nExiting example...")
188 if not init_state:
189 print("Disabling robot...")
190 rs.disable()
191 rospy.on_shutdown(clean_shutdown)
192
193 print("Enabling robot... ")
194 rs.enable()
195
196 map_file(args.file, args.loops)
197
198
199 if __name__ == '__main__':
200 main()
The robot is enabled and the software version is checked if it is compatible with the local version. The map_file() function parses the file contents and plays back the joint positions as explained above.
