sr_hand_calibration_model.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 
18 import rospy
19 
20 from sr_robot_lib.etherCAT_hand_lib import EtherCAT_Hand_Lib
21 from PyQt5.QtGui import QColor, QIcon
22 from PyQt5.QtWidgets import QTreeWidget, QTreeWidgetItem, QTreeWidgetItemIterator, QMessageBox
23 from PyQt5.QtCore import QTimer, pyqtSignal
24 
25 import yaml
26 try:
27  from yaml import CLoader as Loader, CDumper as Dumper
28 except ImportError:
29  from yaml import Loader, Dumper
30 import os
31 
32 from collections import deque
33 
34 green = QColor(153, 231, 96)
35 orange = QColor(247, 206, 134)
36 red = QColor(236, 178, 178)
37 
38 
39 class IndividualCalibration(QTreeWidgetItem):
40 
41  """
42  Calibrate a single joint by raw and calibrated values
43  Calibrated joints will appear as green
44  or orange if calibrations are loaded from a file
45  """
46 
47  def __init__(self, joint_name,
48  raw_value, calibrated_value,
49  parent_widget, tree_widget,
50  robot_lib):
51  self.joint_name = joint_name
52  self.raw_value = int(raw_value)
53  self.calibrated_value = calibrated_value
54  self.tree_widget = tree_widget
55  self.robot_lib = robot_lib
56 
57  QTreeWidgetItem.__init__(self, parent_widget, [
58  "", "", str(self.raw_value), str(self.calibrated_value)])
59 
60  for col in xrange(self.tree_widget.columnCount()):
61  self.setBackground(col, QColor(red))
62 
63  self.tree_widget.addTopLevelItem(self)
64 
65  self.is_calibrated = False
66 
67  def remove(self):
68  self.tree_widget.remove
69 
70  def calibrate(self):
71  """
72  Performs the joint calibration and sets background to green
73  calibrate only the calibration lines, not the items for the fingers / joints / hand
74  """
75  self.raw_value = self.robot_lib.get_average_raw_value(
76  self.joint_name, number_of_samples=100, accept_zeros=False)
77  self.setText(2, str(self.raw_value))
78 
79  for col in xrange(self.tree_widget.columnCount()):
80  if self.text(2) != "":
81  self.setBackground(col, QColor(green))
82 
83  self.is_calibrated = True
84 
86  """
87  set the background to orange: those values are loaded
88  from the file, not recalibrated
89  """
90  for col in xrange(self.tree_widget.columnCount()):
91  if self.text(2) != "":
92  self.setBackground(col, QColor(orange))
93 
94  self.is_calibrated = True
95 
96  def get_calibration(self):
97  return [self.raw_value, self.calibrated_value]
98 
99 
101 
102  """
103  Calibrate coupled joints by raw and calibrated values
104  Calibrated joints will appear as green
105  or orange if calibrations are loaded from a file
106  """
107 
108  def __init__(self, joint_names,
109  raw_values, calibrated_values,
110  parent_widget, tree_widget,
111  robot_lib):
112 
113  self.joint_names = joint_names
114  self.raw_values = [int(raw_value) for raw_value in raw_values]
115  self.calibrated_values = calibrated_values
116  self.tree_widget = tree_widget
117  self.robot_lib = robot_lib
118 
119  QTreeWidgetItem.__init__(self, parent_widget, [
120  "", "", str(self.raw_values[0]) + ", " + str(self.raw_values[1]),
121  str(self.calibrated_values[0]) + ", " + str(self.calibrated_values[1])])
122 
123  for col in xrange(self.tree_widget.columnCount()):
124  self.setBackground(col, QColor(red))
125 
126  self.tree_widget.addTopLevelItem(self)
127 
128  self.is_calibrated = False
129 
130  def calibrate(self):
131  """
132  Performs the joint calibration and sets background to green
133  calibrate only the calibration lines, not the items for the fingers / joints / hand
134  """
135  raw_values_str = []
136  for idx in range(0, 2):
137  self.raw_values[idx] = self.robot_lib.get_average_raw_value(
138  self.joint_names[idx], 100)
139  raw_values_str.append(str(self.raw_values[idx]))
140  self.setText(2, ", ".join(raw_values_str))
141 
142  for col in xrange(self.tree_widget.columnCount()):
143  if self.text(2) != "":
144  self.setBackground(col, QColor(green))
145 
146  self.is_calibrated = True
147 
148  def get_calibration(self):
149  return [self.raw_values, self.calibrated_values]
150 
151 
152 class JointCalibration(QTreeWidgetItem):
153 
154  """
155  Calibrate a single joint by calibrations list
156  Also displays the current joint position in the GUI
157  """
158 
159  nb_values_to_check = 5
160 
161  def __init__(self, joint_name,
162  calibrations,
163  parent_widget, tree_widget,
164  robot_lib):
165  self.joint_name = joint_name
166  self.tree_widget = tree_widget
167  self.robot_lib = robot_lib
168 
169  self.calibrations = []
170  self.last_raw_values = deque()
171 
172  if type(self.joint_name) is not list:
173  QTreeWidgetItem.__init__(
174  self, parent_widget, ["", joint_name, "", ""])
175  for calibration in calibrations:
176  self.calibrations.append(IndividualCalibration(joint_name,
177  calibration[0], calibration[1],
178  self, tree_widget, robot_lib))
179  else:
180  QTreeWidgetItem.__init__(
181  self, parent_widget, ["", joint_name[0] + ", " + joint_name[1], "", ""])
182  for calibration in calibrations:
183  self.calibrations.append(IndividualCalibrationCoupled(joint_name,
184  calibration[0], calibration[1],
185  self, tree_widget, robot_lib))
186 
187  # display the current joint position in the GUI
188  self.timer = QTimer()
189  self.timer.start(200)
190 
191  tree_widget.addTopLevelItem(self)
192  self.timer.timeout.connect(self.update_joint_pos)
193 
194  def load_joint_calibration(self, new_calibrations):
195  for calibration in self.calibrations:
196  self.removeChild(calibration)
197  self.calibrations = []
198 
199  for calibration in new_calibrations:
200  if type(self.joint_name) is not list:
201  new_calib = IndividualCalibration(self.joint_name,
202  calibration[0], calibration[1],
203  self, self.tree_widget, self.robot_lib)
204  else:
205  new_calib = IndividualCalibrationCoupled(self.joint_name,
206  calibration[0], [calibration[1], calibration[2]],
207  self, self.tree_widget, self.robot_lib)
208  new_calib.set_is_loaded_calibration()
209  self.calibrations.append(new_calib)
210 
212  config = []
213  for calibration in self.calibrations:
214  if calibration.is_calibrated:
215  config.append(calibration.get_calibration())
216 
217  if len(config) <= 1:
218  # no config, or only one point
219  # generating flat config
220  if type(self.joint_name) is not list:
221  config = [[0, 0.0], [1, 0.0]]
222  else:
223  config = [[[0, 0], [0.0, 0.0]], [[1, 1], [0.0, 0.0]]]
224  return [self.joint_name, config]
225 
226  def update_joint_pos(self):
227  """
228  Update the joint position if there are enough nonequal values
229  If the values are equal it can be assumed the sensor are not measuring properly
230  """
231  if type(self.joint_name) is not list:
232  raw_value = self.robot_lib.get_raw_value(self.joint_name)
233  self.setText(2, str(raw_value))
234  else:
235  raw_value = []
236  raw_value.append(self.robot_lib.get_raw_value(self.joint_name[0]))
237  raw_value.append(self.robot_lib.get_raw_value(self.joint_name[1]))
238  self.setText(2, str(raw_value[0]) + ", " + str(raw_value[1]))
239 
240  # if the 5 last values are equal, then display a warning
241  # as there's always some noise on the values
242  self.last_raw_values.append(raw_value)
243  if len(self.last_raw_values) > self.nb_values_to_check:
244  self.last_raw_values.popleft()
245 
246  # only check if we have enough values
247  all_equal = True
248  last_data = self.last_raw_values[0]
249  for data in self.last_raw_values:
250  if type(data) is not list:
251  last_data_not_equal = data != last_data
252  else:
253  last_data_not_equal = (data[0] != last_data[0] or data[1] != last_data[1])
254  if last_data_not_equal:
255  all_equal = False
256  break
257  if all_equal:
258  self.setIcon(
259  0, QIcon(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../icons/warn.gif')))
260  self.setToolTip(0, "No noise on the data for the last " + str(
261  self.nb_values_to_check) + " values, there could be a problem with the sensor.")
262  else:
263  self.setIcon(0, QIcon())
264  self.setToolTip(0, "")
265 
266  def on_close(self):
267  self.timer.stop()
268 
269 
270 class FingerCalibration(QTreeWidgetItem):
271 
272  """
273  calibrate all joints of a finger
274  """
275 
276  def __init__(self, finger_name,
277  finger_joints,
278  parent_widget, tree_widget,
279  robot_lib):
280 
281  QTreeWidgetItem.__init__(
282  self, parent_widget, [finger_name, "", "", ""])
283 
284  self.joints = []
285  for joint in finger_joints:
286  self.joints.append(JointCalibration(joint_name=joint[0],
287  calibrations=joint[1],
288  parent_widget=self,
289  tree_widget=tree_widget,
290  robot_lib=robot_lib))
291 
292  tree_widget.addTopLevelItem(self)
293 
294 
295 class HandCalibration(QTreeWidgetItem):
296 
297  """
298  calibrate all joints of all fingers of a hand
299  """
300 
301  def __init__(self,
302  tree_widget,
303  progress_bar,
304  fingers=["First Finger", "Middle Finger",
305  "Ring Finger", "Little Finger",
306  "Thumb", "Wrist"],
307  old_version=False):
308 
309  self.old_version = old_version
310 
311  # TODO: Import this from an xml file?
312  self.joint_map = {"First Finger": [["FFJ1", [[0.0, 0.0],
313  [0.0, 22.5],
314  [0.0, 45.0],
315  [0.0, 67.5],
316  [0.0, 90.0]]],
317  ["FFJ2", [[0.0, 0.0],
318  [0.0, 22.5],
319  [0.0, 45.0],
320  [0.0, 67.5],
321  [0.0, 90.0]]],
322  ["FFJ3", [[0.0, 0.0],
323  [0.0, 22.5],
324  [0.0, 45.0],
325  [0.0, 67.5],
326  [0.0, 90.0]]],
327  ["FFJ4", [[0.0, -20.0],
328  [0.0, -10.0],
329  [0.0, 0.0],
330  [0.0, 10.0],
331  [0.0, 20.0]]]],
332 
333  "Middle Finger": [["MFJ1", [[0.0, 0.0],
334  [0.0, 22.5],
335  [0.0, 45.0],
336  [0.0, 67.5],
337  [0.0, 90.0]]],
338  ["MFJ2", [[0.0, 0.0],
339  [0.0, 22.5],
340  [0.0, 45.0],
341  [0.0, 67.5],
342  [0.0, 90.0]]],
343  ["MFJ3", [[0.0, 0.0],
344  [0.0, 22.5],
345  [0.0, 45.0],
346  [0.0, 67.5],
347  [0.0, 90.0]]],
348  ["MFJ4", [[0.0, -20.0],
349  [0.0, -10.0],
350  [0.0, 0.0],
351  [0.0, 10.0],
352  [0.0, 20.0]]]],
353 
354  "Ring Finger": [["RFJ1", [[0.0, 0.0],
355  [0.0, 22.5],
356  [0.0, 45.0],
357  [0.0, 67.5],
358  [0.0, 90.0]]],
359  ["RFJ2", [[0.0, 0.0],
360  [0.0, 22.5],
361  [0.0, 45.0],
362  [0.0, 67.5],
363  [0.0, 90.0]]],
364  ["RFJ3", [[0.0, 0.0],
365  [0.0, 22.5],
366  [0.0, 45.0],
367  [0.0, 67.5],
368  [0.0, 90.0]]],
369  ["RFJ4", [[0.0, -20.0],
370  [0.0, -10.0],
371  [0.0, 0.0],
372  [0.0, 10.0],
373  [0.0, 20.0]]]],
374 
375  "Little Finger": [["LFJ1", [[0.0, 0.0],
376  [0.0, 22.5],
377  [0.0, 45.0],
378  [0.0, 67.5],
379  [0.0, 90.0]]],
380  ["LFJ2", [[0.0, 0.0],
381  [0.0, 22.5],
382  [0.0, 45.0],
383  [0.0, 67.5],
384  [0.0, 90.0]]],
385  ["LFJ3", [[0.0, 0.0],
386  [0.0, 22.5],
387  [0.0, 45.0],
388  [0.0, 67.5],
389  [0.0, 90.0]]],
390  ["LFJ4", [[0.0, -20.0],
391  [0.0, -10.0],
392  [0.0, 0.0],
393  [0.0, 10.0],
394  [0.0, 20.0]]],
395  ["LFJ5", [[0.0, 0.0],
396  [0.0, 22.5],
397  [0.0, 45.0],
398  [0.0, 67.5],
399  [0.0, 90.0]]]],
400 
401  "Wrist": [["WRJ1", [[0.0, -45.0],
402  [0.0, -22.5],
403  [0.0, 0.0],
404  [0.0, 15.0],
405  [0.0, 30.0]]],
406  ["WRJ2", [[0.0, -30.0],
407  [0.0, 0.0],
408  [0.0, 10.0]]]]
409  }
410 
411  if not self.old_version:
412  self.joint_map["Thumb"] = [[["THJ1", "THJ2"], [[[0.0, 0.0], [0.0, 40]],
413  [[0.0, 0.0], [0.0, 20]],
414  [[0.0, 0.0], [0.0, 0.0]],
415  [[0.0, 0.0], [0.0, -20.0]],
416  [[0.0, 0.0], [0.0, -40.0]],
417  [[0.0, 0.0], [22.5, 40.0]],
418  [[0.0, 0.0], [22.5, 20.0]],
419  [[0.0, 0.0], [22.5, 0.0]],
420  [[0.0, 0.0], [22.5, -20.0]],
421  [[0.0, 0.0], [22.5, -40.0]],
422  [[0.0, 0.0], [45.0, 40.0]],
423  [[0.0, 0.0], [45.0, 20.0]],
424  [[0.0, 0.0], [45.0, 0.0]],
425  [[0.0, 0.0], [45.0, -20.0]],
426  [[0.0, 0.0], [45.0, -40.0]],
427  [[0.0, 0.0], [67.5, 40.0]],
428  [[0.0, 0.0], [67.5, 20.0]],
429  [[0.0, 0.0], [67.5, 0.0]],
430  [[0.0, 0.0], [67.5, -20.0]],
431  [[0.0, 0.0], [67.5, -40.0]],
432  [[0.0, 0.0], [90.0, 40.0]],
433  [[0.0, 0.0], [90.0, 20.0]],
434  [[0.0, 0.0], [90.0, 0.0]],
435  [[0.0, 0.0], [90.0, -20.0]],
436  [[0.0, 0.0], [90.0, -40]]]],
437  ["THJ3", [[0.0, -15.0],
438  [0.0, 0.0],
439  [0.0, 15.0]]],
440  ["THJ4", [[0.0, 0.0],
441  [0.0, 22.5],
442  [0.0, 45.0],
443  [0.0, 67.5]]],
444  ["THJ5", [[0.0, -60.0],
445  [0.0, -30.0],
446  [0.0, 0.0],
447  [0.0, 30.0],
448  [0.0, 60.0]]]]
449  else:
450  self.joint_map["Thumb"] = [["THJ1", [[0.0, 0.0],
451  [0.0, 22.5],
452  [0.0, 45.0],
453  [0.0, 67.5],
454  [0.0, 90.0]]],
455  ["THJ2", [[0.0, -40.0],
456  [0.0, -20.0],
457  [0.0, 0.0],
458  [0.0, 20.0],
459  [0.0, 40.0]]],
460  ["THJ3", [[0.0, -15.0],
461  [0.0, 0.0],
462  [0.0, 15.0]]],
463  ["THJ4", [[0.0, 0.0],
464  [0.0, 22.5],
465  [0.0, 45.0],
466  [0.0, 67.5]]],
467  ["THJ5", [[0.0, -60.0],
468  [0.0, -30.0],
469  [0.0, 0.0],
470  [0.0, 30.0],
471  [0.0, 60.0]]]]
472 
473  self.fingers = []
474  # this is set to False if the user doesn't want to continue
475  # when there are no EtherCAT hand node currently running.
476  self.is_active = True
477 
478  QTreeWidgetItem.__init__(self, ["Hand", "", "", ""])
479 
480  self.robot_lib = EtherCAT_Hand_Lib()
481  if not self.robot_lib.activate():
482  btn_pressed = QMessageBox.warning(
483  tree_widget, "Warning", "The EtherCAT Hand node doesn't seem to be running, or the debug topic is not"
484  " being published. Do you still want to continue? The calibration will be useless.",
485  buttons=QMessageBox.Ok | QMessageBox.Cancel)
486 
487  if btn_pressed == QMessageBox.Cancel:
488  self.is_active = False
489 
490  for finger in fingers:
491  if finger in self.joint_map.keys():
492  self.fingers.append(FingerCalibration(finger,
493  self.joint_map[finger],
494  self, tree_widget,
495  self.robot_lib))
496 
497  else:
498  print finger, " not found in the calibration map"
499 
501 
502  self.progress_bar = progress_bar
503 
504  self.tree_widget = tree_widget
505  self.tree_widget.addTopLevelItem(self)
506  self.tree_widget.itemActivated.connect(self.calibrate_item)
507 
508  def unregister(self):
509  it = QTreeWidgetItemIterator(self)
510  while it.value():
511  try:
512  it.value().on_close()
513  except:
514  pass
515  it += 1
516 
517  self.robot_lib.on_close()
518 
519  def calibrate_item(self, item):
520  try:
521  # only the IndividualCalibration have the calibrate method
522  item.calibrate()
523  except:
524  pass
525 
526  self.progress()
527 
528  # select the next row by default
529  item.setSelected(False)
530  next_item = self.tree_widget.itemBelow(item)
531  if next_item is not None:
532  next_item.setSelected(True)
533  self.tree_widget.setCurrentItem(next_item)
534 
535  def calibrate_joint0s(self, btn_joint_0s):
536  joint0s = ["FFJ1", "FFJ2",
537  "MFJ1", "MFJ2",
538  "RFJ1", "RFJ2",
539  "LFJ1", "LFJ2"]
540 
541  it = QTreeWidgetItemIterator(self)
542  while it.value():
543  if it.value().text(1) in joint0s:
544  it += self.joint_0_calibration_index + 1
545  it.value().calibrate()
546  it += 1
547 
548  self.joint_0_calibration_index += 1
549  if self.joint_0_calibration_index == len(self.joint_map["First Finger"][0][1]):
551 
552  # updating the btn text
553  btn_joint_0s.setText(
554  "Save all Joint 0s (angle = " +
555  str(self.joint_map["First Finger"][0][1][self.joint_0_calibration_index][1]) + ")")
556 
557  self.progress()
558 
559  def progress(self):
560  it = QTreeWidgetItemIterator(self)
561  nb_of_items = 0
562  nb_of_calibrated_items = 0
563  while it.value():
564  it += 1
565  try:
566  if it.value().is_calibrated:
567  nb_of_calibrated_items += 1
568  nb_of_items += 1
569  except:
570  pass
571 
572  self.progress_bar.setValue(
573  int(float(nb_of_calibrated_items) / float(nb_of_items) * 100.0))
574 
575  def load(self, filepath):
576  f = open(filepath, 'r')
577  document = ""
578  for line in f.readlines():
579  document += line
580  f.close()
581  yaml_config = yaml.load(document)
582 
583  if "sr_calibrations" not in yaml_config.keys():
584  error_string = ('The selected calibration file does not contain calibration ' +
585  'values.')
586  rospy.logwarn(error_string)
587  QMessageBox(QMessageBox.Critical, 'Calibration file error', error_string).exec_()
588  return
589 
590  if self.old_version:
591  used_yaml_config = yaml_config["sr_calibrations"]
592  else:
593  if "sr_calibrations_coupled" not in yaml_config.keys():
594  error_string = ('The selected calibration file does not contain coupled thumb calibration ' +
595  'values. Choose one that does, or switch to "Old Version" mode.')
596  rospy.logwarn(error_string)
597  QMessageBox(QMessageBox.Critical, 'Calibration file error', error_string).exec_()
598  return
599  used_yaml_config = yaml_config["sr_calibrations"] + yaml_config["sr_calibrations_coupled"]
600  for joint in used_yaml_config:
601  it = QTreeWidgetItemIterator(self)
602  while it.value():
603  if type(joint[0]) is not list:
604  joint_name = joint[0]
605  else:
606  joint_name = ", ".join(joint[0])
607  if it.value().text(1) == joint_name:
608  it.value().load_joint_calibration(joint[1])
609  it += 1
610 
611  self.progress_bar.setValue(100)
612 
613  def save(self, filepath):
614  yaml_config = {}
615 
616  joint_configs = []
617  it = QTreeWidgetItemIterator(self)
618  while it.value():
619  try:
620  joint_configs.append(it.value().get_joint_calibration())
621  except:
622  pass
623  it += 1
624 
625  # this doesn't work as we'd like
626  # yaml_config["sr_calibrations"] = joint_configs
627  # full_config_to_write = yaml.dump(yaml_config,
628  # default_flow_style=False)
629  full_config_to_write = "sr_calibrations: [\n"
630  for joint_config in joint_configs:
631  if type(joint_config[0]) is not list:
632  full_config_to_write += "[\""
633  full_config_to_write += joint_config[0] + "\", "
634 
635  # the etherCAT driver wants floats
636  for index, calib in enumerate(joint_config[1]):
637  joint_config[1][index][0] = float(calib[0])
638  joint_config[1][index][1] = float(calib[1])
639 
640  full_config_to_write += str(joint_config[1])
641  full_config_to_write += "], \n"
642  full_config_to_write += "]"
643 
644  if not self.old_version:
645  full_config_to_write += "\n\nsr_calibrations_coupled: [\n"
646  for joint_config in joint_configs:
647  if type(joint_config[0]) is list:
648  full_config_to_write += "[[\""
649  full_config_to_write += joint_config[0][0] + "\", \"" + joint_config[0][1] + "\"], ["
650 
651  for index, calib in enumerate(joint_config[1]):
652  joint_config[1][index][0][0] = float(calib[0][0])
653  joint_config[1][index][0][1] = float(calib[0][1])
654  joint_config[1][index][1][0] = float(calib[1][0])
655  joint_config[1][index][1][1] = float(calib[1][1])
656 
657  if index > 0:
658  full_config_to_write += ", \n "
659  full_config_to_write += "["
660  full_config_to_write += str(joint_config[1][index][0]) + ", "
661  full_config_to_write += str(joint_config[1][index][1][0]) + ", " + \
662  str(joint_config[1][index][1][1])
663  full_config_to_write += "]"
664  full_config_to_write += "]]"
665  full_config_to_write += "\n]"
666 
667  f = open(filepath, 'w')
668  f.write(full_config_to_write)
669  f.close()
670 
672  it = QTreeWidgetItemIterator(self)
673  while it.value():
674  try:
675  if not it.value().is_calibrated:
676  return False
677  except:
678  pass
679  it += 1
680 
681  return True
def __init__(self, joint_names, raw_values, calibrated_values, parent_widget, tree_widget, robot_lib)
def __init__(self, joint_name, calibrations, parent_widget, tree_widget, robot_lib)
def __init__(self, joint_name, raw_value, calibrated_value, parent_widget, tree_widget, robot_lib)
def __init__(self, tree_widget, progress_bar, fingers=["First Finger", Middle, Finger, Ring, Finger, Little, Finger, Thumb, Wrist, old_version=False)
def __init__(self, finger_name, finger_joints, parent_widget, tree_widget, robot_lib)


sr_gui_hand_calibration
Author(s): Ugo Cupcic
autogenerated on Wed Oct 14 2020 03:22:44