full_chain_unittest.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 
35 import roslib; roslib.load_manifest('calibration_estimation')
36 
37 import sys
38 import unittest
39 import rospy
40 import numpy
41 import yaml
42 from sensor_msgs.msg import JointState
43 from calibration_estimation.full_chain import FullChainRobotParams
44 from calibration_estimation.single_transform import SingleTransform
45 from calibration_estimation.joint_chain import JointChain
46 from calibration_estimation.urdf_params import UrdfParams
47 
48 from numpy import *
49 import numpy
50 
52  urdf = '''
53 <robot>
54  <link name="base_link"/>
55  <joint name="j0" type="fixed">
56  <origin xyz="10 0 0" rpy="0 0 0"/>
57  <parent link="base_link"/>
58  <child link="j0_link"/>
59  </joint>
60  <link name="j0_link"/>
61  <joint name="j1" type="revolute">
62  <axis xyz="0 0 1"/>
63  <origin xyz="1 0 0" rpy="0 0 0"/>
64  <parent link="j0_link"/>
65  <child link="j1_link"/>
66  </joint>
67  <link name="j1_link"/>
68  <joint name="j2" type="revolute">
69  <axis xyz="0 0 1"/>
70  <origin xyz="0 0 0" rpy="0 0 0"/>
71  <parent link="j1_link"/>
72  <child link="j2_link"/>
73  </joint>
74  <link name="j2_link"/>
75  <joint name="j3" type="fixed">
76  <origin xyz="0 0 20" rpy="0 0 0"/>
77  <parent link="j2_link"/>
78  <child link="j3_link"/>
79  </joint>
80  <link name="j3_link"/>
81 </robot>
82 '''
83  config = yaml.load('''
84 sensors:
85  chains:
86  chain1:
87  root: j0_link
88  tip: j2_link
89  cov:
90  joint_angles: [0.001, 0.001]
91  gearing: [1.0, 1.0]
92  rectified_cams: {}
93  tilting_lasers: {}
94 transforms: {}
95 checkerboards: {}
96 ''')
97 
98  return UrdfParams(urdf, config)
99 
100 class TestFullChainCalcBlock(unittest.TestCase):
101 
102  def test_fk_1(self):
103  print ""
104  params = loadSystem1()
105  chain = FullChainRobotParams('chain1', 'j3_link')
106  chain.update_config(params)
107 
108  chain_state = JointState(position=[0, 0])
109  result = chain.calc_block.fk(chain_state)
110  expected = numpy.matrix( [[ 1, 0, 0,11],
111  [ 0, 1, 0, 0],
112  [ 0, 0, 1,20],
113  [ 0, 0, 0, 1]], float )
114  print result
115  self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
116 
117  def test_fk_2(self):
118  print ""
119  params = loadSystem1()
120  chain = FullChainRobotParams('chain1', 'j3_link')
121  chain.update_config(params)
122 
123  chain_state = JointState(position=[pi/2, 0])
124  result = chain.calc_block.fk(chain_state)
125  expected = numpy.matrix( [[ 0,-1, 0,11],
126  [ 1, 0, 0, 0],
127  [ 0, 0, 1,20],
128  [ 0, 0, 0, 1]], float )
129  print result
130  self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
131 
132  def test_fk_partial(self):
133  print ""
134  params = loadSystem1()
135  chain = FullChainRobotParams('chain1', 'j1_link')
136  chain.update_config(params)
137 
138  chain_state = JointState(position=[0, 0])
139  result = chain.calc_block.fk(chain_state)
140  expected = numpy.matrix( [[ 1, 0, 0,11],
141  [ 0, 1, 0, 0],
142  [ 0, 0, 1, 0],
143  [ 0, 0, 0, 1]], float )
144  print result
145  self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
146 
147 
148 if __name__ == '__main__':
149  import rostest
150  rostest.unitrun('calibration_estimation', 'test_FullChainCalcBlock', TestFullChainCalcBlock, coverage_packages=['calibration_estimation.full_chain', 'calibration_estimation.urdf_params'])


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Fri Apr 2 2021 02:12:53