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

sr_gui_hand_calibration
Author(s): Ugo Cupcic
autogenerated on Mon Feb 28 2022 23:51:22