00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 import roslib; roslib.load_manifest('pr2_calibration_estimation')
00036
00037 import sys
00038 import unittest
00039 import rospy
00040 import time
00041
00042 from pr2_calibration_estimation.full_chain import FullChainRobotParams
00043 from pr2_calibration_estimation.robot_params import RobotParams
00044 from sensor_msgs.msg import JointState
00045
00046 import yaml
00047 from pr2_calibration_estimation.srv import FkTest
00048 from numpy import *
00049 import numpy
00050
00051 class LoadData(unittest.TestCase):
00052 def setUp(self):
00053
00054
00055
00056
00057
00058 f = rospy.get_param("system")
00059 all_config = yaml.load(f)
00060
00061 self.robot_params = RobotParams()
00062 self.robot_params.configure(all_config)
00063
00064
00065 rospy.wait_for_service('fk', 3.0)
00066 self._fk_ref = rospy.ServiceProxy('fk', FkTest)
00067
00068
00069 def loadCommands(self, param_name):
00070
00071
00072
00073
00074 command_str = rospy.get_param(param_name)
00075
00076
00077 cmds = [ [float(y) for y in x.split()] for x in command_str.strip().split('\n')]
00078
00079 return cmds
00080
00081 def getExpected(self, root, tip, cmd):
00082 resp = self._fk_ref(root, tip, cmd)
00083
00084 T = matrix(zeros((4,4)), float)
00085 T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
00086 T[3,3] = 1.0;
00087 T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
00088 return T
00089
00090 class TestPR2Fk(LoadData):
00091
00092 def run_test(self, full_chain, root, tip, cmds):
00093 for cmd in cmds:
00094 print "On Command: %s" % cmd
00095
00096 expected_T = self.getExpected(root, tip, cmd)
00097 chain_state = JointState(position=cmd)
00098 actual_T = full_chain.calc_block.fk(chain_state)
00099
00100 print "Expected_T:"
00101 print expected_T
00102 print "Actual T:"
00103 print actual_T
00104
00105 self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
00106
00107 def test_head_tilt_link(self):
00108 print ""
00109
00110 config_str = '''
00111 before_chain: [head_pan_joint]
00112 chain_id: head_chain
00113 after_chain: [head_chain_tip_adj]
00114 dh_link_num: 1
00115 '''
00116
00117 full_chain = FullChainRobotParams(yaml.load(config_str))
00118 full_chain.update_config(self.robot_params)
00119
00120 cmds = self.loadCommands('head_commands')
00121
00122 self.run_test(full_chain, 'torso_lift_link', 'head_tilt_link', cmds)
00123
00124 def test_head_plate(self):
00125 print ""
00126 config_str = '''
00127 before_chain: [head_pan_joint]
00128 chain_id: head_chain
00129 after_chain: [head_chain_tip_adj, head_plate_frame_joint]
00130 dh_link_num: 1'''
00131 full_chain = FullChainRobotParams(yaml.load(config_str))
00132 full_chain.update_config(self.robot_params)
00133 cmds = self.loadCommands('head_commands')
00134 self.run_test(full_chain, 'torso_lift_link', 'head_plate_frame', cmds)
00135
00136 def test_double_stereo(self):
00137 print ""
00138 config_str = '''
00139 before_chain: [head_pan_joint]
00140 chain_id: head_chain
00141 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint]
00142 dh_link_num: 1 '''
00143 full_chain = FullChainRobotParams(yaml.load(config_str))
00144 full_chain.update_config(self.robot_params)
00145 cmds = self.loadCommands('head_commands')
00146 self.run_test(full_chain, 'torso_lift_link', 'double_stereo_link', cmds)
00147
00148 def test_wide_stereo(self):
00149 print ""
00150 config_str = '''
00151 before_chain: [head_pan_joint]
00152 chain_id: head_chain
00153 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, wide_stereo_frame_joint]
00154 dh_link_num: 1 '''
00155 full_chain = FullChainRobotParams(yaml.load(config_str))
00156 full_chain.update_config(self.robot_params)
00157 cmds = self.loadCommands('head_commands')
00158 self.run_test(full_chain, 'torso_lift_link', 'wide_stereo_link', cmds)
00159
00160 def test_wide_stereo_optical(self):
00161 print ""
00162 config_str = '''
00163 before_chain: [head_pan_joint]
00164 chain_id: head_chain
00165 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, wide_stereo_frame_joint, wide_stereo_optical_frame_joint]
00166 dh_link_num: 1 '''
00167 full_chain = FullChainRobotParams(yaml.load(config_str))
00168 full_chain.update_config(self.robot_params)
00169 cmds = self.loadCommands('head_commands')
00170 self.run_test(full_chain, 'torso_lift_link', 'wide_stereo_optical_frame', cmds)
00171
00172 def test_narrow_stereo(self):
00173 print ""
00174 config_str = '''
00175 before_chain: [head_pan_joint]
00176 chain_id: head_chain
00177 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, narrow_stereo_frame_joint]
00178 dh_link_num: 1 '''
00179 full_chain = FullChainRobotParams(yaml.load(config_str))
00180 full_chain.update_config(self.robot_params)
00181 cmds = self.loadCommands('head_commands')
00182 self.run_test(full_chain, 'torso_lift_link', 'narrow_stereo_link', cmds)
00183
00184 def test_narrow_stereo_optical(self):
00185 print ""
00186 config_str = '''
00187 before_chain: [head_pan_joint]
00188 chain_id: head_chain
00189 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, narrow_stereo_frame_joint, narrow_stereo_optical_frame_joint]
00190 dh_link_num: 1 '''
00191 full_chain = FullChainRobotParams(yaml.load(config_str))
00192 full_chain.update_config(self.robot_params)
00193 cmds = self.loadCommands('head_commands')
00194 self.run_test(full_chain, 'torso_lift_link', 'narrow_stereo_optical_frame', cmds)
00195
00196 def test_high_def(self):
00197 print ""
00198 config_str = '''
00199 before_chain: [head_pan_joint]
00200 chain_id: head_chain
00201 after_chain: [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint]
00202 dh_link_num: 1 '''
00203 full_chain = FullChainRobotParams(yaml.load(config_str))
00204 full_chain.update_config(self.robot_params)
00205 cmds = self.loadCommands('head_commands')
00206 self.run_test(full_chain, 'torso_lift_link', 'high_def_frame', cmds)
00207
00208 def test_high_def_optical(self):
00209 print ""
00210 config_str = '''
00211 before_chain: [head_pan_joint]
00212 chain_id: head_chain
00213 after_chain: [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint, high_def_optical_frame_joint]
00214 dh_link_num: 1 '''
00215 full_chain = FullChainRobotParams(yaml.load(config_str))
00216 full_chain.update_config(self.robot_params)
00217 cmds = self.loadCommands('head_commands')
00218 self.run_test(full_chain, 'torso_lift_link', 'high_def_optical_frame', cmds)
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266 def check_tilt_laser(self, cmd):
00267 actual_T = self.robot_params.tilting_lasers["tilt_laser"].compute_pose([cmd])
00268 expected_T = self.getExpected("torso_lift_link", "laser_tilt_link", [cmd])
00269
00270 print "Expected_T:"
00271 print expected_T
00272 print "Actual T:"
00273 print actual_T
00274
00275 self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
00276
00277 def test_tilt_laser(self):
00278 print ""
00279 self.check_tilt_laser(0)
00280 self.check_tilt_laser(1)
00281
00282 if __name__ == '__main__':
00283 import rostest
00284 rospy.init_node("fk_test")
00285 rostest.unitrun('pr2_calibration_estimation', 'test_PR2Fk', TestPR2Fk)