pr2_head_fk_unittest.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 
00035 import roslib; roslib.load_manifest('pr2_calibration_estimation')
00036 
00037 import sys
00038 import unittest
00039 import rospy
00040 import time
00041 
00042 from pr2_calibration_estimation.full_chain import FullChainRobotParams
00043 from pr2_calibration_estimation.robot_params import RobotParams
00044 from sensor_msgs.msg import JointState
00045 
00046 import yaml
00047 from pr2_calibration_estimation.srv import FkTest
00048 from numpy import *
00049 import numpy
00050 
00051 class LoadData(unittest.TestCase):
00052     def setUp(self):
00053 
00054 
00055 
00056         #config_filename = "config/system.yaml"
00057         #f = open(config_filename)
00058         f = rospy.get_param("system")
00059         all_config = yaml.load(f)
00060 
00061         self.robot_params = RobotParams()
00062         self.robot_params.configure(all_config)
00063 
00064 
00065         rospy.wait_for_service('fk', 3.0)
00066         self._fk_ref = rospy.ServiceProxy('fk', FkTest)
00067         #f.close()
00068 
00069     def loadCommands(self, param_name):
00070         #f = open(filename)
00071         #cmds = [ [ float(y) for y in x.split()] for x in f.readlines()]
00072         #f.close()
00073 
00074         command_str = rospy.get_param(param_name)
00075 
00076 
00077         cmds = [ [float(y) for y in x.split()] for x in command_str.strip().split('\n')]
00078 
00079         return cmds
00080 
00081     def getExpected(self, root, tip, cmd):
00082         resp = self._fk_ref(root, tip, cmd)
00083         #print resp
00084         T = matrix(zeros((4,4)), float)
00085         T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
00086         T[3,3] = 1.0;
00087         T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
00088         return T
00089 
00090 class TestPR2Fk(LoadData):
00091 
00092     def run_test(self, full_chain, root, tip, cmds):
00093         for cmd in cmds:
00094             print "On Command: %s" % cmd
00095 
00096             expected_T = self.getExpected(root, tip, cmd)
00097             chain_state = JointState(position=cmd)
00098             actual_T = full_chain.calc_block.fk(chain_state)
00099 
00100             print "Expected_T:"
00101             print expected_T
00102             print "Actual T:"
00103             print actual_T
00104 
00105             self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
00106 
00107     def test_head_tilt_link(self):
00108         print ""
00109 
00110         config_str = '''
00111         before_chain: [head_pan_joint]
00112         chain_id:     head_chain
00113         after_chain:  [head_chain_tip_adj]
00114         dh_link_num:  1
00115         '''
00116 
00117         full_chain = FullChainRobotParams(yaml.load(config_str))
00118         full_chain.update_config(self.robot_params)
00119 
00120         cmds = self.loadCommands('head_commands')
00121 
00122         self.run_test(full_chain, 'torso_lift_link', 'head_tilt_link', cmds)
00123 
00124     def test_head_plate(self):
00125         print ""
00126         config_str = '''
00127         before_chain: [head_pan_joint]
00128         chain_id:     head_chain
00129         after_chain:  [head_chain_tip_adj, head_plate_frame_joint]
00130         dh_link_num:  1'''
00131         full_chain = FullChainRobotParams(yaml.load(config_str))
00132         full_chain.update_config(self.robot_params)
00133         cmds = self.loadCommands('head_commands')
00134         self.run_test(full_chain, 'torso_lift_link', 'head_plate_frame', cmds)
00135 
00136     def test_double_stereo(self):
00137         print ""
00138         config_str = '''
00139         before_chain: [head_pan_joint]
00140         chain_id:     head_chain
00141         after_chain:  [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint]
00142         dh_link_num:  1 '''
00143         full_chain = FullChainRobotParams(yaml.load(config_str))
00144         full_chain.update_config(self.robot_params)
00145         cmds = self.loadCommands('head_commands')
00146         self.run_test(full_chain, 'torso_lift_link', 'double_stereo_link', cmds)
00147 
00148     def test_wide_stereo(self):
00149         print ""
00150         config_str = '''
00151         before_chain: [head_pan_joint]
00152         chain_id:     head_chain
00153         after_chain:  [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, wide_stereo_frame_joint]
00154         dh_link_num:  1 '''
00155         full_chain = FullChainRobotParams(yaml.load(config_str))
00156         full_chain.update_config(self.robot_params)
00157         cmds = self.loadCommands('head_commands')
00158         self.run_test(full_chain, 'torso_lift_link', 'wide_stereo_link', cmds)
00159 
00160     def test_wide_stereo_optical(self):
00161         print ""
00162         config_str = '''
00163         before_chain: [head_pan_joint]
00164         chain_id:     head_chain
00165         after_chain:  [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, wide_stereo_frame_joint, wide_stereo_optical_frame_joint]
00166         dh_link_num:  1 '''
00167         full_chain = FullChainRobotParams(yaml.load(config_str))
00168         full_chain.update_config(self.robot_params)
00169         cmds = self.loadCommands('head_commands')
00170         self.run_test(full_chain, 'torso_lift_link', 'wide_stereo_optical_frame', cmds)
00171 
00172     def test_narrow_stereo(self):
00173         print ""
00174         config_str = '''
00175         before_chain: [head_pan_joint]
00176         chain_id:     head_chain
00177         after_chain:  [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, narrow_stereo_frame_joint]
00178         dh_link_num:  1 '''
00179         full_chain = FullChainRobotParams(yaml.load(config_str))
00180         full_chain.update_config(self.robot_params)
00181         cmds = self.loadCommands('head_commands')
00182         self.run_test(full_chain, 'torso_lift_link', 'narrow_stereo_link', cmds)
00183 
00184     def test_narrow_stereo_optical(self):
00185         print ""
00186         config_str = '''
00187         before_chain: [head_pan_joint]
00188         chain_id:     head_chain
00189         after_chain:  [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, narrow_stereo_frame_joint, narrow_stereo_optical_frame_joint]
00190         dh_link_num:  1 '''
00191         full_chain = FullChainRobotParams(yaml.load(config_str))
00192         full_chain.update_config(self.robot_params)
00193         cmds = self.loadCommands('head_commands')
00194         self.run_test(full_chain, 'torso_lift_link', 'narrow_stereo_optical_frame', cmds)
00195 
00196     def test_high_def(self):
00197         print ""
00198         config_str = '''
00199         before_chain: [head_pan_joint]
00200         chain_id:     head_chain
00201         after_chain:  [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint]
00202         dh_link_num:  1 '''
00203         full_chain = FullChainRobotParams(yaml.load(config_str))
00204         full_chain.update_config(self.robot_params)
00205         cmds = self.loadCommands('head_commands')
00206         self.run_test(full_chain, 'torso_lift_link', 'high_def_frame', cmds)
00207 
00208     def test_high_def_optical(self):
00209         print ""
00210         config_str = '''
00211         before_chain: [head_pan_joint]
00212         chain_id:     head_chain
00213         after_chain:  [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint, high_def_optical_frame_joint]
00214         dh_link_num:  1 '''
00215         full_chain = FullChainRobotParams(yaml.load(config_str))
00216         full_chain.update_config(self.robot_params)
00217         cmds = self.loadCommands('head_commands')
00218         self.run_test(full_chain, 'torso_lift_link', 'high_def_optical_frame', cmds)
00219 
00220 #   def test_kinect_head_def(self):
00221 #       print ""
00222 #       config_str = '''
00223 #       before_chain: [head_pan_joint]
00224 #       chain_id:     head_chain
00225 #       after_chain:  [head_chain_tip_adj, head_plate_frame_joint, kinect_head_rgb_frame_joint]
00226 #       dh_link_num:  1 '''
00227 #       full_chain = FullChainRobotParams(yaml.load(config_str))
00228 #       full_chain.update_config(self.robot_params)
00229 #       cmds = self.loadCommands('head_commands')
00230 #       self.run_test(full_chain, 'torso_lift_link', 'kinect_head_rgb_frame', cmds)
00231 #
00232 #   def test_kinect_head_optical(self):
00233 #       print ""
00234 #       config_str = '''
00235 #       before_chain: [head_pan_joint]
00236 #       chain_id:     head_chain
00237 #       after_chain:  [head_chain_tip_adj, head_plate_frame_joint, kinect_head_rgb_frame_joint, kinect_head_rgb_optical_frame_joint]
00238 #       dh_link_num:  1 '''
00239 #       full_chain = FullChainRobotParams(yaml.load(config_str))
00240 #       full_chain.update_config(self.robot_params)
00241 #       cmds = self.loadCommands('head_commands')
00242 #       self.run_test(full_chain, 'torso_lift_link', 'kinect_head_rgb_optical_frame', cmds)
00243 #
00244 #   def test_kinect_torso_def(self):
00245 #       print ""
00246 #       config_str = '''
00247 #       before_chain: [kinect_torso_rgb_frame_joint, kinect_torso_rgb_optical_frame_joint]
00248 #       chain_id:     NULL
00249 #       after_chain:  [] '''
00250 #       full_chain = FullChainRobotParams(yaml.load(config_str))
00251 #       full_chain.update_config(self.robot_params)
00252 #       cmds = self.loadCommands('head_commands')
00253 #       self.run_test(full_chain, 'torso_lift_link', 'kinect_torso_rgb_frame', cmds)
00254 #
00255 #   def test_kinect_torso_optical(self):
00256 #       print ""
00257 #       config_str = '''
00258 #       before_chain: [kinect_torso_rgb_frame_joint, kinect_torso_rgb_optical_frame_joint]
00259 #       chain_id:     NULL
00260 #       after_chain:  [] '''
00261 #       full_chain = FullChainRobotParams(yaml.load(config_str))
00262 #       full_chain.update_config(self.robot_params)
00263 #       cmds = self.loadCommands('head_commands')
00264 #       self.run_test(full_chain, 'torso_lift_link', 'kinect_torso_rgb_optical_frame', cmds)
00265 
00266     def check_tilt_laser(self, cmd):
00267         actual_T = self.robot_params.tilting_lasers["tilt_laser"].compute_pose([cmd])
00268         expected_T = self.getExpected("torso_lift_link", "laser_tilt_link", [cmd])
00269 
00270         print "Expected_T:"
00271         print expected_T
00272         print "Actual T:"
00273         print actual_T
00274 
00275         self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
00276 
00277     def test_tilt_laser(self):
00278         print ""
00279         self.check_tilt_laser(0)
00280         self.check_tilt_laser(1)
00281 
00282 if __name__ == '__main__':
00283     import rostest
00284     rospy.init_node("fk_test")
00285     rostest.unitrun('pr2_calibration_estimation', 'test_PR2Fk', TestPR2Fk)


pr2_calibration_launch
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:37