Joint Position File Playback - Code Walkthrough

From sdk-wiki
Jump to: navigation, search


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
36 import rospy
38 import baxter_interface
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
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
76     @param filename: the file to play
77     @param loops: number of times to loop
78                   values < 0 mean 'infinite'
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(',')
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...")
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()
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
150     Uses Joint Position Control mode to play back a series of
151     recorded joint and gripper positions.
153     Run the 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.
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:
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
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)
193     print("Enabling robot... ")
194     rs.enable()
196     map_file(args.file, args.loops)
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.