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
27 from yaml
import CLoader
as Loader, CDumper
as Dumper
29 from yaml
import Loader, Dumper
32 from collections
import deque
34 green = QColor(153, 231, 96)
35 orange = QColor(247, 206, 134)
36 red = QColor(236, 178, 178)
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 48 raw_value, calibrated_value,
49 parent_widget, tree_widget,
57 QTreeWidgetItem.__init__(self, parent_widget, [
60 for col
in xrange(self.tree_widget.columnCount()):
61 self.setBackground(col, QColor(red))
63 self.tree_widget.addTopLevelItem(self)
68 self.tree_widget.remove
72 Performs the joint calibration and sets background to green 73 calibrate only the calibration lines, not the items for the fingers / joints / hand 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))
79 for col
in xrange(self.tree_widget.columnCount()):
80 if self.text(2) !=
"":
81 self.setBackground(col, QColor(green))
83 self.is_calibrated =
True 87 set the background to orange: those values are loaded 88 from the file, not recalibrated 90 for col
in xrange(self.tree_widget.columnCount()):
91 if self.text(2) !=
"":
92 self.setBackground(col, QColor(orange))
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 109 raw_values, calibrated_values,
110 parent_widget, tree_widget,
114 self.
raw_values = [int(raw_value)
for raw_value
in raw_values]
119 QTreeWidgetItem.__init__(self, parent_widget, [
123 for col
in xrange(self.tree_widget.columnCount()):
124 self.setBackground(col, QColor(red))
126 self.tree_widget.addTopLevelItem(self)
132 Performs the joint calibration and sets background to green 133 calibrate only the calibration lines, not the items for the fingers / joints / hand 136 for idx
in range(0, 2):
137 self.
raw_values[idx] = self.robot_lib.get_average_raw_value(
139 raw_values_str.append(str(self.
raw_values[idx]))
140 self.setText(2,
", ".join(raw_values_str))
142 for col
in xrange(self.tree_widget.columnCount()):
143 if self.text(2) !=
"":
144 self.setBackground(col, QColor(green))
155 Calibrate a single joint by calibrations list 156 Also displays the current joint position in the GUI 159 nb_values_to_check = 5
163 parent_widget, tree_widget,
173 QTreeWidgetItem.__init__(
174 self, parent_widget, [
"", joint_name,
"",
""])
175 for calibration
in calibrations:
177 calibration[0], calibration[1],
178 self, tree_widget, robot_lib))
180 QTreeWidgetItem.__init__(
181 self, parent_widget, [
"", joint_name[0] +
", " + joint_name[1],
"",
""])
182 for calibration
in calibrations:
184 calibration[0], calibration[1],
185 self, tree_widget, robot_lib))
189 self.timer.start(200)
191 tree_widget.addTopLevelItem(self)
196 self.removeChild(calibration)
199 for calibration
in new_calibrations:
202 calibration[0], calibration[1],
206 calibration[0], [calibration[1], calibration[2]],
208 new_calib.set_is_loaded_calibration()
209 self.calibrations.append(new_calib)
214 if calibration.is_calibrated:
215 config.append(calibration.get_calibration())
221 config = [[0, 0.0], [1, 0.0]]
223 config = [[[0, 0], [0.0, 0.0]], [[1, 1], [0.0, 0.0]]]
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 232 raw_value = self.robot_lib.get_raw_value(self.
joint_name)
233 self.setText(2, str(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]))
242 self.last_raw_values.append(raw_value)
244 self.last_raw_values.popleft()
250 if type(data)
is not list:
251 last_data_not_equal = data != last_data
253 last_data_not_equal = (data[0] != last_data[0]
or data[1] != last_data[1])
254 if last_data_not_equal:
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(
263 self.setIcon(0, QIcon())
264 self.setToolTip(0,
"")
273 calibrate all joints of a finger 278 parent_widget, tree_widget,
281 QTreeWidgetItem.__init__(
282 self, parent_widget, [finger_name,
"",
"",
""])
285 for joint
in finger_joints:
287 calibrations=joint[1],
289 tree_widget=tree_widget,
290 robot_lib=robot_lib))
292 tree_widget.addTopLevelItem(self)
298 calibrate all joints of all fingers of a hand 304 fingers=[
"First Finger",
"Middle Finger",
305 "Ring Finger",
"Little Finger",
312 self.
joint_map = {
"First Finger": [[
"FFJ1", [[0.0, 0.0],
317 [
"FFJ2", [[0.0, 0.0],
322 [
"FFJ3", [[0.0, 0.0],
327 [
"FFJ4", [[0.0, -20.0],
333 "Middle Finger": [[
"MFJ1", [[0.0, 0.0],
338 [
"MFJ2", [[0.0, 0.0],
343 [
"MFJ3", [[0.0, 0.0],
348 [
"MFJ4", [[0.0, -20.0],
354 "Ring Finger": [[
"RFJ1", [[0.0, 0.0],
359 [
"RFJ2", [[0.0, 0.0],
364 [
"RFJ3", [[0.0, 0.0],
369 [
"RFJ4", [[0.0, -20.0],
375 "Little Finger": [[
"LFJ1", [[0.0, 0.0],
380 [
"LFJ2", [[0.0, 0.0],
385 [
"LFJ3", [[0.0, 0.0],
390 [
"LFJ4", [[0.0, -20.0],
395 [
"LFJ5", [[0.0, 0.0],
401 "Wrist": [[
"WRJ1", [[0.0, -45.0],
406 [
"WRJ2", [[0.0, -30.0],
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],
440 [
"THJ4", [[0.0, 0.0],
444 [
"THJ5", [[0.0, -60.0],
450 self.
joint_map[
"Thumb"] = [[
"THJ1", [[0.0, 0.0],
455 [
"THJ2", [[0.0, -40.0],
460 [
"THJ3", [[0.0, -15.0],
463 [
"THJ4", [[0.0, 0.0],
467 [
"THJ5", [[0.0, -60.0],
478 QTreeWidgetItem.__init__(self, [
"Hand",
"",
"",
""])
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)
487 if btn_pressed == QMessageBox.Cancel:
490 for finger
in fingers:
491 if finger
in self.joint_map.keys():
498 print finger,
" not found in the calibration map" 505 self.tree_widget.addTopLevelItem(self)
509 it = QTreeWidgetItemIterator(self)
512 it.value().on_close()
517 self.robot_lib.on_close()
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)
536 joint0s = [
"FFJ1",
"FFJ2",
541 it = QTreeWidgetItemIterator(self)
543 if it.value().text(1)
in joint0s:
545 it.value().calibrate()
553 btn_joint_0s.setText(
554 "Save all Joint 0s (angle = " +
560 it = QTreeWidgetItemIterator(self)
562 nb_of_calibrated_items = 0
566 if it.value().is_calibrated:
567 nb_of_calibrated_items += 1
572 self.progress_bar.setValue(
573 int(float(nb_of_calibrated_items) / float(nb_of_items) * 100.0))
576 f = open(filepath,
'r') 578 for line
in f.readlines():
581 yaml_config = yaml.load(document)
583 if "sr_calibrations" not in yaml_config.keys():
584 error_string = (
'The selected calibration file does not contain calibration ' +
586 rospy.logwarn(error_string)
587 QMessageBox(QMessageBox.Critical,
'Calibration file error', error_string).exec_()
591 used_yaml_config = yaml_config[
"sr_calibrations"]
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_()
599 used_yaml_config = yaml_config[
"sr_calibrations"] + yaml_config[
"sr_calibrations_coupled"]
600 for joint
in used_yaml_config:
601 it = QTreeWidgetItemIterator(self)
603 if type(joint[0])
is not list:
604 joint_name = joint[0]
606 joint_name =
", ".join(joint[0])
607 if it.value().text(1) == joint_name:
608 it.value().load_joint_calibration(joint[1])
611 self.progress_bar.setValue(100)
617 it = QTreeWidgetItemIterator(self)
620 joint_configs.append(it.value().get_joint_calibration())
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] +
"\", " 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])
640 full_config_to_write += str(joint_config[1])
641 full_config_to_write +=
"], \n" 642 full_config_to_write +=
"]" 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] +
"\"], [" 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])
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]" 667 f = open(filepath,
'w')
668 f.write(full_config_to_write)
672 it = QTreeWidgetItemIterator(self)
675 if not it.value().is_calibrated:
joint_0_calibration_index
def update_joint_pos(self)
def calibrate_item(self, item)
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 is_calibration_complete(self)
def get_calibration(self)
def load_joint_calibration(self, new_calibrations)
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 set_is_loaded_calibration(self)
def calibrate_joint0s(self, btn_joint_0s)
def get_joint_calibration(self)
def get_calibration(self)
def __init__(self, finger_name, finger_joints, parent_widget, tree_widget, robot_lib)