sr_gui_grasp_controller: grasp_controller.py Source File

Go to the documentation of this file.
00001 # Copyright 2011 Shadow Robot Company Ltd.
00002 #
00003 # This program is free software: you can redistribute it and/or modify it
00004 # under the terms of the GNU General Public License as published by the Free
00005 # Software Foundation, either version 2 of the License, or (at your option)
00006 # any later version.
00007 #
00008 # This program is distributed in the hope that it will be useful, but WITHOUT
00009 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00010 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00011 # more details.
00012 #
00013 # You should have received a copy of the GNU General Public License along
00014 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00015 
00016 import os, time
00017 import roslib; roslib.load_manifest('sr_gui_grasp_controller')
00018 import rospy
00019 
00020 from rospy import loginfo, logerr, logdebug
00021 
00022 from qt_gui.plugin import Plugin
00023 from python_qt_binding import loadUi
00024 
00025 import QtCore
00026 from QtCore import Qt, QEvent, QObject
00027 import QtGui
00028 from QtGui import *
00029 
00030 from Grasp import Grasp
00031 from grasps_interpoler import GraspInterpoler
00032 from grasps_parser import GraspParser
00033 
00034 from shadowhand_ros import ShadowHand_ROS
00035 
00036 class JointSelecter(QtGui.QWidget):
00037     """
00038     Select which joints to save in a new grasp
00039     """
00040     def __init__(self, parent, all_joints):
00041         QtGui.QWidget.__init__(self, parent=parent)
00042         self.frame = QtGui.QFrame()
00043         self.layout = QtGui.QGridLayout()
00044         self.checkboxes = []
00045 
00046         col = 0
00047         #vectors to set the correct row in the layout for each col
00048         rows = [0, 0, 0, 0, 0, 0]
00049         joint_names = all_joints.keys()
00050         joint_names.sort()
00051         for joint in joint_names:
00052             if "fj1" in joint.lower():
00053                 continue
00054             if "fj2" in joint.lower():
00055                 continue
00056             if "ff" in joint.lower():
00057                 col = 0
00058             elif "mf" in joint.lower():
00059                 col = 1
00060             elif "rf" in joint.lower():
00061                 col = 2
00062             elif "lf" in joint.lower():
00063                 col = 3
00064             elif "th" in joint.lower():
00065                 col = 4
00066             else:
00067                 col = 5
00068 
00069             row = rows[col]
00070             rows[col] = row + 1
00071             cb = QtGui.QCheckBox(str(joint), self.frame)
00072             self.checkboxes.append(cb)
00073             self.layout.addWidget(cb, row, col)
00074 
00075         self.frame.setLayout(self.layout)
00076         layout = QtGui.QVBoxLayout()
00077         layout.addWidget(self.frame)
00078         self.frame.show()
00079         self.setLayout(layout)
00080         self.show()
00081 
00082     def get_selected(self):
00083         joints = []
00084         for cb in self.checkboxes:
00085             if cb.isChecked():
00086                 joints.append(str(cb.text()))
00087 
00088         return joints
00089 
00090     def select_all(self):
00091         for cb in self.checkboxes:
00092             cb.setChecked(True)
00093 
00094     def deselect_all(self):
00095         for cb in self.checkboxes:
00096             cb.setChecked(False)
00097 
00098 class GraspSaver(QtGui.QDialog):
00099     """
00100     Save a new grasp from the current joints positions.
00101     """
00102     def __init__(self, parent, all_joints, plugin_parent):
00103         QtGui.QDialog.__init__(self, parent)
00104         self.plugin_parent = plugin_parent
00105         self.all_joints = all_joints
00106         self.setModal(True)
00107         self.setWindowTitle("Save Grasp")
00108 
00109         self.grasp_name = ""
00110 
00111         self.upper_frame = QtGui.QFrame()
00112         self.upper_layout = QtGui.QHBoxLayout()
00113         label_name = QtGui.QLabel()
00114         label_name.setText("Grasp Name: ")
00115         name_widget = QtGui.QLineEdit()
00116         self.upper_frame.connect(name_widget, QtCore.SIGNAL('textChanged(QString)'), self.name_changed)
00117 
00118         self.upper_layout.addWidget(label_name)
00119         self.upper_layout.addWidget(name_widget)
00120         self.upper_frame.setLayout(self.upper_layout)
00121 
00122         select_all_frame = QtGui.QFrame()
00123         select_all_layout = QtGui.QHBoxLayout()
00124         btn_select_all = QtGui.QPushButton(select_all_frame)
00125         btn_select_all.setText("Select All")
00126         select_all_layout.addWidget(btn_select_all)
00127         self.connect(btn_select_all, QtCore.SIGNAL("clicked()"), self.select_all)
00128         btn_deselect_all = QtGui.QPushButton(select_all_frame)
00129         btn_deselect_all.setText("Deselect All")
00130         select_all_layout.addWidget(btn_deselect_all)
00131         self.connect(btn_deselect_all, QtCore.SIGNAL("clicked()"), self.deselect_all)
00132         select_all_frame.setLayout(select_all_layout)
00133 
00134         self.joint_selecter = JointSelecter(self, self.all_joints)
00135 
00136         btn_frame = QtGui.QFrame()
00137         self.btn_ok = QtGui.QPushButton(btn_frame)
00138         self.btn_ok.setText("OK")
00139         self.btn_ok.setDisabled(True)
00140         self.connect(self.btn_ok, QtCore.SIGNAL("clicked()"), self.accept)
00141         btn_cancel = QtGui.QPushButton(btn_frame)
00142         btn_cancel.setText("Cancel")
00143         self.connect(btn_cancel, QtCore.SIGNAL("clicked()"), self.reject)
00144 
00145         btn_layout = QtGui.QHBoxLayout()
00146         btn_layout.addWidget(self.btn_ok)
00147         btn_layout.addWidget(btn_cancel)
00148         btn_frame.setLayout(btn_layout)
00149 
00150         self.layout = QtGui.QVBoxLayout()
00151         self.layout.addWidget(self.upper_frame)
00152         self.layout.addWidget(select_all_frame)
00153         self.layout.addWidget(self.joint_selecter)
00154         self.layout.addWidget(btn_frame)
00155 
00156         self.setLayout(self.layout)
00157         self.show()
00158 
00159     def select_all(self):
00160         self.joint_selecter.select_all()
00161 
00162     def deselect_all(self):
00163         self.joint_selecter.deselect_all()
00164 
00165     def name_changed(self, name):
00166         self.grasp_name = name
00167         if self.grasp_name != "":
00168             self.btn_ok.setEnabled(True)
00169         else:
00170             self.btn_ok.setDisabled(True)
00171 
00172     def accept(self):
00173         grasp = Grasp()
00174         grasp.grasp_name = self.grasp_name
00175 
00176         joints_to_save = self.joint_selecter.get_selected()
00177         if len(joints_to_save) == 0:
00178             joints_to_save = self.all_joints.keys()
00179         for joint_to_save in joints_to_save:
00180             grasp.joints_and_positions[joint_to_save] = self.all_joints[joint_to_save]
00181 
00182         self.plugin_parent.sr_lib.grasp_parser.write_grasp_to_file(grasp)
00183         self.plugin_parent.sr_lib.grasp_parser.refresh()
00184 
00185         self.plugin_parent.reloadGraspSig['int'].emit(1)
00186 
00187         QtGui.QDialog.accept(self)
00188 
00189 class GraspChooser(QtGui.QWidget):
00190     """
00191     Choose a grasp from a list of grasps.
00192     """
00193     def __init__(self, parent, plugin_parent, title):
00194         QtGui.QWidget.__init__(self)
00195         self.plugin_parent = plugin_parent
00196         self.grasp = None
00197         self.title = QtGui.QLabel()
00198         self.title.setText(title)
00199 
00200     def draw(self):
00201         self.frame = QtGui.QFrame(self)
00202 
00203         self.list = QtGui.QListWidget()
00204         first_item = self.refresh_list()
00205         self.connect(self.list, QtCore.SIGNAL('itemClicked(QListWidgetItem*)'), self.grasp_choosed)
00206 
00207         self.connect(self.list, QtCore.SIGNAL('itemDoubleClicked(QListWidgetItem*)'), self.double_click)
00208         self.list.setViewMode(QtGui.QListView.ListMode)
00209         self.list.setResizeMode(QtGui.QListView.Adjust)
00210         self.list.setItemSelected(first_item, True)
00211         self.grasp_choosed(first_item, first_time=True)
00212 
00213         self.layout = QtGui.QVBoxLayout()
00214         self.layout.addWidget(self.title)
00215         self.layout.addWidget(self.list)
00216 
00217         ###
00218         # SIGNALS
00219         ##
00220         self.plugin_parent.reloadGraspSig['int'].connect(self.refresh_list)
00221 
00222         self.frame.setLayout(self.layout)
00223         layout = QtGui.QVBoxLayout()
00224         layout.addWidget(self.frame)
00225         self.frame.show()
00226         self.setLayout(layout)
00227         self.show()
00228 
00229     def double_click(self, item):
00230         self.grasp = self.plugin_parent.sr_lib.grasp_parser.grasps[str(item.text())]
00231         self.plugin_parent.sr_lib.sendupdate_from_dict(self.grasp.joints_and_positions)
00232         self.plugin_parent.set_reference_grasp()
00233 
00234     def grasp_choosed(self, item, first_time=False):
00235         self.grasp = self.plugin_parent.sr_lib.grasp_parser.grasps[str(item.text())]
00236         if not first_time:
00237             self.plugin_parent.grasp_changed()
00238             self.plugin_parent.set_reference_grasp()
00239 
00240     def refresh_list(self, value = 0):
00241         self.list.clear()
00242         first_item = None
00243         grasps = self.plugin_parent.sr_lib.grasp_parser.grasps.keys()
00244         grasps.sort()
00245         for grasp_name in grasps:
00246             item = QtGui.QListWidgetItem(grasp_name)
00247             if first_item == None:
00248                 first_item = item
00249             self.list.addItem(item)
00250         return first_item
00251 
00252 class GraspSlider(QtGui.QWidget):
00253     """
00254     Slide from one grasp to another.
00255     """
00256     def __init__(self, parent, plugin_parent):
00257         QtGui.QWidget.__init__(self, parent)
00258         self.plugin_parent = plugin_parent
00259 
00260     def draw(self):
00261         self.frame = QtGui.QFrame(self)
00262         label_frame = QtGui.QFrame(self.frame)
00263         from_label = QtGui.QLabel()
00264         from_label.setText("From")
00265         ref_label = QtGui.QLabel()
00266         ref_label.setText("Reference")
00267         to_label = QtGui.QLabel()
00268         to_label.setText("To")
00269         label_layout = QtGui.QHBoxLayout()
00270         label_layout.addWidget(from_label)
00271         label_layout.addWidget(ref_label)
00272         label_layout.addWidget(to_label)
00273 
00274         label_frame.setLayout(label_layout)
00275 
00276         self.slider = QtGui.QSlider()
00277         self.slider.setOrientation(QtCore.Qt.Horizontal)
00278         self.slider.setFocusPolicy(QtCore.Qt.NoFocus)
00279         self.slider.setTickInterval(100)
00280         self.slider.setTickPosition(QSlider.TicksAbove)
00281         self.slider.setMinimum(-100)
00282         self.slider.setMaximum(100)
00283 
00284         self.connect(self.slider, QtCore.SIGNAL('valueChanged(int)'), self.changeValue)
00285 
00286         self.layout = QtGui.QVBoxLayout()
00287         self.layout.addWidget(label_frame)
00288         self.layout.addWidget(self.slider)
00289 
00290         self.frame.setLayout(self.layout)
00291         layout = QtGui.QVBoxLayout()
00292         layout.addWidget(self.frame)
00293         self.frame.show()
00294         self.setLayout(layout)
00295         self.show()
00296 
00297     def changeValue(self, value):
00298         self.plugin_parent.interpolate_grasps(value)
00299 
00300 class SrGuiGraspController(Plugin):
00301     """Main GraspController plugin Dock window."""
00302 
00303     reloadGraspSig = QtCore.pyqtSignal(int)
00304 
00305     def __init__(self, context):
00306         super(SrGuiGraspController, self).__init__(context)
00307 
00308         self.setObjectName('SrGuiGraspController')
00309         self_dir      = os.path.dirname(os.path.realpath(__file__));
00310         self.ui_dir   = os.path.join(self_dir, '../../ui')
00311         self.img_dir  = os.path.join(self_dir, '../../images')
00312         self.icon_dir = os.path.join(self_dir, '../../images/icons')
00313         self.sr_lib   = ShadowHand_ROS()
00314 
00315         ui_file      = os.path.join(self.ui_dir, 'SrGuiGraspController.ui')
00316         self._widget     = QWidget()
00317         loadUi(ui_file, self._widget)
00318         context.add_widget(self._widget)
00319 
00320         self.current_grasp = Grasp()
00321         self.current_grasp.name = 'CURRENT_UNSAVED'
00322         self.grasp_interpoler_1 = None
00323         self.grasp_interpoler_2 = None
00324 
00325         self.layout = self._widget.layout
00326 
00327         subframe = QtGui.QFrame()
00328         sublayout = QtGui.QVBoxLayout()
00329 
00330         self.grasp_slider = GraspSlider(self._widget, self)
00331         sublayout.addWidget(self.grasp_slider)
00332 
00333         btn_frame = QtGui.QFrame()
00334         btn_layout = QtGui.QHBoxLayout()
00335         self.btn_save = QtGui.QPushButton()
00336         self.btn_save.setText("Save")
00337         self.btn_save.setFixedWidth(130)
00338         self.btn_save.setIcon(QtGui.QIcon(self.icon_dir + '/save.png'))
00339         btn_frame.connect(self.btn_save, QtCore.SIGNAL('clicked()'), self.save_grasp)
00340         btn_layout.addWidget(self.btn_save)
00341         btn_set_ref = QtGui.QPushButton()
00342         btn_set_ref.setText("Set Reference")
00343         btn_set_ref.setFixedWidth(130)
00344         btn_set_ref.setIcon(QtGui.QIcon(self.icon_dir + '/iconHand.png'))
00345         btn_frame.connect(btn_set_ref, QtCore.SIGNAL('clicked()'), self.set_reference_grasp)
00346         btn_layout.addWidget(btn_set_ref)
00347 
00348         btn_frame.setLayout(btn_layout)
00349         sublayout.addWidget(btn_frame)
00350         subframe.setLayout(sublayout)
00351 
00352         self.grasp_from_chooser = GraspChooser(self._widget, self, "From: ")
00353         self.layout.addWidget(self.grasp_from_chooser)
00354         self.layout.addWidget(subframe)
00355 
00356         self.grasp_to_chooser = GraspChooser(self._widget, self, "To: ")
00357         self.layout.addWidget(self.grasp_to_chooser)
00358 
00359         self.grasp_slider.draw()
00360         self.grasp_to_chooser.draw()
00361         self.grasp_from_chooser.draw()
00362 
00363         time.sleep(0.2)
00364         self.set_reference_grasp()
00365 
00366     def shutdown_plugin(self):
00367         self._widget.close()
00368         self._widget.deleteLater()
00369 
00370     def save_settings(self, global_settings, perspective_settings):
00371         pass
00372 
00373     def restore_settings(self, global_settings, perspective_settings):
00374         pass
00375 
00376     def save_grasp(self):
00377         all_joints = self.sr_lib.read_all_current_positions()
00378         GraspSaver(self._widget, all_joints, self)
00379 
00380     def set_reference_grasp(self):
00381         self.current_grasp.joints_and_positions = self.sr_lib.read_all_current_positions()
00382         
00383         if self.current_grasp.joints_and_positions is None:
00384             #We try to activate ethercat hand again (detect if any controllers are running, and listen to them to extract the current position)
00385             self.sr_lib.activate_etherCAT_hand()
00386             #Some time to start receiving data
00387             rospy.sleep(0.5)
00388             self.current_grasp.joints_and_positions = self.sr_lib.read_all_current_positions()
00389         
00390             if self.current_grasp.joints_and_positions is None:
00391                 QMessageBox.warning(self._widget, "Warning", "Could not read current grasp.\nCheck that the hand controllers are running.\nThen click \"Set Reference\"")
00392                 return
00393 
00394         self.grasp_interpoler_1 = GraspInterpoler(self.grasp_from_chooser.grasp, self.current_grasp)
00395         self.grasp_interpoler_2 = GraspInterpoler(self.current_grasp, self.grasp_to_chooser.grasp)
00396 
00397         self.grasp_slider.slider.setValue(0)
00398 
00399     def grasp_changed(self):
00400         self.current_grasp.joints_and_positions = self.sr_lib.read_all_current_positions()
00401         
00402         if self.current_grasp.joints_and_positions is None:
00403             QMessageBox.warning(self._widget, "Warning", "Could not read current grasp.\nCheck that the hand controllers are running.\nThen click \"Set Reference\"")
00404             return
00405         
00406         self.grasp_interpoler_1 = GraspInterpoler(self.grasp_from_chooser.grasp, self.current_grasp)
00407         self.grasp_interpoler_2 = GraspInterpoler(self.current_grasp, self.grasp_to_chooser.grasp)
00408 
00409     def interpolate_grasps(self, value):
00410         if self.grasp_interpoler_1 is None \
00411             or self.grasp_interpoler_2 is None:
00412             QMessageBox.warning(self._widget, "Warning", "Could not read current grasp.\nCheck that the hand controllers are running.\nThen click \"Set Reference\"")
00413             return
00414         #from -> current
00415         if value < 0:
00416             targets_to_send = self.grasp_interpoler_1.interpolate(100 + value)
00417             self.sr_lib.sendupdate_from_dict(targets_to_send)
00418         else:   #current -> to
00419             targets_to_send = self.grasp_interpoler_2.interpolate(value)
00420             self.sr_lib.sendupdate_from_dict(targets_to_send)