pr2_calibration_estimation: pr2_calibration_estimation: opt_runner

00001 #! /usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2008, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of Willow Garage, Inc. nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 # author: Vijay Pradeep
00036 
00037 import roslib; roslib.load_manifest('pr2_calibration_estimation')
00038 
00039 import sys
00040 import unittest
00041 import rospy
00042 import numpy
00043 from numpy import matrix, concatenate
00044 import yaml
00045 from pr2_calibration_estimation.sensors.chain_sensor import ChainSensor
00046 from pr2_calibration_estimation.sensors.multi_sensor import MultiSensor
00047 from pr2_calibration_estimation.sensors.tilting_laser_sensor import TiltingLaserSensor
00048 from pr2_calibration_estimation.robot_params import RobotParams
00049 from pr2_calibration_estimation.opt_runner import ErrorCalc
00050 
00051 from calibration_msgs.msg import *
00052 from sensor_msgs.msg import JointState
00053 
00054 class TestParamJacobian(unittest.TestCase):
00055     def test_single_sensor(self):
00056         config = yaml.load('''
00057                 chain_id: chainA
00058                 before_chain: [transformA]
00059                 dh_link_num:  1
00060                 after_chain:  [transformB]
00061             ''')
00062 
00063         robot_params = RobotParams()
00064         robot_params.configure( yaml.load('''
00065             dh_chains:
00066               chainA:
00067               - [ 0, 0, 1, 0 ]
00068               chainB:
00069               - [ 0, 0, 2, 0 ]
00070             tilting_lasers: {}
00071             rectified_cams: {}
00072             transforms:
00073                 transformA: [0, 0, 0, 0, 0, 0]
00074                 transformB: [0, 0, 0, 0, 0, 0]
00075             checkerboards:
00076               boardA:
00077                 corners_x: 2
00078                 corners_y: 2
00079                 spacing_x: 1
00080                 spacing_y: 1
00081             ''' ) )
00082 
00083         free_dict = yaml.load('''
00084             dh_chains:
00085               chainA:
00086               - [ 1, 0, 1, 0 ]
00087               chainB:
00088               - [ 0, 0, 0, 0 ]
00089             tilting_lasers: {}
00090             rectified_cams: {}
00091             transforms:
00092                 transformA: [1, 0, 0, 0, 0, 0]
00093                 transformB: [0, 0, 0, 0, 0, 0]
00094             checkerboards:
00095               boardA:
00096                 spacing_x: 1
00097                 spacing_y: 1
00098             ''')
00099 
00100         chainM = ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0]))
00101 
00102         sensor = ChainSensor(config, chainM, "boardA" )
00103         sensor.update_config(robot_params)
00104 
00105         target_pts = matrix([[1, 2, 1, 2],
00106                              [0, 0, 1, 1],
00107                              [0, 0, 0, 0],
00108                              [1, 1, 1, 1]])
00109 
00110         calc = ErrorCalc(robot_params, free_dict, [])
00111 
00112         expanded_param_vec = robot_params.deflate()
00113         free_list = robot_params.calc_free(free_dict)
00114         opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
00115         opt_param = numpy.array(opt_param_mat)[0]
00116 
00117         J = calc.single_sensor_params_jacobian(opt_param, target_pts, sensor)
00118 
00119         print J
00120 
00121 class TestPoseJacobian(unittest.TestCase):
00122     def test_multisensor_pose_jacobian(self):
00123         configA = yaml.load('''
00124                 chain_id: chainA
00125                 before_chain: [transformA]
00126                 dh_link_num:  1
00127                 after_chain:  [transformB]
00128             ''')
00129 
00130         configB = yaml.load('''
00131                 chain_id: chainB
00132                 before_chain: [transformA]
00133                 dh_link_num:  1
00134                 after_chain:  [transformB]
00135             ''')
00136 
00137         robot_params = RobotParams()
00138         robot_params.configure( yaml.load('''
00139             dh_chains:
00140               chainA:
00141               - [ 0, 0, 1, 0 ]
00142               chainB:
00143               - [ 0, 0, 2, 0 ]
00144             tilting_lasers: {}
00145             rectified_cams: {}
00146             transforms:
00147                 transformA: [0, 0, 0, 0, 0, 0]
00148                 transformB: [0, 0, 0, 0, 0, 0]
00149             checkerboards:
00150               boardA:
00151                 corners_x: 2
00152                 corners_y: 2
00153                 spacing_x: 1
00154                 spacing_y: 1
00155             ''' ) )
00156 
00157         free_dict = yaml.load('''
00158             dh_chains:
00159               chainA:
00160               - [ 1, 0, 0, 0 ]
00161               chainB:
00162               - [ 0, 0, 0, 0 ]
00163             tilting_lasers: {}
00164             rectified_cams: {}
00165             transforms:
00166                 transformA: [0, 0, 0, 0, 0, 0]
00167                 transformB: [0, 0, 0, 0, 0, 0]
00168             checkerboards:
00169               boardA:
00170                 spacing_x: 0
00171                 spacing_y: 0
00172             ''')
00173 
00174         sensorA = ChainSensor(configA, ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0])), "boardA")
00175 
00176         sensorB = ChainSensor(configB, ChainMeasurement( chain_id="chainB", chain_state=JointState(position=[0])), "boardA")
00177 
00178         multisensor = MultiSensor(None)
00179         multisensor.sensors = [sensorA, sensorB]
00180         multisensor.checkerboard = "boardA"
00181 
00182         pose_param_vec = numpy.array([0, 0, 0, 0, 0, 0])
00183         calc = ErrorCalc(robot_params, free_dict, [])
00184 
00185         expanded_param_vec = robot_params.deflate()
00186         free_list = robot_params.calc_free(free_dict)
00187         opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
00188         opt_param = numpy.array(opt_param_mat)[0]
00189 
00190         J = calc.multisensor_pose_jacobian(opt_param, pose_param_vec, multisensor)
00191 
00192         print J
00193 
00194 class TestFullJacobian(unittest.TestCase):
00195     def test_full_jacobian(self):
00196         configA = yaml.load('''
00197                 chain_id: chainA
00198                 before_chain: [transformA]
00199                 dh_link_num:  1
00200                 after_chain:  []
00201             ''')
00202 
00203         configB = yaml.load('''
00204                 chain_id: chainB
00205                 before_chain: []
00206                 dh_link_num:  1
00207                 after_chain:  [transformB]
00208             ''')
00209 
00210         robot_params = RobotParams()
00211         robot_params.configure( yaml.load('''
00212             dh_chains:
00213               chainA:
00214               - [ 0, 0, 1, 0 ]
00215               chainB:
00216               - [ 0, 0, 2, 0 ]
00217             tilting_lasers:
00218               laserB:
00219                 before_joint: [0, 0, 0, 0, 0, 0]
00220                 after_joint:  [0, 0, 0, 0, 0, 0]
00221             rectified_cams: {}
00222             transforms:
00223                 transformA: [0, 0, 0, 0, 0, 0]
00224                 transformB: [0, 0, 0, 0, 0, 0]
00225             checkerboards:
00226               boardA:
00227                 corners_x: 2
00228                 corners_y: 2
00229                 spacing_x: 1
00230                 spacing_y: 1
00231               boardB:
00232                 corners_x: 1
00233                 corners_y: 1
00234                 spacing_x: 1
00235                 spacing_y: 1
00236             ''' ) )
00237 
00238         free_dict = yaml.load('''
00239             dh_chains:
00240               chainA:
00241               - [ 1, 0, 0, 0 ]
00242               chainB:
00243               - [ 0, 0, 0, 0 ]
00244             tilting_lasers:
00245               laserB:
00246                 before_joint: [1, 0, 0, 0, 0, 0]
00247                 after_joint:  [0, 0, 0, 0, 0, 0]
00248             rectified_cams: {}
00249             transforms:
00250                 transformA: [0, 0, 0, 0, 0, 0]
00251                 transformB: [1, 0, 0, 0, 0, 0]
00252             checkerboards:
00253               boardA:
00254                 spacing_x: 1
00255                 spacing_y: 1
00256               boardB:
00257                 spacing_x: 0
00258                 spacing_y: 0
00259             ''')
00260 
00261         sensorA = ChainSensor(configA, ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0])), "boardA")
00262         sensorB = ChainSensor(configB, ChainMeasurement( chain_id="chainB", chain_state=JointState(position=[0])), "boardB")
00263         laserB  = TiltingLaserSensor({'laser_id':'laserB'}, LaserMeasurement( laser_id="laserB",
00264                                                                               joint_points=[ JointState(position=[0,0,2]) ] ) )
00265 
00266         multisensorA = MultiSensor(None)
00267         multisensorA.sensors = [sensorA]
00268         multisensorA.checkerboard = "boardA"
00269         multisensorA.update_config(robot_params)
00270 
00271         multisensorB = MultiSensor(None)
00272         multisensorB.sensors = [sensorB, laserB]
00273         multisensorB.checkerboard = "boardB"
00274         multisensorB.update_config(robot_params)
00275 
00276         poseA = numpy.array([1, 0, 0, 0, 0, 0])
00277         poseB = numpy.array([2, 0, 0, 0, 0, 0])
00278 
00279         calc = ErrorCalc(robot_params, free_dict, [multisensorA, multisensorB])
00280 
00281         expanded_param_vec = robot_params.deflate()
00282         free_list = robot_params.calc_free(free_dict)
00283         opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
00284         opt_param = numpy.array(opt_param_mat)[0]
00285         opt_all_vec = concatenate([opt_param, poseA, poseB])
00286 
00287         print "Calculating Sparse"
00288         J = calc.calculate_jacobian(opt_all_vec)
00289         numpy.savetxt("J_sparse.txt", J, fmt="% 4.3f")
00290 
00291         #import code; code.interact(local=locals())
00292 
00293         print "Calculating Dense"
00294         from scipy.optimize.slsqp import approx_jacobian
00295         J_naive = approx_jacobian(opt_all_vec, calc.calculate_error, 1e-6)
00296         numpy.savetxt("J_naive.txt", J_naive, fmt="% 4.3f")
00297 
00298 
00299         self.assertAlmostEqual(numpy.linalg.norm(J-J_naive), 0.0, 6)
00300 
00301 if __name__ == '__main__':
00302     import rostest
00303     #rostest.unitrun('pr2_calibration_estimation', 'test_opt_runner', TestParamJacobian, coverage_packages=['pr2_calibration_estimation.opt_runner'])
00304     #rostest.unitrun('pr2_calibration_estimation', 'test_pose_J', TestPoseJacobian, coverage_packages=['pr2_calibration_estimation.opt_runner'])
00305     rostest.unitrun('pr2_calibration_estimation', 'test_full_J', TestFullJacobian, coverage_packages=['pr2_calibration_estimation.opt_runner'])
opt_runner_unittest.py
