pr2_head_fk_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('pr2_calibration_estimation')
36 
37 import sys
38 import unittest
39 import rospy
40 import time
41 
42 from pr2_calibration_estimation.full_chain import FullChainRobotParams
43 from pr2_calibration_estimation.robot_params import RobotParams
44 from sensor_msgs.msg import JointState
45 
46 import yaml
47 from pr2_calibration_estimation.srv import FkTest
48 from numpy import *
49 import numpy
50 
51 class LoadData(unittest.TestCase):
52  def setUp(self):
53 
54 
55 
56  #config_filename = "config/system.yaml"
57  #f = open(config_filename)
58  f = rospy.get_param("system")
59  all_config = yaml.load(f)
60 
61  self.robot_params = RobotParams()
62  self.robot_params.configure(all_config)
63 
64 
65  rospy.wait_for_service('fk', 3.0)
66  self._fk_ref = rospy.ServiceProxy('fk', FkTest)
67  #f.close()
68 
69  def loadCommands(self, param_name):
70  #f = open(filename)
71  #cmds = [ [ float(y) for y in x.split()] for x in f.readlines()]
72  #f.close()
73 
74  command_str = rospy.get_param(param_name)
75 
76 
77  cmds = [ [float(y) for y in x.split()] for x in command_str.strip().split('\n')]
78 
79  return cmds
80 
81  def getExpected(self, root, tip, cmd):
82  resp = self._fk_ref(root, tip, cmd)
83  #print resp
84  T = matrix(zeros((4,4)), float)
85  T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
86  T[3,3] = 1.0;
87  T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
88  return T
89 
91 
92  def run_test(self, full_chain, root, tip, cmds):
93  for cmd in cmds:
94  print("On Command: %s" % cmd)
95 
96  expected_T = self.getExpected(root, tip, cmd)
97  chain_state = JointState(position=cmd)
98  actual_T = full_chain.calc_block.fk(chain_state)
99 
100  print("Expected_T:")
101  print(expected_T)
102  print("Actual T:")
103  print(actual_T)
104 
105  self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
106 
108  print("")
109 
110  config_str = '''
111  before_chain: [head_pan_joint]
112  chain_id: head_chain
113  after_chain: [head_chain_tip_adj]
114  dh_link_num: 1
115  '''
116 
117  full_chain = FullChainRobotParams(yaml.load(config_str))
118  full_chain.update_config(self.robot_params)
119 
120  cmds = self.loadCommands('head_commands')
121 
122  self.run_test(full_chain, 'torso_lift_link', 'head_tilt_link', cmds)
123 
124  def test_head_plate(self):
125  print("")
126  config_str = '''
127  before_chain: [head_pan_joint]
128  chain_id: head_chain
129  after_chain: [head_chain_tip_adj, head_plate_frame_joint]
130  dh_link_num: 1'''
131  full_chain = FullChainRobotParams(yaml.load(config_str))
132  full_chain.update_config(self.robot_params)
133  cmds = self.loadCommands('head_commands')
134  self.run_test(full_chain, 'torso_lift_link', 'head_plate_frame', cmds)
135 
137  print("")
138  config_str = '''
139  before_chain: [head_pan_joint]
140  chain_id: head_chain
141  after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint]
142  dh_link_num: 1 '''
143  full_chain = FullChainRobotParams(yaml.load(config_str))
144  full_chain.update_config(self.robot_params)
145  cmds = self.loadCommands('head_commands')
146  self.run_test(full_chain, 'torso_lift_link', 'double_stereo_link', cmds)
147 
148  def test_wide_stereo(self):
149  print("")
150  config_str = '''
151  before_chain: [head_pan_joint]
152  chain_id: head_chain
153  after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, wide_stereo_frame_joint]
154  dh_link_num: 1 '''
155  full_chain = FullChainRobotParams(yaml.load(config_str))
156  full_chain.update_config(self.robot_params)
157  cmds = self.loadCommands('head_commands')
158  self.run_test(full_chain, 'torso_lift_link', 'wide_stereo_link', cmds)
159 
161  print("")
162  config_str = '''
163  before_chain: [head_pan_joint]
164  chain_id: head_chain
165  after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, wide_stereo_frame_joint, wide_stereo_optical_frame_joint]
166  dh_link_num: 1 '''
167  full_chain = FullChainRobotParams(yaml.load(config_str))
168  full_chain.update_config(self.robot_params)
169  cmds = self.loadCommands('head_commands')
170  self.run_test(full_chain, 'torso_lift_link', 'wide_stereo_optical_frame', cmds)
171 
173  print("")
174  config_str = '''
175  before_chain: [head_pan_joint]
176  chain_id: head_chain
177  after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, narrow_stereo_frame_joint]
178  dh_link_num: 1 '''
179  full_chain = FullChainRobotParams(yaml.load(config_str))
180  full_chain.update_config(self.robot_params)
181  cmds = self.loadCommands('head_commands')
182  self.run_test(full_chain, 'torso_lift_link', 'narrow_stereo_link', cmds)
183 
185  print("")
186  config_str = '''
187  before_chain: [head_pan_joint]
188  chain_id: head_chain
189  after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, narrow_stereo_frame_joint, narrow_stereo_optical_frame_joint]
190  dh_link_num: 1 '''
191  full_chain = FullChainRobotParams(yaml.load(config_str))
192  full_chain.update_config(self.robot_params)
193  cmds = self.loadCommands('head_commands')
194  self.run_test(full_chain, 'torso_lift_link', 'narrow_stereo_optical_frame', cmds)
195 
196  def test_high_def(self):
197  print("")
198  config_str = '''
199  before_chain: [head_pan_joint]
200  chain_id: head_chain
201  after_chain: [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint]
202  dh_link_num: 1 '''
203  full_chain = FullChainRobotParams(yaml.load(config_str))
204  full_chain.update_config(self.robot_params)
205  cmds = self.loadCommands('head_commands')
206  self.run_test(full_chain, 'torso_lift_link', 'high_def_frame', cmds)
207 
209  print("")
210  config_str = '''
211  before_chain: [head_pan_joint]
212  chain_id: head_chain
213  after_chain: [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint, high_def_optical_frame_joint]
214  dh_link_num: 1 '''
215  full_chain = FullChainRobotParams(yaml.load(config_str))
216  full_chain.update_config(self.robot_params)
217  cmds = self.loadCommands('head_commands')
218  self.run_test(full_chain, 'torso_lift_link', 'high_def_optical_frame', cmds)
219 
220 # def test_kinect_head_def(self):
221 # print ""
222 # config_str = '''
223 # before_chain: [head_pan_joint]
224 # chain_id: head_chain
225 # after_chain: [head_chain_tip_adj, head_plate_frame_joint, kinect_head_rgb_frame_joint]
226 # dh_link_num: 1 '''
227 # full_chain = FullChainRobotParams(yaml.load(config_str))
228 # full_chain.update_config(self.robot_params)
229 # cmds = self.loadCommands('head_commands')
230 # self.run_test(full_chain, 'torso_lift_link', 'kinect_head_rgb_frame', cmds)
231 #
232 # def test_kinect_head_optical(self):
233 # print ""
234 # config_str = '''
235 # before_chain: [head_pan_joint]
236 # chain_id: head_chain
237 # after_chain: [head_chain_tip_adj, head_plate_frame_joint, kinect_head_rgb_frame_joint, kinect_head_rgb_optical_frame_joint]
238 # dh_link_num: 1 '''
239 # full_chain = FullChainRobotParams(yaml.load(config_str))
240 # full_chain.update_config(self.robot_params)
241 # cmds = self.loadCommands('head_commands')
242 # self.run_test(full_chain, 'torso_lift_link', 'kinect_head_rgb_optical_frame', cmds)
243 #
244 # def test_kinect_torso_def(self):
245 # print ""
246 # config_str = '''
247 # before_chain: [kinect_torso_rgb_frame_joint, kinect_torso_rgb_optical_frame_joint]
248 # chain_id: NULL
249 # after_chain: [] '''
250 # full_chain = FullChainRobotParams(yaml.load(config_str))
251 # full_chain.update_config(self.robot_params)
252 # cmds = self.loadCommands('head_commands')
253 # self.run_test(full_chain, 'torso_lift_link', 'kinect_torso_rgb_frame', cmds)
254 #
255 # def test_kinect_torso_optical(self):
256 # print ""
257 # config_str = '''
258 # before_chain: [kinect_torso_rgb_frame_joint, kinect_torso_rgb_optical_frame_joint]
259 # chain_id: NULL
260 # after_chain: [] '''
261 # full_chain = FullChainRobotParams(yaml.load(config_str))
262 # full_chain.update_config(self.robot_params)
263 # cmds = self.loadCommands('head_commands')
264 # self.run_test(full_chain, 'torso_lift_link', 'kinect_torso_rgb_optical_frame', cmds)
265 
266  def check_tilt_laser(self, cmd):
267  actual_T = self.robot_params.tilting_lasers["tilt_laser"].compute_pose([cmd])
268  expected_T = self.getExpected("torso_lift_link", "laser_tilt_link", [cmd])
269 
270  print("Expected_T:")
271  print(expected_T)
272  print("Actual T:")
273  print(actual_T)
274 
275  self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
276 
277  def test_tilt_laser(self):
278  print("")
279  self.check_tilt_laser(0)
280  self.check_tilt_laser(1)
281 
282 if __name__ == '__main__':
283  import rostest
284  rospy.init_node("fk_test")
285  rostest.unitrun('pr2_calibration_estimation', 'test_PR2Fk', TestPR2Fk)
def loadCommands(self, param_name)
def getExpected(self, root, tip, cmd)
def run_test(self, full_chain, root, tip, cmds)


pr2_calibration_launch
Author(s): Vijay Pradeep
autogenerated on Tue Jun 1 2021 02:50:59