full_chain_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('calibration_estimation')
00036 
00037 import sys
00038 import unittest
00039 import rospy
00040 import numpy
00041 import yaml
00042 from sensor_msgs.msg import JointState
00043 from calibration_estimation.full_chain import FullChainRobotParams
00044 from calibration_estimation.single_transform import SingleTransform
00045 from calibration_estimation.joint_chain import JointChain
00046 from calibration_estimation.urdf_params import UrdfParams
00047 
00048 from numpy import *
00049 import numpy
00050 
00051 def loadSystem1():
00052     urdf = '''
00053 <robot>
00054   <link name="base_link"/>
00055   <joint name="j0" type="fixed">
00056     <origin xyz="10 0 0" rpy="0 0 0"/>
00057     <parent link="base_link"/>
00058     <child link="j0_link"/>
00059   </joint>
00060   <link name="j0_link"/>
00061   <joint name="j1" type="revolute">
00062     <axis xyz="0 0 1"/>
00063     <origin xyz="1 0 0" rpy="0 0 0"/>
00064     <parent link="j0_link"/>
00065     <child link="j1_link"/>
00066   </joint>
00067   <link name="j1_link"/>
00068   <joint name="j2" type="revolute">
00069     <axis xyz="0 0 1"/>
00070     <origin xyz="0 0 0" rpy="0 0 0"/>
00071     <parent link="j1_link"/>
00072     <child link="j2_link"/>
00073   </joint>
00074   <link name="j2_link"/>
00075   <joint name="j3" type="fixed">
00076     <origin xyz="0 0 20" rpy="0 0 0"/>
00077     <parent link="j2_link"/>
00078     <child link="j3_link"/>
00079   </joint>
00080   <link name="j3_link"/>
00081 </robot>
00082 '''
00083     config = yaml.load('''
00084 sensors:
00085   chains:
00086     chain1:
00087       root: j0_link
00088       tip: j2_link
00089       cov:
00090         joint_angles: [0.001, 0.001]
00091       gearing: [1.0, 1.0]
00092   rectified_cams: {}
00093   tilting_lasers: {}
00094 transforms: {}
00095 checkerboards: {}
00096 ''')
00097         
00098     return UrdfParams(urdf, config)
00099 
00100 class TestFullChainCalcBlock(unittest.TestCase):
00101 
00102     def test_fk_1(self):
00103         print ""
00104         params = loadSystem1()
00105         chain = FullChainRobotParams('chain1', 'j3_link')
00106         chain.update_config(params)
00107 
00108         chain_state = JointState(position=[0, 0])
00109         result = chain.calc_block.fk(chain_state)
00110         expected = numpy.matrix( [[ 1, 0, 0,11],
00111                                   [ 0, 1, 0, 0],
00112                                   [ 0, 0, 1,20],
00113                                   [ 0, 0, 0, 1]], float )
00114         print result
00115         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
00116 
00117     def test_fk_2(self):
00118         print ""
00119         params = loadSystem1()
00120         chain = FullChainRobotParams('chain1', 'j3_link')
00121         chain.update_config(params)
00122 
00123         chain_state = JointState(position=[pi/2, 0])
00124         result = chain.calc_block.fk(chain_state)
00125         expected = numpy.matrix( [[ 0,-1, 0,10],
00126                                   [ 1, 0, 0, 1],
00127                                   [ 0, 0, 1,20],
00128                                   [ 0, 0, 0, 1]], float )
00129         print result
00130         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
00131 
00132     def test_fk_partial(self):
00133         print ""
00134         params = loadSystem1()
00135         chain = FullChainRobotParams('chain1', 'j1_link')
00136         chain.update_config(params)
00137 
00138         chain_state = JointState(position=[0, 0])
00139         result = chain.calc_block.fk(chain_state)
00140         expected = numpy.matrix( [[ 1, 0, 0,11],
00141                                   [ 0, 1, 0, 0],
00142                                   [ 0, 0, 1, 0],
00143                                   [ 0, 0, 0, 1]], float )
00144         print result
00145         self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
00146 
00147 
00148 if __name__ == '__main__':
00149     import rostest
00150     rostest.unitrun('calibration_estimation', 'test_FullChainCalcBlock',   TestFullChainCalcBlock,   coverage_packages=['calibration_estimation.full_chain', 'calibration_estimation.urdf_params'])
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Thu Aug 15 2013 10:15:40