joint_position_file_playback.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2015, Rethink Robotics
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. Redistributions in binary form must reproduce the above copyright
00012 #    notice, this list of conditions and the following disclaimer in the
00013 #    documentation and/or other materials provided with the distribution.
00014 # 3. Neither the name of the Rethink Robotics nor the names of its
00015 #    contributors may be used to endorse or promote products derived from
00016 #    this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 """
00031 Baxter RSDK Joint Position Example: file playback
00032 """
00033 import argparse
00034 import sys
00035 
00036 import rospy
00037 
00038 import baxter_interface
00039 
00040 from baxter_interface import CHECK_VERSION
00041 
00042 
00043 def try_float(x):
00044     try:
00045         return float(x)
00046     except ValueError:
00047         return None
00048 
00049 
00050 def clean_line(line, names):
00051     """
00052     Cleans a single line of recorded joint positions
00053 
00054     @param line: the line described in a list to process
00055     @param names: joint name keys
00056     """
00057     #convert the line of strings to a float or None
00058     line = [try_float(x) for x in line.rstrip().split(',')]
00059     #zip the values with the joint names
00060     combined = zip(names[1:], line[1:])
00061     #take out any tuples that have a none value
00062     cleaned = [x for x in combined if x[1] is not None]
00063     #convert it to a dictionary with only valid commands
00064     command = dict(cleaned)
00065     left_command = dict((key, command[key]) for key in command.keys()
00066                         if key[:-2] == 'left_')
00067     right_command = dict((key, command[key]) for key in command.keys()
00068                          if key[:-2] == 'right_')
00069     return (command, left_command, right_command, line)
00070 
00071 
00072 def map_file(filename, loops=1):
00073     """
00074     Loops through csv file
00075 
00076     @param filename: the file to play
00077     @param loops: number of times to loop
00078                   values < 0 mean 'infinite'
00079 
00080     Does not loop indefinitely, but only until the file is read
00081     and processed. Reads each line, split up in columns and
00082     formats each line into a controller command in the form of
00083     name/value pairs. Names come from the column headers
00084     first column is the time stamp
00085     """
00086     left = baxter_interface.Limb('left')
00087     right = baxter_interface.Limb('right')
00088     grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
00089     grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
00090     rate = rospy.Rate(1000)
00091 
00092     if grip_left.error():
00093         grip_left.reset()
00094     if grip_right.error():
00095         grip_right.reset()
00096     if (not grip_left.calibrated() and
00097         grip_left.type() != 'custom'):
00098         grip_left.calibrate()
00099     if (not grip_right.calibrated() and
00100         grip_right.type() != 'custom'):
00101         grip_right.calibrate()
00102 
00103     print("Playing back: %s" % (filename,))
00104     with open(filename, 'r') as f:
00105         lines = f.readlines()
00106     keys = lines[0].rstrip().split(',')
00107 
00108     l = 0
00109     # If specified, repeat the file playback 'loops' number of times
00110     while loops < 1 or l < loops:
00111         i = 0
00112         l += 1
00113         print("Moving to start position...")
00114 
00115         _cmd, lcmd_start, rcmd_start, _raw = clean_line(lines[1], keys)
00116         left.move_to_joint_positions(lcmd_start)
00117         right.move_to_joint_positions(rcmd_start)
00118         start_time = rospy.get_time()
00119         for values in lines[1:]:
00120             i += 1
00121             loopstr = str(loops) if loops > 0 else "forever"
00122             sys.stdout.write("\r Record %d of %d, loop %d of %s" %
00123                              (i, len(lines) - 1, l, loopstr))
00124             sys.stdout.flush()
00125 
00126             cmd, lcmd, rcmd, values = clean_line(values, keys)
00127             #command this set of commands until the next frame
00128             while (rospy.get_time() - start_time) < values[0]:
00129                 if rospy.is_shutdown():
00130                     print("\n Aborting - ROS shutdown")
00131                     return False
00132                 if len(lcmd):
00133                     left.set_joint_positions(lcmd)
00134                 if len(rcmd):
00135                     right.set_joint_positions(rcmd)
00136                 if ('left_gripper' in cmd and
00137                     grip_left.type() != 'custom'):
00138                     grip_left.command_position(cmd['left_gripper'])
00139                 if ('right_gripper' in cmd and
00140                     grip_right.type() != 'custom'):
00141                     grip_right.command_position(cmd['right_gripper'])
00142                 rate.sleep()
00143         print
00144     return True
00145 
00146 
00147 def main():
00148     """RSDK Joint Position Example: File Playback
00149 
00150     Uses Joint Position Control mode to play back a series of
00151     recorded joint and gripper positions.
00152 
00153     Run the joint_recorder.py example first to create a recording
00154     file for use with this example. This example uses position
00155     control to replay the recorded positions in sequence.
00156 
00157     Note: This version of the playback example simply drives the
00158     joints towards the next position at each time stamp. Because
00159     it uses Position Control it will not attempt to adjust the
00160     movement speed to hit set points "on time".
00161     """
00162     epilog = """
00163 Related examples:
00164   joint_recorder.py; joint_trajectory_file_playback.py.
00165     """
00166     arg_fmt = argparse.RawDescriptionHelpFormatter
00167     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00168                                      description=main.__doc__,
00169                                      epilog=epilog)
00170     parser.add_argument(
00171         '-f', '--file', metavar='PATH', required=True,
00172         help='path to input file'
00173     )
00174     parser.add_argument(
00175         '-l', '--loops', type=int, default=1,
00176         help='number of times to loop the input file. 0=infinite.'
00177     )
00178     args = parser.parse_args(rospy.myargv()[1:])
00179 
00180     print("Initializing node... ")
00181     rospy.init_node("rsdk_joint_position_file_playback")
00182     print("Getting robot state... ")
00183     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00184     init_state = rs.state().enabled
00185 
00186     def clean_shutdown():
00187         print("\nExiting example...")
00188         if not init_state:
00189             print("Disabling robot...")
00190             rs.disable()
00191     rospy.on_shutdown(clean_shutdown)
00192 
00193     print("Enabling robot... ")
00194     rs.enable()
00195 
00196     map_file(args.file, args.loops)
00197 
00198 
00199 if __name__ == '__main__':
00200     main()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Sat Jun 8 2019 20:16:27