test_hand_finder.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright 2011 Shadow Robot Company Ltd.
4 #
5 # This program is free software: you can redistribute it and/or modify it
6 # under the terms of the GNU General Public License as published by the Free
7 # Software Foundation version 2 of the License.
8 #
9 # This program is distributed in the hope that it will be useful, but WITHOUT
10 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11 # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
12 # more details.
13 #
14 # You should have received a copy of the GNU General Public License along
15 # with this program. If not, see <http://www.gnu.org/licenses/>.
16 
17 import rospy
18 import rospkg
19 import rostest
20 import unittest
21 from sr_utilities.hand_finder import HandFinder
22 joint_names = ["FFJ1", "FFJ2", "FFJ3", "FFJ4", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
23  "RFJ1", "RFJ2", "RFJ3", "RFJ4", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
24  "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"]
25 controller_params = ["sr_edc_calibration_controllers.yaml",
26  "sr_edc_joint_velocity_controllers_PWM.yaml",
27  "sr_edc_effort_controllers_PWM.yaml",
28  "sr_edc_joint_velocity_controllers.yaml",
29  "sr_edc_effort_controllers.yaml",
30  "sr_edc_mixed_position_velocity_joint_controllers_PWM.yaml",
31  "sr_edc_joint_position_controllers_PWM.yaml",
32  "sr_edc_mixed_position_velocity_joint_controllers.yaml",
33  "sr_edc_joint_position_controllers.yaml"]
34 
35 
36 class TestHandFinder(unittest.TestCase):
37  rospack = rospkg.RosPack()
38  ethercat_path = rospack.get_path('sr_ethercat_hand_config')
39 
40  def test_no_hand_no_robot_description_finder(self):
41  if rospy.has_param("/hand"):
42  rospy.delete_param("/hand")
43 
44  if rospy.has_param("robot_description"):
45  rospy.delete_param("robot_description")
46 
47  hand_finder = HandFinder(1.0)
48 
49  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 0, "correct parameters without a hand")
50  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0, "correct parameters without a hand")
51  self.assertEqual(len(hand_finder.get_hand_joints()), 0, "correct joints without a hand")
52  self.assertEqual(len(hand_finder.get_calibration_path()), 0, "correct calibration path without a hand")
53  self.assertEqual(len(hand_finder.get_hand_control_tuning(). friction_compensation), 0,
54  "correct tuning without a hands")
55  self.assertEqual(len(hand_finder.get_hand_control_tuning(). host_control), 0, "correct tuning without a hands")
56  self.assertEqual(len(hand_finder.get_hand_control_tuning(). motor_control), 0,
57  "correct tuning without a hands")
58 
59  def test_one_hand_no_robot_description_finder(self):
60  if rospy.has_param("/hand"):
61  rospy.delete_param("/hand")
62 
63  if rospy.has_param("robot_description"):
64  rospy.delete_param("robot_description")
65 
66  rospy.set_param("/hand/joint_prefix/1", "rh_")
67  rospy.set_param("/hand/mapping/1", "right")
68 
69  hand_finder = HandFinder(1.0)
70  # hand params
71  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
72  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
73  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix")
74  self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "right", "It should be only right mapping")
75  self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "rh_", "It should be only rh_ prefix")
76  # hand joints
77  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
78  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1, "It should be only one mapping")
79  self.assertIn("right", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
80  joints = hand_finder.get_hand_joints()['right']
81  self.assertEqual(len(joints), 24, "Joint number should be 24")
82  self.assertIn("rh_FFJ3", hand_finder.get_hand_joints()["right"], "FFJ3 joint should be in the joints list")
83  # calibration
84  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
85  calibration_path = hand_finder.get_calibration_path()['right']
86  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/right/" + "calibration.yaml",
87  "incorrect calibration file")
88  # tuning
89  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
90  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['right']
91  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
92  "incorrect friction compensation file")
93  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['right']
94  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
95  "/controls/motors/right/motor_board_effort_controllers.yaml",
96  "incorrect motor config file")
97  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['right']
98  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
99  "incorrect number of host controllers param")
100  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
101  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/right/" + controller_param,
102  "incorrect controller config file")
103 
104  def test_one_hand_no_mapping_no_robot_description_finder(self):
105  if rospy.has_param("/hand"):
106  rospy.delete_param("/hand")
107 
108  if rospy.has_param("robot_description"):
109  rospy.delete_param("robot_description")
110 
111  rospy.set_param("/hand/joint_prefix/1", "rh_")
112  rospy.set_param("/hand/mapping/1", "")
113 
114  hand_finder = HandFinder(1.0)
115  # hand params
116  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
117  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
118  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix")
119  self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1", "It should be the serial id as mapping")
120  self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "rh_", "It should be only rh_ prefix")
121  # hand joints
122  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
123  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1, "It should be only one mapping")
124  print hand_finder.get_hand_joints()
125  self.assertIn("1", hand_finder.get_hand_joints(), "Serial should be in the joints result")
126  joints = hand_finder.get_hand_joints()['1'] # use serial
127  self.assertEqual(len(joints), 24, "Joint number should be 24")
128  self.assertIn("rh_FFJ3", hand_finder.get_hand_joints()["1"], "FFJ3 joint should be in the joints list")
129  # calibration
130  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
131  calibration_path = hand_finder.get_calibration_path()['1']
132  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/1/" + "calibration.yaml",
133  "incorrect calibration file")
134  # tuning
135  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
136  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['1']
137  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
138  "incorrect friction compensation file")
139  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['1']
140  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
141  "/controls/motors/1/motor_board_effort_controllers.yaml",
142  "incorrect motor config file")
143  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['1']
144  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
145  "incorrect number of host controllers param")
146  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
147  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/1/" + controller_param,
148  "incorrect controller config file")
149 
150  def test_one_hand_no_prefix_no_robot_description_finder(self):
151  if rospy.has_param("/hand"):
152  rospy.delete_param("/hand")
153 
154  if rospy.has_param("robot_description"):
155  rospy.delete_param("robot_description")
156 
157  rospy.set_param("/hand/joint_prefix/1", "")
158  rospy.set_param("/hand/mapping/1", "rh")
159 
160  hand_finder = HandFinder(1.0)
161  # hand params
162  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
163  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
164  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix")
165  self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "rh", "It should be only right mapping")
166  self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "", "It should be only an empty prefix")
167  # hand joints
168  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
169  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1, "It should be only one mapping")
170  self.assertIn("rh", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
171  joints = hand_finder.get_hand_joints()['rh']
172  self.assertEqual(len(joints), 24, "Joint number should be 24")
173  self.assertIn("FFJ3", hand_finder.get_hand_joints()["rh"], "FFJ3 joint should be in the joints list")
174  # calibration
175  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
176  calibration_path = hand_finder.get_calibration_path()['rh']
177  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/rh/" + "calibration.yaml",
178  "incorrect calibration file")
179  # tuning
180  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
181  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['rh']
182  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
183  "incorrect friction compensation file")
184  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['rh']
185  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
186  "/controls/motors/rh/motor_board_effort_controllers.yaml",
187  "incorrect motor config file")
188  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['rh']
189  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
190  "incorrect number of host controllers param")
191  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
192  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/rh/" + controller_param,
193  "incorrect controller config file")
194 
195  def test_one_hand_no_prefix_no_mapping_no_robot_description_finder(self):
196  if rospy.has_param("/hand"):
197  rospy.delete_param("/hand")
198 
199  if rospy.has_param("robot_description"):
200  rospy.delete_param("robot_description")
201 
202  rospy.set_param("/hand/joint_prefix/1", "")
203  rospy.set_param("/hand/mapping/1", "")
204 
205  hand_finder = HandFinder(1.0)
206  # hand params
207  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
208  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
209  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix")
210  self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1", "It should be the serial id as mapping")
211  self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "", "It should be only an empty prefix")
212  # hand joints
213  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
214  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1, "It should be only one mapping")
215  self.assertIn("1", hand_finder.get_hand_joints(), "Serial should be in the joints result")
216  joints = hand_finder.get_hand_joints()['1']
217  self.assertEqual(len(joints), 24, "Joint number should be 24")
218  self.assertIn("FFJ3", hand_finder.get_hand_joints()["1"], "FFJ3 joint should be in the joints list")
219  # calibration
220  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
221  calibration_path = hand_finder.get_calibration_path()['1']
222  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/1/" + "calibration.yaml",
223  "incorrect calibration file")
224  # tuning
225  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
226  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['1']
227  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
228  "incorrect friction compensation file")
229  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['1']
230  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
231  "/controls/motors/1/motor_board_effort_controllers.yaml",
232  "incorrect motor config file")
233  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['1']
234  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
235  "incorrect number of host controllers param")
236  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
237  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/1/" + controller_param,
238  "incorrect controller config file")
239 
240  def test_two_hand_no_robot_description_finder(self):
241  if rospy.has_param("/hand"):
242  rospy.delete_param("/hand")
243 
244  if rospy.has_param("robot_description"):
245  rospy.delete_param("robot_description")
246 
247  rospy.set_param("/hand/joint_prefix/1", "rh_")
248  rospy.set_param("/hand/mapping/1", "right")
249  rospy.set_param("/hand/joint_prefix/2", "lh_")
250  rospy.set_param("/hand/mapping/2", "left")
251 
252  hand_finder = HandFinder(1.0)
253 
254  # hand params
255  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
256  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 2, "It should be two mappings")
257  self.assertIn("right", hand_finder.get_hand_parameters().mapping.values(), "It should be right mapping")
258  self.assertIn("left", hand_finder.get_hand_parameters().mapping.values(), "It should be left mapping")
259  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 2, "It should be two joint_prefixes")
260 
261  self.assertIn("rh_", hand_finder.get_hand_parameters().joint_prefix.values(), "It should be rh_ prefix")
262  self.assertIn("lh_", hand_finder.get_hand_parameters().joint_prefix.values(), "It should be rh_ prefix")
263 
264  # hand joints
265  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
266  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 2, "It should be two mappings")
267  self.assertIn("right", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
268  joints = hand_finder.get_hand_joints()['right']
269  self.assertEqual(len(joints), 24, "Joint number should be 24")
270  self.assertIn("rh_FFJ3", hand_finder.get_hand_joints()["right"], "FFJ3 joint should be in the joints list")
271  self.assertIn("left", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
272  joints = hand_finder.get_hand_joints()['left']
273  self.assertEqual(len(joints), 24, "Joint number should be 24")
274  self.assertIn("lh_FFJ1", hand_finder.get_hand_joints()["left"], "FFJ1 joint should be in the joints list")
275 
276  # calibration
277  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
278  calibration_path = hand_finder.get_calibration_path()['right']
279  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/right/" + "calibration.yaml",
280  "incorrect calibration file")
281  calibration_path = hand_finder.get_calibration_path()['left']
282  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/left/" + "calibration.yaml",
283  "incorrect calibration file")
284  # tuning
285  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
286  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['right']
287  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
288  "incorrect friction compensation file")
289  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['right']
290  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
291  "/controls/motors/right/motor_board_effort_controllers.yaml",
292  "incorrect motor config file")
293  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['right']
294  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
295  "incorrect number of host controllers param")
296  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
297  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/right/" + controller_param,
298  "incorrect controller config file")
299 
300  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['left']
301  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
302  "incorrect friction compensation file")
303  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['left']
304  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
305  "/controls/motors/left/motor_board_effort_controllers.yaml",
306  "incorrect motor config file")
307  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['left']
308  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
309  "incorrect number of host controllers param")
310  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
311  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/left/" + controller_param,
312  "incorrect controller config file")
313 
314  def test_no_hand_robot_description_exists_finder(self):
315  if rospy.has_param("/hand"):
316  rospy.delete_param("/hand")
317 
318  if rospy.has_param("robot_description"):
319  rospy.delete_param("robot_description")
320 
321  rospy.set_param("robot_description", rospy.get_param("/two_hands_description"))
322 
323  hand_finder = HandFinder(1.0)
324 
325  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 0, "correct parameters without a hand")
326  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0, "correct parameters without a hand")
327  self.assertEqual(len(hand_finder.get_hand_joints()), 0, "correct joints without a hand")
328  self.assertEqual(len(hand_finder.get_calibration_path()), 0, "correct calibration path without a hand")
329  self.assertEqual(len(hand_finder.get_hand_control_tuning(). friction_compensation), 0,
330  "correct tuning without a hands")
331  self.assertEqual(len(hand_finder.get_hand_control_tuning(). host_control), 0, "correct tuning without a hands")
332  self.assertEqual(len(hand_finder.get_hand_control_tuning(). motor_control), 0,
333  "correct tuning without a hands")
334 
335  def test_one_hand_robot_description_exists_finder(self):
336  if rospy.has_param("/hand"):
337  rospy.delete_param("/hand")
338 
339  if rospy.has_param("robot_description"):
340  rospy.delete_param("robot_description")
341 
342  rospy.set_param("/hand/joint_prefix/1", "rh_")
343  rospy.set_param("/hand/mapping/1", "right")
344  rospy.set_param("robot_description", rospy.get_param("/right_hand_description"))
345 
346  hand_finder = HandFinder(1.0)
347  # hand params
348  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
349  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
350  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix")
351  self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "right", "It should be only right mapping")
352  self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "rh_", "It should be only rh_ prefix")
353  # hand joints
354  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
355  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1, "It should be only one mapping")
356  self.assertIn("right", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
357  joints = hand_finder.get_hand_joints()['right']
358  self.assertEqual(len(joints), 1, "Joint number should be 1")
359  self.assertNotIn("rh_FFJ3", hand_finder.get_hand_joints()["right"],
360  "FFJ3 joint should not be in the joints list")
361  self.assertIn("rh_RFJ4", hand_finder.get_hand_joints()["right"], "RFJ4 joint should be in the joints list")
362  # calibration
363  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
364  calibration_path = hand_finder.get_calibration_path()['right']
365  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/right/" + "calibration.yaml",
366  "incorrect calibration file")
367  # tuning
368  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
369  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['right']
370  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
371  "incorrect friction compensation file")
372  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['right']
373  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
374  "/controls/motors/right/motor_board_effort_controllers.yaml",
375  "incorrect motor config file")
376  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['right']
377  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
378  "incorrect number of host controllers param")
379  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
380  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/right/" + controller_param,
381  "incorrect controller config file")
382 
383  def test_one_hand_no_mapping_robot_description_exists_finder(self):
384  if rospy.has_param("/hand"):
385  rospy.delete_param("/hand")
386 
387  if rospy.has_param("robot_description"):
388  rospy.delete_param("robot_description")
389 
390  rospy.set_param("/hand/joint_prefix/1", "rh_")
391  rospy.set_param("/hand/mapping/1", "")
392  rospy.set_param("robot_description", rospy.get_param("/right_hand_description"))
393 
394  hand_finder = HandFinder(1.0)
395  # hand params
396  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
397  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
398  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix")
399  self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1", "It should be the serial id as mapping")
400  self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "rh_", "It should be only rh_ prefix")
401  # hand joints
402  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
403  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1, "It should be only one mapping")
404  self.assertIn("1", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
405  joints = hand_finder.get_hand_joints()['1']
406  self.assertEqual(len(joints), 1, "Joint number should be 1")
407  self.assertNotIn("rh_FFJ3", hand_finder.get_hand_joints()["1"], "FFJ3 joint should not be in the joints list")
408  self.assertIn("rh_RFJ4", hand_finder.get_hand_joints()["1"], "RFJ4 joint should be in the joints list")
409  # calibration
410  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
411  calibration_path = hand_finder.get_calibration_path()['1']
412  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/1/" + "calibration.yaml",
413  "incorrect calibration file")
414  # tuning
415  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
416  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['1']
417  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
418  "incorrect friction compensation file")
419  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['1']
420  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
421  "/controls/motors/1/motor_board_effort_controllers.yaml",
422  "incorrect motor config file")
423  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['1']
424  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
425  "incorrect number of host controllers param")
426  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
427  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/1/" + controller_param,
428  "incorrect controller config file")
429 
430  def test_one_hand_no_prefix_robot_description_exists_finder(self):
431  if rospy.has_param("/hand"):
432  rospy.delete_param("/hand")
433 
434  if rospy.has_param("robot_description"):
435  rospy.delete_param("robot_description")
436 
437  rospy.set_param("/hand/joint_prefix/1", "")
438  rospy.set_param("/hand/mapping/1", "rh")
439  rospy.set_param("robot_description", rospy.get_param("/right_hand_description_no_prefix"))
440 
441  hand_finder = HandFinder(1.0)
442  # hand params
443  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
444  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
445  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix")
446  self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "rh", "It should be only right mapping")
447  self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "", "It should be only an empty prefix")
448  # hand joints
449  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
450  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1, "It should be only one mapping")
451  self.assertIn("rh", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
452  joints = hand_finder.get_hand_joints()['rh']
453  self.assertEqual(len(joints), 1, "Joint number should be 1")
454  self.assertNotIn("FFJ3", hand_finder.get_hand_joints()["rh"], "FFJ3 joint should not be in the joints list")
455  self.assertIn("RFJ4", hand_finder.get_hand_joints()["rh"], "RFJ4 joint should be in the joints list")
456  # calibration
457  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
458  calibration_path = hand_finder.get_calibration_path()['rh']
459  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/rh/" + "calibration.yaml",
460  "incorrect calibration file")
461  # tuning
462  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
463  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['rh']
464  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
465  "incorrect friction compensation file")
466  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['rh']
467  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
468  "/controls/motors/rh/motor_board_effort_controllers.yaml",
469  "incorrect motor config file")
470  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['rh']
471  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
472  "incorrect number of host controllers param")
473  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
474  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/rh/" + controller_param,
475  "incorrect controller config file")
476 
477  def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self):
478  if rospy.has_param("/hand"):
479  rospy.delete_param("/hand")
480 
481  if rospy.has_param("robot_description"):
482  rospy.delete_param("robot_description")
483 
484  rospy.set_param("/hand/joint_prefix/1", "")
485  rospy.set_param("/hand/mapping/1", "")
486  rospy.set_param("robot_description", rospy.get_param("/right_hand_description_no_prefix"))
487 
488  hand_finder = HandFinder(1.0)
489  # hand params
490  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
491  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
492  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix")
493  self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1", "It should be the serial id as mapping")
494  self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "", "It should be only an empty prefix")
495  # hand joints
496  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
497  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1, "It should be only one mapping")
498  self.assertIn("1", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
499  joints = hand_finder.get_hand_joints()['1']
500  self.assertEqual(len(joints), 1, "Joint number should be 1")
501  self.assertNotIn("FFJ3", hand_finder.get_hand_joints()["1"], "FFJ3 joint should not be in the joints list")
502  self.assertIn("RFJ4", hand_finder.get_hand_joints()["1"], "RFJ4 joint should be in the joints list")
503  # calibration
504  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
505  calibration_path = hand_finder.get_calibration_path()['1']
506  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/1/" + "calibration.yaml",
507  "incorrect calibration file")
508  # tuning
509  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
510  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['1']
511  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
512  "incorrect friction compensation file")
513  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['1']
514  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
515  "/controls/motors/1/motor_board_effort_controllers.yaml",
516  "incorrect motor config file")
517  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['1']
518  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
519  "incorrect number of host controllers param")
520  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
521  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/1/" + controller_param,
522  "incorrect controller config file")
523 
524  def test_two_hand_robot_description_exists_finder(self):
525  if rospy.has_param("/hand"):
526  rospy.delete_param("/hand")
527 
528  if rospy.has_param("robot_description"):
529  rospy.delete_param("robot_description")
530 
531  rospy.set_param("/hand/joint_prefix/1", "rh_")
532  rospy.set_param("/hand/mapping/1", "right")
533  rospy.set_param("/hand/joint_prefix/2", "lh_")
534  rospy.set_param("/hand/mapping/2", "left")
535  rospy.set_param("robot_description", rospy.get_param("/two_hands_description"))
536 
537  hand_finder = HandFinder(1.0)
538  # hand params
539  self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
540  self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 2, "It should be two mappings")
541  self.assertIn("right", hand_finder.get_hand_parameters().mapping.values(), "It should be right mapping")
542  self.assertIn("left", hand_finder.get_hand_parameters().mapping.values(), "It should be left mapping")
543  self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 2, "It should be two joint_prefixes")
544 
545  self.assertIn("rh_", hand_finder.get_hand_parameters().joint_prefix.values(), "It should be rh_ prefix")
546  self.assertIn("lh_", hand_finder.get_hand_parameters().joint_prefix.values(), "It should be rh_ prefix")
547 
548  # hand joints
549  self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.")
550  self.assertEqual(len(hand_finder.get_hand_joints().keys()), 2, "It should be two mappings")
551  self.assertIn("right", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
552  joints = hand_finder.get_hand_joints()['right']
553  self.assertEqual(len(joints), 1, "Joint number should be 1")
554  self.assertNotIn("rh_FFJ3", hand_finder.get_hand_joints()["right"],
555  "rh_FFJ3 joint should not be in the joints list")
556  self.assertIn("rh_RFJ4", hand_finder.get_hand_joints()["right"], "rh_RFJ4 joint should be in the joints list")
557  self.assertIn("left", hand_finder.get_hand_joints(), "Mapping should be in the joints result")
558  joints = hand_finder.get_hand_joints()['left']
559  self.assertEqual(len(joints), 1, "Joint number should be 1")
560  self.assertNotIn("lh_FFJ3", hand_finder.get_hand_joints()["left"],
561  "lh_FFJ3 joint should not be in the joints list")
562  self.assertIn("lh_LFJ4", hand_finder.get_hand_joints()["left"], "lh_LFJ4 joint should be in the joints list")
563 
564  # calibration
565  self.assertIsNotNone(hand_finder.get_calibration_path(), "Calibration extracted.")
566  calibration_path = hand_finder.get_calibration_path()['right']
567  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/right/" + "calibration.yaml",
568  "incorrect calibration file")
569  calibration_path = hand_finder.get_calibration_path()['left']
570  self.assertEqual(calibration_path, self.ethercat_path + "/calibrations/left/" + "calibration.yaml",
571  "incorrect calibration file")
572  # tuning
573  self.assertIsNotNone(hand_finder.get_hand_control_tuning(), "Control tuning parameters extracted.")
574  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['right']
575  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
576  "incorrect friction compensation file")
577  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['right']
578  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
579  "/controls/motors/right/motor_board_effort_controllers.yaml",
580  "incorrect motor config file")
581  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['right']
582  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
583  "incorrect number of host controllers param")
584  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
585  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/right/" + controller_param,
586  "incorrect controller config file")
587 
588  ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning().friction_compensation['left']
589  self.assertEqual(ctrl_tun_friction_comp_path, self.ethercat_path + "/controls/friction_compensation.yaml",
590  "incorrect friction compensation file")
591  ctrl_tun_motors_path = hand_finder.get_hand_control_tuning().motor_control['left']
592  self.assertEqual(ctrl_tun_motors_path, self.ethercat_path +
593  "/controls/motors/left/motor_board_effort_controllers.yaml",
594  "incorrect motor config file")
595  ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning().host_control['left']
596  self.assertEqual(len(ctrl_tun_host_control_paths), len(controller_params),
597  "incorrect number of host controllers param")
598  for controller_path, controller_param in zip(ctrl_tun_host_control_paths, controller_params):
599  self.assertEqual(controller_path, self.ethercat_path + "/controls/host/left/" + controller_param,
600  "incorrect controller config file")
601 
602 if __name__ == "__main__":
603  rospy.init_node("test_hand_finder")
604  rostest.rosrun("sr_utilities", "test_hand_finder", TestHandFinder)
test_hand_finder.TestHandFinder.test_one_hand_no_prefix_no_robot_description_finder
def test_one_hand_no_prefix_no_robot_description_finder(self)
Definition: test_hand_finder.py:150
test_hand_finder.TestHandFinder.test_one_hand_no_prefix_robot_description_exists_finder
def test_one_hand_no_prefix_robot_description_exists_finder(self)
Definition: test_hand_finder.py:430
test_hand_finder.TestHandFinder.test_no_hand_no_robot_description_finder
def test_no_hand_no_robot_description_finder(self)
Definition: test_hand_finder.py:40
test_hand_finder.TestHandFinder.test_no_hand_robot_description_exists_finder
def test_no_hand_robot_description_exists_finder(self)
Definition: test_hand_finder.py:314
test_hand_finder.TestHandFinder.test_one_hand_no_prefix_no_ns_robot_description_exists_finder
def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self)
Definition: test_hand_finder.py:477
test_hand_finder.TestHandFinder
Definition: test_hand_finder.py:36
test_hand_finder.TestHandFinder.test_one_hand_no_mapping_no_robot_description_finder
def test_one_hand_no_mapping_no_robot_description_finder(self)
Definition: test_hand_finder.py:104
test_hand_finder.TestHandFinder.ethercat_path
ethercat_path
Definition: test_hand_finder.py:38
test_hand_finder.TestHandFinder.test_two_hand_no_robot_description_finder
def test_two_hand_no_robot_description_finder(self)
Definition: test_hand_finder.py:240
test_hand_finder.TestHandFinder.test_one_hand_robot_description_exists_finder
def test_one_hand_robot_description_exists_finder(self)
Definition: test_hand_finder.py:335
test_hand_finder.TestHandFinder.test_one_hand_no_prefix_no_mapping_no_robot_description_finder
def test_one_hand_no_prefix_no_mapping_no_robot_description_finder(self)
Definition: test_hand_finder.py:195
test_hand_finder.TestHandFinder.test_two_hand_robot_description_exists_finder
def test_two_hand_robot_description_exists_finder(self)
Definition: test_hand_finder.py:524
test_hand_finder.TestHandFinder.test_one_hand_no_robot_description_finder
def test_one_hand_no_robot_description_finder(self)
Definition: test_hand_finder.py:59
test_hand_finder.TestHandFinder.test_one_hand_no_mapping_robot_description_exists_finder
def test_one_hand_no_mapping_robot_description_exists_finder(self)
Definition: test_hand_finder.py:383

sr_utilities
Author(s): Ugo Cupcic
autogenerated on Mon Feb 28 2022 23:52:19