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

sr_utilities
Author(s): Ugo Cupcic
autogenerated on Tue Oct 13 2020 03:55:49