opt_runner_unittest.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2008, Willow Garage, Inc.
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of Willow Garage, Inc. nor the names of its
19 # contributors may be used to endorse or promote products derived
20 # from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 
35 # author: Vijay Pradeep
36 
37 import roslib; roslib.load_manifest('pr2_calibration_estimation')
38 
39 import sys
40 import unittest
41 import rospy
42 import numpy
43 from numpy import matrix, concatenate
44 import yaml
45 from pr2_calibration_estimation.sensors.chain_sensor import ChainSensor
46 from pr2_calibration_estimation.sensors.multi_sensor import MultiSensor
47 from pr2_calibration_estimation.sensors.tilting_laser_sensor import TiltingLaserSensor
48 from pr2_calibration_estimation.robot_params import RobotParams
49 from pr2_calibration_estimation.opt_runner import ErrorCalc
50 
51 from calibration_msgs.msg import *
52 from sensor_msgs.msg import JointState
53 
54 class TestParamJacobian(unittest.TestCase):
55  def test_single_sensor(self):
56  config = yaml.safe_load('''
57  chain_id: chainA
58  before_chain: [transformA]
59  dh_link_num: 1
60  after_chain: [transformB]
61  ''')
62 
63  robot_params = RobotParams()
64  robot_params.configure( yaml.safe_load('''
65  dh_chains:
66  chainA:
67  - [ 0, 0, 1, 0 ]
68  chainB:
69  - [ 0, 0, 2, 0 ]
70  tilting_lasers: {}
71  rectified_cams: {}
72  transforms:
73  transformA: [0, 0, 0, 0, 0, 0]
74  transformB: [0, 0, 0, 0, 0, 0]
75  checkerboards:
76  boardA:
77  corners_x: 2
78  corners_y: 2
79  spacing_x: 1
80  spacing_y: 1
81  ''' ) )
82 
83  free_dict = yaml.safe_load('''
84  dh_chains:
85  chainA:
86  - [ 1, 0, 1, 0 ]
87  chainB:
88  - [ 0, 0, 0, 0 ]
89  tilting_lasers: {}
90  rectified_cams: {}
91  transforms:
92  transformA: [1, 0, 0, 0, 0, 0]
93  transformB: [0, 0, 0, 0, 0, 0]
94  checkerboards:
95  boardA:
96  spacing_x: 1
97  spacing_y: 1
98  ''')
99 
100  chainM = ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0]))
101 
102  sensor = ChainSensor(config, chainM, "boardA" )
103  sensor.update_config(robot_params)
104 
105  target_pts = matrix([[1, 2, 1, 2],
106  [0, 0, 1, 1],
107  [0, 0, 0, 0],
108  [1, 1, 1, 1]])
109 
110  calc = ErrorCalc(robot_params, free_dict, [])
111 
112  expanded_param_vec = robot_params.deflate()
113  free_list = robot_params.calc_free(free_dict)
114  opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
115  opt_param = numpy.array(opt_param_mat)[0]
116 
117  J = calc.single_sensor_params_jacobian(opt_param, target_pts, sensor)
118 
119  print J
120 
121 class TestPoseJacobian(unittest.TestCase):
122  def test_multisensor_pose_jacobian(self):
123  configA = yaml.safe_load('''
124  chain_id: chainA
125  before_chain: [transformA]
126  dh_link_num: 1
127  after_chain: [transformB]
128  ''')
129 
130  configB = yaml.safe_load('''
131  chain_id: chainB
132  before_chain: [transformA]
133  dh_link_num: 1
134  after_chain: [transformB]
135  ''')
136 
137  robot_params = RobotParams()
138  robot_params.configure( yaml.safe_load('''
139  dh_chains:
140  chainA:
141  - [ 0, 0, 1, 0 ]
142  chainB:
143  - [ 0, 0, 2, 0 ]
144  tilting_lasers: {}
145  rectified_cams: {}
146  transforms:
147  transformA: [0, 0, 0, 0, 0, 0]
148  transformB: [0, 0, 0, 0, 0, 0]
149  checkerboards:
150  boardA:
151  corners_x: 2
152  corners_y: 2
153  spacing_x: 1
154  spacing_y: 1
155  ''' ) )
156 
157  free_dict = yaml.safe_load('''
158  dh_chains:
159  chainA:
160  - [ 1, 0, 0, 0 ]
161  chainB:
162  - [ 0, 0, 0, 0 ]
163  tilting_lasers: {}
164  rectified_cams: {}
165  transforms:
166  transformA: [0, 0, 0, 0, 0, 0]
167  transformB: [0, 0, 0, 0, 0, 0]
168  checkerboards:
169  boardA:
170  spacing_x: 0
171  spacing_y: 0
172  ''')
173 
174  sensorA = ChainSensor(configA, ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0])), "boardA")
175 
176  sensorB = ChainSensor(configB, ChainMeasurement( chain_id="chainB", chain_state=JointState(position=[0])), "boardA")
177 
178  multisensor = MultiSensor(None)
179  multisensor.sensors = [sensorA, sensorB]
180  multisensor.checkerboard = "boardA"
181 
182  pose_param_vec = numpy.array([0, 0, 0, 0, 0, 0])
183  calc = ErrorCalc(robot_params, free_dict, [])
184 
185  expanded_param_vec = robot_params.deflate()
186  free_list = robot_params.calc_free(free_dict)
187  opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
188  opt_param = numpy.array(opt_param_mat)[0]
189 
190  J = calc.multisensor_pose_jacobian(opt_param, pose_param_vec, multisensor)
191 
192  print J
193 
194 class TestFullJacobian(unittest.TestCase):
195  def test_full_jacobian(self):
196  configA = yaml.safe_load('''
197  chain_id: chainA
198  before_chain: [transformA]
199  dh_link_num: 1
200  after_chain: []
201  ''')
202 
203  configB = yaml.safe_load('''
204  chain_id: chainB
205  before_chain: []
206  dh_link_num: 1
207  after_chain: [transformB]
208  ''')
209 
210  robot_params = RobotParams()
211  robot_params.configure( yaml.safe_load('''
212  dh_chains:
213  chainA:
214  - [ 0, 0, 1, 0 ]
215  chainB:
216  - [ 0, 0, 2, 0 ]
217  tilting_lasers:
218  laserB:
219  before_joint: [0, 0, 0, 0, 0, 0]
220  after_joint: [0, 0, 0, 0, 0, 0]
221  rectified_cams: {}
222  transforms:
223  transformA: [0, 0, 0, 0, 0, 0]
224  transformB: [0, 0, 0, 0, 0, 0]
225  checkerboards:
226  boardA:
227  corners_x: 2
228  corners_y: 2
229  spacing_x: 1
230  spacing_y: 1
231  boardB:
232  corners_x: 1
233  corners_y: 1
234  spacing_x: 1
235  spacing_y: 1
236  ''' ) )
237 
238  free_dict = yaml.safe_load('''
239  dh_chains:
240  chainA:
241  - [ 1, 0, 0, 0 ]
242  chainB:
243  - [ 0, 0, 0, 0 ]
244  tilting_lasers:
245  laserB:
246  before_joint: [1, 0, 0, 0, 0, 0]
247  after_joint: [0, 0, 0, 0, 0, 0]
248  rectified_cams: {}
249  transforms:
250  transformA: [0, 0, 0, 0, 0, 0]
251  transformB: [1, 0, 0, 0, 0, 0]
252  checkerboards:
253  boardA:
254  spacing_x: 1
255  spacing_y: 1
256  boardB:
257  spacing_x: 0
258  spacing_y: 0
259  ''')
260 
261  sensorA = ChainSensor(configA, ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0])), "boardA")
262  sensorB = ChainSensor(configB, ChainMeasurement( chain_id="chainB", chain_state=JointState(position=[0])), "boardB")
263  laserB = TiltingLaserSensor({'laser_id':'laserB'}, LaserMeasurement( laser_id="laserB",
264  joint_points=[ JointState(position=[0,0,2]) ] ) )
265 
266  multisensorA = MultiSensor(None)
267  multisensorA.sensors = [sensorA]
268  multisensorA.checkerboard = "boardA"
269  multisensorA.update_config(robot_params)
270 
271  multisensorB = MultiSensor(None)
272  multisensorB.sensors = [sensorB, laserB]
273  multisensorB.checkerboard = "boardB"
274  multisensorB.update_config(robot_params)
275 
276  poseA = numpy.array([1, 0, 0, 0, 0, 0])
277  poseB = numpy.array([2, 0, 0, 0, 0, 0])
278 
279  calc = ErrorCalc(robot_params, free_dict, [multisensorA, multisensorB])
280 
281  expanded_param_vec = robot_params.deflate()
282  free_list = robot_params.calc_free(free_dict)
283  opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
284  opt_param = numpy.array(opt_param_mat)[0]
285  opt_all_vec = concatenate([opt_param, poseA, poseB])
286 
287  print "Calculating Sparse"
288  J = calc.calculate_jacobian(opt_all_vec)
289  numpy.savetxt("J_sparse.txt", J, fmt="% 4.3f")
290 
291  #import code; code.interact(local=locals())
292 
293  print "Calculating Dense"
294  from scipy.optimize.slsqp import approx_jacobian
295  J_naive = approx_jacobian(opt_all_vec, calc.calculate_error, 1e-6)
296  numpy.savetxt("J_naive.txt", J_naive, fmt="% 4.3f")
297 
298 
299  self.assertAlmostEqual(numpy.linalg.norm(J-J_naive), 0.0, 6)
300 
301 if __name__ == '__main__':
302  import rostest
303  #rostest.unitrun('pr2_calibration_estimation', 'test_opt_runner', TestParamJacobian, coverage_packages=['pr2_calibration_estimation.opt_runner'])
304  #rostest.unitrun('pr2_calibration_estimation', 'test_pose_J', TestPoseJacobian, coverage_packages=['pr2_calibration_estimation.opt_runner'])
305  rostest.unitrun('pr2_calibration_estimation', 'test_full_J', TestFullJacobian, coverage_packages=['pr2_calibration_estimation.opt_runner'])
opt_runner_unittest.TestPoseJacobian
Definition: opt_runner_unittest.py:121
opt_runner_unittest.TestFullJacobian.test_full_jacobian
def test_full_jacobian(self)
Definition: opt_runner_unittest.py:195
msg
opt_runner_unittest.TestParamJacobian.test_single_sensor
def test_single_sensor(self)
Definition: opt_runner_unittest.py:55
opt_runner_unittest.TestPoseJacobian.test_multisensor_pose_jacobian
def test_multisensor_pose_jacobian(self)
Definition: opt_runner_unittest.py:122
opt_runner_unittest.TestParamJacobian
Definition: opt_runner_unittest.py:54
opt_runner_unittest.TestFullJacobian
Definition: opt_runner_unittest.py:194

calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Tue Mar 1 2022 23:58:46