recorder.py
Go to the documentation of this file.
00001 # Copyright (c) 2013-2014, Rethink Robotics
00002 # All rights reserved.
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions are met:
00006 #
00007 # 1. Redistributions of source code must retain the above copyright notice,
00008 #    this list of conditions and the following disclaimer.
00009 # 2. Redistributions in binary form must reproduce the above copyright
00010 #    notice, this list of conditions and the following disclaimer in the
00011 #    documentation and/or other materials provided with the distribution.
00012 # 3. Neither the name of the Rethink Robotics nor the names of its
00013 #    contributors may be used to endorse or promote products derived from
00014 #    this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026 # POSSIBILITY OF SUCH DAMAGE.
00027 
00028 import rospy
00029 
00030 import baxter_interface
00031 
00032 from baxter_interface import CHECK_VERSION
00033 
00034 
00035 class JointRecorder(object):
00036     def __init__(self, filename, rate):
00037         """
00038         Records joint data to a file at a specified rate.
00039         """
00040         self._filename = filename
00041         self._raw_rate = rate
00042         self._rate = rospy.Rate(rate)
00043         self._start_time = rospy.get_time()
00044         self._done = False
00045 
00046         self._limb_left = baxter_interface.Limb("left")
00047         self._limb_right = baxter_interface.Limb("right")
00048         self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION)
00049         self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION)
00050         self._io_left_lower = baxter_interface.DigitalIO('left_lower_button')
00051         self._io_left_upper = baxter_interface.DigitalIO('left_upper_button')
00052         self._io_right_lower = baxter_interface.DigitalIO('right_lower_button')
00053         self._io_right_upper = baxter_interface.DigitalIO('right_upper_button')
00054 
00055         # Verify Grippers Have No Errors and are Calibrated
00056         if self._gripper_left.error():
00057             self._gripper_left.reset()
00058         if self._gripper_right.error():
00059             self._gripper_right.reset()
00060         if (not self._gripper_left.calibrated() and
00061             self._gripper_left.type() != 'custom'):
00062             self._gripper_left.calibrate()
00063         if (not self._gripper_right.calibrated() and
00064             self._gripper_right.type() != 'custom'):
00065             self._gripper_right.calibrate()
00066 
00067     def _time_stamp(self):
00068         return rospy.get_time() - self._start_time
00069 
00070     def stop(self):
00071         """
00072         Stop recording.
00073         """
00074         self._done = True
00075 
00076     def done(self):
00077         """
00078         Return whether or not recording is done.
00079         """
00080         if rospy.is_shutdown():
00081             self.stop()
00082         return self._done
00083 
00084     def record(self):
00085         """
00086         Records the current joint positions to a csv file if outputFilename was
00087         provided at construction this function will record the latest set of
00088         joint angles in a csv format.
00089 
00090         This function does not test to see if a file exists and will overwrite
00091         existing files.
00092         """
00093         if self._filename:
00094             joints_left = self._limb_left.joint_names()
00095             joints_right = self._limb_right.joint_names()
00096             with open(self._filename, 'w') as f:
00097                 f.write('time,')
00098                 f.write(','.join([j for j in joints_left]) + ',')
00099                 f.write('left_gripper,')
00100                 f.write(','.join([j for j in joints_right]) + ',')
00101                 f.write('right_gripper\n')
00102 
00103                 while not self.done():
00104                     # Look for gripper button presses
00105                     if self._io_left_lower.state:
00106                         self._gripper_left.open()
00107                     elif self._io_left_upper.state:
00108                         self._gripper_left.close()
00109                     if self._io_right_lower.state:
00110                         self._gripper_right.open()
00111                     elif self._io_right_upper.state:
00112                         self._gripper_right.close()
00113                     angles_left = [self._limb_left.joint_angle(j)
00114                                    for j in joints_left]
00115                     angles_right = [self._limb_right.joint_angle(j)
00116                                     for j in joints_right]
00117 
00118                     f.write("%f," % (self._time_stamp(),))
00119 
00120                     f.write(','.join([str(x) for x in angles_left]) + ',')
00121                     f.write(str(self._gripper_left.position()) + ',')
00122 
00123                     f.write(','.join([str(x) for x in angles_right]) + ',')
00124                     f.write(str(self._gripper_right.position()) + '\n')
00125 
00126                     self._rate.sleep()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Fri Oct 3 2014 16:37:39