cb_check.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2010, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of the Willow Garage nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 ##\author Kevin Watts
00036 ##\brief Prints output of PR2 counterbalance check
00037 
00038 PKG = 'pr2_counterbalance_check'
00039 import roslib
00040 roslib.load_manifest(PKG)
00041 
00042 import os, sys
00043 import rospy
00044 
00045 from pr2_self_test_msgs.srv import *
00046 from std_msgs.msg import Bool
00047 from joint_qualification_controllers.msg import CounterbalanceTestData
00048 
00049 from pr2_counterbalance_check.counterbalance_analysis import *
00050 
00051 from optparse import OptionParser
00052 
00053 def dir(turns):
00054     return 'CW' if turns > 0 else 'CCW'
00055 
00056 SPRING_TOL = 2.0
00057 BAR_TOL = 0.8
00058 
00059 def _check_valid(params):
00060     if params.timeout_hit:
00061         print >> sys.stderr, "Counterbalance controller timeout hit. Increase teh timeout and retry."
00062         return False
00063     
00064     return True
00065 
00066 class CounterbalanceCheck(object):
00067     def __init__(self, model_file):
00068         self._motors_halted = True
00069         self._data = None
00070 
00071         self.motors_topic = rospy.Subscriber('pr2_etherCAT/motors_halted', Bool, self._motors_cb)
00072         self.data_topic = rospy.Subscriber('cb_test_data', CounterbalanceTestData, self._data_callback)
00073 
00074         self._model_file = model_file
00075 
00076         self._ok = True
00077 
00078     @property
00079     def has_data(self): return self._data is not None
00080 
00081     def _motors_cb(self, msg):
00082         self._motors_halted = msg.data
00083         
00084         if self._motors_halted:
00085             print >> sys.stderr, "Motors halted. Adjustment data will be invalid."
00086 
00087         self._ok = self._ok and not self._motors_halted
00088 
00089     @property
00090     def ok(self): return self._ok
00091 
00092     def _data_callback(self, msg):
00093         self._data = msg
00094         
00095     def process_results(self):
00096         data = CounterbalanceAnalysisData(self._data)
00097         params = CounterbalanceAnalysisParams(self._data)
00098 
00099         if not _check_valid(params):
00100             print >> sys.stderr, "Unable to calculate adjustment, invalid data"
00101             return
00102 
00103         (secondary, cb_bar) = calc_cb_adjust(data, self._model_file)
00104 
00105         if (abs(secondary) > 25 or abs(cb_bar) > 25):
00106             print >> sys.stderr, "Unable to calculate CB adjustment. This could mean the counterbalance is extremely out of adjustment, or your training data is invalid."
00107             return
00108 
00109         print 'Calculated counterbalance adjustment recommendations:'
00110         print '\tSecondary Spring: %.1f (%s)' % (abs(secondary), dir(secondary))
00111         print '\tArm Gimbal Shaft: %.1f (%s)' % (abs(cb_bar), dir(cb_bar))
00112         print 'Make sure to follow proper adjustment procedures if you choose to make an adjustment.\n'
00113         print 'Your counterbalance state is up to you, these adjustments are only recommendations.\n'
00114 
00115             
00116 if __name__ == '__main__':
00117     parser = OptionParser("./cb_check.py model_file")
00118     parser.add_option("-t", "--tol", action="store_true",
00119                       dest="tolerance", default=False,
00120                       help="Print recommended tolerances and exit")
00121     
00122     options, args = parser.parse_args(rospy.myargv())
00123     
00124     if options.tolerance:
00125         print 'Recommended CB tolerances for secondary spring, arm gimbal shaft'
00126         print '\tSecondary spring: +/-%.1f' % SPRING_TOL
00127         print '\tArm Gimbal Shaft: +/-%.1f' % BAR_TOL
00128         print 'If your recommended adjustment is less than this many turns, do not adjust your CB'
00129         sys.exit()
00130 
00131     if len(args) < 2:
00132         parser.error("No model file specified. Please give training data file to calculate adjustment")
00133         sys.exit(2)
00134 
00135     if not os.path.exists(args[1]):
00136         parser.error("Model file does not exist. Please give training data file to calculate adjustment")
00137         sys.exit(2)
00138 
00139     rospy.init_node('cb_analysis')
00140     app = CounterbalanceCheck(args[1])
00141     try:
00142         my_rate = rospy.Rate(5)
00143         while app.ok and not app.has_data and not rospy.is_shutdown():
00144             my_rate.sleep()
00145 
00146         if not app.ok:
00147             print >> sys.stderr, "Unable to calculate adjustment. Check motors halted. Please retry."
00148             sys.exit(2)
00149 
00150         if not rospy.is_shutdown():
00151             app.process_results()
00152     except KeyboardInterrupt, e:
00153         pass
00154     except Exception, e:
00155         print 'Caught Exception in CB application check'
00156         import traceback
00157         traceback.print_exc()


pr2_counterbalance_check
Author(s): Kevin Watts
autogenerated on Tue Apr 22 2014 19:40:12