cb_analysis_test.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 ##\brief Unit tests of counterbalance analysis code
00036 
00037 from __future__ import division
00038 
00039 PKG = 'pr2_counterbalance_check'
00040 import roslib; roslib.load_manifest(PKG)
00041 
00042 from pr2_counterbalance_check.counterbalance_analysis import *
00043 
00044 import rostest, unittest
00045 
00046 import copy
00047 import os, sys
00048 
00049 # Dummy classes to store data
00050 class DummyCBAnalysisParams(object): pass
00051 class DummyCBAnalysisData(object): pass
00052 class DummyCBRunAnalysisData(object): pass
00053 class DummyCBPositionAnalysisData(object): pass
00054 class DummyJointPositionAnalysisData(object): pass
00055 
00056 def get_position(minv, maxv, num_pts, i):
00057     return minv + (maxv - minv) * i / num_pts
00058 
00059 def generate_data(params):
00060     data = DummyCBAnalysisData()
00061     data.lift_data = []
00062     for i in range(params.num_lifts):
00063         lift_data = DummyCBRunAnalysisData()
00064         lift_data.lift_position = get_position(params.min_lift, params.max_lift, params.num_lifts, i)
00065         lift_data.flex_data = []
00066         
00067         for j in range(params.num_flexes):
00068             fd = DummyCBPositionAnalysisData()
00069             fd.flex_position = get_position(params.min_flex, params.max_flex, params.num_flexes, j)
00070             
00071             fd.lift_hold = DummyJointPositionAnalysisData()
00072             fd.lift_hold.position_avg = lift_data.lift_position
00073             fd.lift_hold.position_sd  = 0.0
00074             fd.lift_hold.effort_avg   = j / 100.0 - params.num_flexes/200.0
00075             fd.lift_hold.effort_sd    = 0.0
00076 
00077             fd.flex_hold = DummyJointPositionAnalysisData()
00078             fd.flex_hold.position_avg = fd.flex_position
00079             fd.flex_hold.position_sd  = 0.0
00080             fd.flex_hold.effort_avg   = j / 100.0 - params.num_flexes/200.0
00081             fd.flex_hold.effort_sd    = 0.0
00082             
00083             lift_data.flex_data.append(fd)
00084 
00085         data.lift_data.append(lift_data)
00086 
00087     return data
00088 
00089 class TestCounterbalanceAnalysis(unittest.TestCase):
00090     def setUp(self):
00091         # Set up parameters
00092         self.params = DummyCBAnalysisParams()
00093         self.params.lift_dither  = 5.0
00094         self.params.flex_dither  = 5.0
00095         self.params.lift_joint   = 'lift_joint'
00096         self.params.flex_joint   = 'flex_joint'
00097         self.params.timeout_hit  = False
00098         self.params.flex_test    = True # Set to False for lift-only test
00099         self.params.lift_mse     = 1.0
00100         self.params.lift_avg_abs = 1.0
00101         self.params.lift_avg_eff = 1.0
00102         self.params.flex_mse     = 1.0
00103         self.params.flex_avg_abs = 1.0
00104         self.params.flex_avg_eff = 1.0
00105 
00106         self.params.screw_tol = 2.0
00107         self.params.bar_tol   = 0.8
00108 
00109         self.params.num_lifts = 8
00110         self.params.min_lift = -0.2
00111         self.params.max_lift = 1.2
00112 
00113         self.params.num_flexes = 9
00114         self.params.min_flex = -1.8
00115         self.params.max_flex = -0.2
00116 
00117         # Set up data
00118         self.data = generate_data(self.params)
00119 
00120         # Model file for analyzing data
00121         self.model_file = os.path.join(roslib.packages.get_pkg_dir(PKG), 'cb_data/counterbalance_model.dat')
00122             
00123 
00124     def test_lift_effort(self):
00125         result = analyze_lift_efforts(self.params, self.data)
00126         self.assert_(result.result, "Lift effort result wasn't OK. %s\n%s" % (result.summary, result.html))
00127 
00128     def test_flex_effort(self):
00129         result = analyze_flex_efforts(self.params, self.data)
00130         self.assert_(result.result, "Flex effort result wasn't OK. %s\n%s" % (result.summary, result.html))
00131 
00132 
00133     
00134     
00135     def test_adjustment(self):
00136         """
00137         Test that CB adjustment runs successfully
00138         """
00139         (secondary, bar) = calc_cb_adjust(self.data, self.model_file)
00140 
00141         self.assert_(abs(secondary) < self.params.screw_tol and abs(bar) < self.params.bar_tol,
00142                      "Calculated adjustment didn't match. Adjustment: %.2f, %.2f" % (secondary, bar))
00143 
00144         adjust_result = check_cb_adjustment(self.params, self.data, self.model_file)
00145         self.assert_(adjust_result.result, "Adjustment result was unsuccessful! %s\n%s" % (adjust_result.summary, adjust_result.html))
00146         
00147         # Bad data has invalid number of dimensions
00148         bad_params = copy.deepcopy(self.params)
00149         bad_params.num_lifts = 7
00150         bad_params.max_lift = 1.0
00151         
00152         bad_data = generate_data(bad_params)
00153 
00154         (secondary, bar) = calc_cb_adjust(bad_data, self.model_file)
00155 
00156         self.assert_(abs(secondary) > 50 and abs(bar) > 50,
00157                      "Calculated adjustment successful on bad data. Adjustment: %.2f, %.2f" % (secondary, bar))
00158         
00159             
00160     def test_plots(self):
00161         p_contout_lift = plot_effort_contour(self.params, self.data, True)
00162         p_contout_flex = plot_effort_contour(self.params, self.data, False)
00163 
00164         p_eff = plot_efforts_by_lift_position(self.params, self.data)
00165 
00166 
00167 if __name__ == '__main__':
00168     rostest.unitrun(PKG, 'test_cb_analysis', TestCounterbalanceAnalysis)


pr2_counterbalance_check
Author(s): Kevin Watts
autogenerated on Sat Dec 28 2013 17:30:27