chain_sensor_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 
43 from calibration_estimation.sensors.chain_sensor import ChainBundler, ChainSensor
44 #from pr2_calibration_estimation.blocks.camera_chain import CameraChainCalcBlock
45 #from pr2_calibration_estimation.blocks.camera_chain import CameraChainRobotParamsBlock
46 
47 from calibration_msgs.msg import *
48 from sensor_msgs.msg import JointState, CameraInfo
49 
50 from calibration_estimation.single_transform import SingleTransform
51 from calibration_estimation.joint_chain import JointChain
52 from calibration_estimation.camera import RectifiedCamera
53 from calibration_estimation.tilting_laser import TiltingLaser
54 from calibration_estimation.full_chain import FullChainCalcBlock
55 from calibration_estimation.checkerboard import Checkerboard
56 from calibration_estimation.urdf_params import UrdfParams
57 
58 from numpy import *
59 
60 def loadSystem():
61  urdf = '''
62 <robot>
63  <link name="base_link"/>
64  <joint name="j0" type="fixed">
65  <origin xyz="0 0 0" rpy="0 0 0"/>
66  <parent link="base_link"/>
67  <child link="j0_link"/>
68  </joint>
69  <link name="j0_link"/>
70  <joint name="j1" type="revolute">
71  <axis xyz="0 0 1"/>
72  <origin xyz="1 0 0" rpy="0 0 0"/>
73  <parent link="j0_link"/>
74  <child link="j1_link"/>
75  </joint>
76  <link name="j1_link"/>
77  <joint name="j2" type="revolute">
78  <axis xyz="0 0 1"/>
79  <origin xyz="1 0 0" rpy="0 0 0"/>
80  <parent link="j1_link"/>
81  <child link="j2_link"/>
82  </joint>
83  <link name="j2_link"/>
84  <joint name="j3" type="fixed">
85  <origin xyz="0 0 0" rpy="0 0 0"/>
86  <parent link="j2_link"/>
87  <child link="j3_link"/>
88  </joint>
89  <link name="j3_link"/>
90 </robot>
91 '''
92  config = yaml.load('''
93 sensors:
94  chains:
95  chainA:
96  sensor_id: chainA
97  root: j0_link
98  tip: j1_link
99  cov:
100  joint_angles: [1]
101  gearing: [1.0]
102  chainB:
103  sensor_id: chainB
104  root: j0_link
105  tip: j2_link
106  cov:
107  joint_angles: [1, 1]
108  gearing: [1.0, 1.0]
109  rectified_cams: {}
110  tilting_lasers: {}
111 transforms:
112  chainA_cb: [0, 0, 0, 0, 0, 0]
113  chainB_cb: [0, 0, 0, 0, 0, 0]
114 checkerboards:
115  boardA:
116  corners_x: 2
117  corners_y: 2
118  spacing_x: 1
119  spacing_y: 1
120 ''')
121 
122  return config["sensors"]["chains"], UrdfParams(urdf, config)
123 
124 
125 class TestChainBundler(unittest.TestCase):
126  def test_basic_match(self):
127  config, robot_params = loadSystem()
128 
129  bundler = ChainBundler(config.values())
130 
131  M_robot = RobotMeasurement( target_id = "targetA",
132  chain_id = "chainA",
133  M_cam = [],
134  M_chain = [ ChainMeasurement(chain_id="chainA") ])
135 
136  blocks = bundler.build_blocks(M_robot)
137 
138  self.assertEqual( len(blocks), 1)
139  block = blocks[0]
140  self.assertEqual( block._M_chain.chain_id, "chainA")
141  self.assertEqual( block._target_id, "targetA")
142 
144  config, robot_params = loadSystem()
145 
146  bundler = ChainBundler(config.values())
147 
148  M_robot = RobotMeasurement( target_id = "targetA",
149  chain_id = "chainA",
150  M_chain = [ ChainMeasurement(chain_id="chainB") ])
151 
152  blocks = bundler.build_blocks(M_robot)
153 
154  self.assertEqual( len(blocks), 0)
155 
156 class TestChainSensor(unittest.TestCase):
157  def test_cov(self):
158  config, robot_params = loadSystem()
159  block = ChainSensor(config["chainA"],
160  ChainMeasurement(chain_id="chainA",
161  chain_state=JointState(position=[0]) ),
162  "boardA")
163  block.update_config(robot_params)
164  cov = block.compute_cov(None)
165  print cov
166 
167  #self.assertAlmostEqual(cov[0,0], 0.0, 6)
168  #self.assertAlmostEqual(cov[1,0], 0.0, 6)
169  #self.assertAlmostEqual(cov[1,1], 1.0, 6)
170  #self.assertAlmostEqual(cov[4,4], 4.0, 6)
171 
172  def test_update1(self):
173  config, robot_params = loadSystem()
174  block = ChainSensor(config["chainA"],
175  ChainMeasurement(chain_id="chainA",
176  chain_state=JointState(position=[0]) ),
177  "boardA")
178  block.update_config(robot_params)
179 
180  target = matrix([[0.5, 1.5, 0.5, 1.5],
181  [0.5, 0.5, -0.5, -0.5],
182  [0, 0, 0, 0],
183  [1, 1, 1, 1]])
184 
185  h = block.compute_expected(target)
186  z = block.get_measurement()
187  r = block.compute_residual(target)
188 
189  self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
190 
191  print "z=\n",z
192  print "target=\n",target
193 
194  self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
195  self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
196 
197  def test_update2(self):
198  config, robot_params = loadSystem()
199  block = ChainSensor(config["chainA"],
200  ChainMeasurement(chain_id="chainA",
201  chain_state=JointState(position=[numpy.pi / 2.0]) ),
202  "boardA")
203  block.update_config(robot_params)
204 
205  target = matrix([[0.5, 0.5,1.5,1.5],
206  [-0.5, 0.5, -0.5, 0.5],
207  [0, 0, 0, 0],
208  [1, 1, 1, 1]])
209 
210  h = block.compute_expected(target)
211  z = block.get_measurement()
212  r = block.compute_residual(target)
213 
214  self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
215 
216  print "z=\n",z
217  print "target=\n",target
218 
219  self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
220  self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
221 
222  def test_update3(self):
223  config, robot_params = loadSystem()
224  block = ChainSensor(config["chainB"],
225  ChainMeasurement(chain_id="chainB",
226  chain_state=JointState(position=[0.0, 0.0]) ),
227  "boardA")
228  block.update_config(robot_params)
229 
230  target = matrix([[1.5, 2.5, 1.5, 2.5],
231  [0.5, 0.5, -0.5, -0.5],
232  [0, 0, 0, 0],
233  [1, 1, 1, 1]])
234 
235  h = block.compute_expected(target)
236  z = block.get_measurement()
237  r = block.compute_residual(target)
238 
239  self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6)
240 
241  print "z=\n",z
242  print "target=\n",target
243 
244  self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6)
245  self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6)
246 
247  def test_sparsity(self):
248  config, robot_params = loadSystem()
249  block = ChainSensor(config["chainA"],
250  ChainMeasurement(chain_id="chainA",
251  chain_state=JointState(position=[numpy.pi / 2.0]) ),
252  "boardA")
253  block.update_config(robot_params)
254  sparsity = block.build_sparsity_dict()
255  self.assertEqual(sparsity['transforms']['j0'], [1,1,1,1,1,1])
256  self.assertEqual(sparsity['transforms']['j1'], [1,1,1,1,1,1])
257  self.assertEqual(sparsity['chains']['chainA'], {'gearing':[1]})
258 
259 if __name__ == '__main__':
260  import rostest
261  rostest.unitrun('calibration_estimation', 'test_ChainBundler', TestChainBundler)
262  rostest.unitrun('calibration_estimation', 'test_ChainSensor', TestChainSensor)


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