00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 import os, subprocess, math, time
00020 import rospy, rosparam, rospkg
00021
00022 from qt_gui.plugin import Plugin
00023 from python_qt_binding import loadUi
00024
00025 from QtCore import QEvent, QObject, Qt, QTimer, Slot, SIGNAL, QThread
00026 from QtGui import QWidget, QTreeWidgetItem, QCheckBox, QSpinBox, QDoubleSpinBox, QFileDialog, QMessageBox, QPushButton, QFrame, QHBoxLayout
00027 from functools import partial
00028 from tempfile import NamedTemporaryFile
00029
00030 from sensor_msgs.msg import JointState
00031
00032 from sr_gui_controller_tuner.sr_controller_tuner import SrControllerTunerApp
00033
00034 class PlotThread(QThread):
00035 def __init__(self, parent = None, joint_name = "FFJ0", controller_type = "Motor Force"):
00036 QThread.__init__(self, parent)
00037 self.joint_name_ = joint_name
00038 self.is_joint_0_ = False
00039 self.joint_index_in_joint_state_ = None
00040 try:
00041 self.joint_index_in_joint_state_ = ["FFJ0", "MFJ0", "RFJ0", "LFJ0"].index(self.joint_name_)
00042 self.is_joint_0_ = True
00043 except ValueError:
00044 self.is_joint_0_ = False
00045
00046 self.controller_type_ = controller_type
00047
00048
00049 self.plot_title_ = self.joint_name_ + " " + self.controller_type_
00050 self.plot_title_ = self.plot_title_.replace(" ", "_").replace("/","_")
00051
00052
00053 self.subprocess_ = []
00054
00055 def run(self):
00056 """
00057 Creates an appropriate plot according to controller type
00058 Also creates a subscription if controller is of Motor Force type
00059 """
00060
00061 rxplot_str = "rosrun rqt_plot rqt_plot "
00062
00063 if self.controller_type_ == "Motor Force":
00064
00065 if not self.is_joint_0_:
00066
00067 self.subscriber_ = rospy.Subscriber("joint_states", JointState, self.js_callback_)
00068
00069
00070
00071 while self.joint_index_in_joint_state_ == None:
00072 time.sleep(0.01)
00073
00074 rxplot_str += "joint_states/effort["+ str(self.joint_index_in_joint_state_) +"]"
00075 else:
00076 rxplot_str += "joint_0s/joint_states/effort["+ str(self.joint_index_in_joint_state_) +"]"
00077
00078 elif self.controller_type_ == "Position":
00079 rxplot_str += "sh_"+self.joint_name_.lower()+"_position_controller/state/set_point,sh_"+self.joint_name_.lower()+"_position_controller/state/process_value sh_" + self.joint_name_.lower()+"_position_controller/state/command"
00080 elif self.controller_type_ == "Muscle Position":
00081 rxplot_str += "sh_"+self.joint_name_.lower()+"_muscle_position_controller/state/set_point,sh_"+self.joint_name_.lower()+"_muscle_position_controller/state/process_value sh_" + self.joint_name_.lower()+"_muscle_position_controller/state/pseudo_command sh_" + self.joint_name_.lower()+"_muscle_position_controller/state/valve_muscle_0,sh_" + self.joint_name_.lower()+"_muscle_position_controller/state/valve_muscle_1"
00082 elif self.controller_type_ == "Velocity":
00083 rxplot_str += "sh_"+self.joint_name_.lower()+"_velocity_controller/state/set_point,sh_"+self.joint_name_.lower()+"_velocity_controller/state/process_value"
00084 elif self.controller_type_ == "Mixed Position/Velocity":
00085 rxplot_str += "sh_"+self.joint_name_.lower()+"_mixed_position_velocity_controller/state/set_point,sh_"+self.joint_name_.lower()+"_mixed_position_velocity_controller/state/process_value sh_"+self.joint_name_.lower()+"_mixed_position_velocity_controller/state/process_value_dot,sh_"+self.joint_name_.lower()+"_mixed_position_velocity_controller/state/commanded_velocity sh_"+self.joint_name_.lower()+"_mixed_position_velocity_controller/state/command,sh_"+self.joint_name_.lower()+"_mixed_position_velocity_controller/state/measured_effort,sh_"+self.joint_name_.lower()+"_mixed_position_velocity_controller/state/friction_compensation"
00086 elif self.controller_type_ == "Effort":
00087 rxplot_str += "sh_"+self.joint_name_.lower()+"_effort_controller/state/set_point,sh_"+self.joint_name_.lower()+"_effort_controller/state/process_value"
00088
00089 self.subprocess_.append( subprocess.Popen(rxplot_str.split()) )
00090
00091 def js_callback_(self, msg):
00092
00093 self.joint_index_in_joint_state_ = msg.name.index( self.joint_name_.upper() )
00094 self.subscriber_.unregister()
00095 self.subscriber_ = None
00096
00097 def __del__(self):
00098 for subprocess in self.subprocess_:
00099
00100 subprocess.kill()
00101
00102 if self.subscriber_ != None:
00103 self.subscriber_.unregister()
00104 self.subscriber_ = None
00105
00106 self.wait()
00107
00108 class MoveThread(QThread):
00109 def __init__(self, parent = None, joint_name = "FFJ0", controller_type = "Motor Force"):
00110 QThread.__init__(self, parent)
00111 self.joint_name_ = joint_name
00112 self.controller_type_ = controller_type
00113 self.btn = parent
00114 self.subprocess_ = []
00115
00116 def run(self):
00117 self.launch_()
00118
00119 def __del__(self):
00120 for subprocess in self.subprocess_:
00121 subprocess.terminate()
00122 self.wait()
00123
00124 def create_launch_file_(self):
00125 """
00126 Create a launch file dynamically
00127 """
00128
00129 controller_name_ = ""
00130 if self.controller_type_ == "Position":
00131 controller_name_ = "sh_"+self.joint_name_.lower()+"_position_controller"
00132 if self.controller_type_ == "Muscle Position":
00133 controller_name_ = "sh_"+self.joint_name_.lower()+"_muscle_position_controller"
00134 elif self.controller_type_ == "Mixed Position/Velocity":
00135 controller_name_ = "sh_"+self.joint_name_.lower()+"_mixed_position_velocity_controller"
00136
00137 min_max = self.get_min_max_()
00138 ns = rospy.get_namespace()
00139
00140 string = "<launch> <node ns=\"" + ns + "\" pkg=\"sr_movements\" name=\"sr_movements\" type=\"sr_movements\">"
00141 string += "<remap from=\"~targets\" to=\""+ controller_name_ +"/command\"/>"
00142 string += "<remap from=\"~inputs\" to=\""+ controller_name_ +"/state\"/>"
00143 string += "<param name=\"image_path\" value=\"$(find sr_movements)/movements/test.png\"/>"
00144 string += "<param name=\"min\" value=\""+ str(min_max[0]) +"\"/>"
00145 string += "<param name=\"max\" value=\""+ str(min_max[1]) + "\"/>"
00146 string += "<param name=\"publish_rate\" value=\"100\"/>"
00147 string += "<param name=\"repetition\" value=\"1000\"/>"
00148 string += "<param name=\"nb_step\" value=\"10000\"/>"
00149 string += "<param name=\"msg_type\" value=\"sr\"/>"
00150 string += "</node> </launch>"
00151
00152 tmp_launch_file = NamedTemporaryFile(delete=False)
00153
00154 tmp_launch_file.writelines(string)
00155 tmp_launch_file.close()
00156
00157 return tmp_launch_file.name
00158
00159 def get_min_max_(self):
00160 """
00161 Retrieve joint limits in radians for joint in self.joint_name
00162 """
00163 if self.joint_name_ in ["FFJ0", "MFJ0", "RFJ0", "LFJ0"]:
00164 return [0.0, math.radians(180.0)]
00165 elif self.joint_name_ in ["FFJ3", "MFJ3", "RFJ3", "LFJ3", "THJ1"]:
00166 return [0.0, math.radians(90.0)]
00167 elif self.joint_name_ in ["LFJ5"]:
00168 return [0.0, math.radians(45.0)]
00169 elif self.joint_name_ in ["FFJ4", "MFJ4", "RFJ4", "LFJ4"]:
00170 return [math.radians(-20.0), math.radians(20.0)]
00171 elif self.joint_name_ in ["THJ2"]:
00172 return [math.radians(-40.0), math.radians(40.0)]
00173 elif self.joint_name_ in ["THJ3"]:
00174 return [math.radians(-15.0), math.radians(15.0)]
00175 elif self.joint_name_ in ["THJ4"]:
00176 return [math.radians(0.0), math.radians(70.0)]
00177 elif self.joint_name_ in ["THJ5"]:
00178 return [math.radians(-60.0), math.radians(60.0)]
00179 elif self.joint_name_ in ["WRJ1"]:
00180 return [math.radians(-30.0), math.radians(45.0)]
00181 elif self.joint_name_ in ["WRJ2"]:
00182 return [math.radians(-30.0), math.radians(10.0)]
00183
00184 def launch_(self):
00185 """
00186 launch a dynamically created launch file
00187 """
00188 filename = self.create_launch_file_()
00189
00190 launch_string = "roslaunch "+filename
00191
00192 self.subprocess_.append( subprocess.Popen(launch_string.split()) )
00193
00194 class SrGuiControllerTuner(Plugin):
00195 """
00196 a rosgui plugin for tuning the sr_mechanism_controllers
00197 """
00198 def __init__(self, context):
00199 super(SrGuiControllerTuner, self).__init__(context)
00200 self.setObjectName('SrGuiControllerTuner')
00201
00202 self.controller_type = None
00203
00204 self._publisher = None
00205 self._widget = QWidget()
00206
00207 self.file_to_save = None
00208
00209 ui_file = os.path.join(rospkg.RosPack().get_path('sr_gui_controller_tuner'), 'uis', 'SrGuiControllerTuner.ui')
00210 loadUi(ui_file, self._widget)
00211 self._widget.setObjectName('SrControllerTunerUi')
00212 context.add_widget(self._widget)
00213
00214
00215 self.move_threads = []
00216
00217
00218 self.controllers_in_dropdown = []
00219
00220
00221
00222 self.ctrl_widgets = {}
00223
00224
00225 self.sr_controller_tuner_app_ = SrControllerTunerApp( os.path.join(rospkg.RosPack().get_path('sr_gui_controller_tuner'), 'data', 'controller_settings.xml') )
00226
00227
00228 self.on_btn_refresh_ctrl_clicked_()
00229
00230 self._widget.btn_refresh_ctrl.pressed.connect(self.on_btn_refresh_ctrl_clicked_)
00231 self._widget.dropdown_ctrl.currentIndexChanged.connect(self.on_changed_controller_type_)
00232
00233 self._widget.btn_save_selected.pressed.connect(self.on_btn_save_selected_clicked_)
00234 self._widget.btn_save_all.pressed.connect(self.on_btn_save_all_clicked_)
00235 self._widget.btn_select_file_path.pressed.connect(self.on_btn_select_file_path_clicked_)
00236
00237 self._widget.btn_set_selected.pressed.connect(self.on_btn_set_selected_clicked_)
00238 self._widget.btn_set_all.pressed.connect(self.on_btn_set_all_clicked_)
00239
00240 self._widget.btn_load.pressed.connect(self.on_btn_load_clicked_)
00241 self._widget.btn_stop_mvts.pressed.connect(self.on_btn_stop_mvts_clicked_)
00242
00243 def on_btn_plot_pressed_(self, joint_name, btn):
00244 plot_thread = PlotThread(btn, joint_name, self.controller_type)
00245 plot_thread.start()
00246
00247 def on_btn_move_pressed_(self, joint_name, btn):
00248 move_thread = MoveThread(btn, joint_name, self.controller_type)
00249 move_thread.start()
00250 self.move_threads.append(move_thread)
00251
00252 def on_changed_controller_type_(self, index = None):
00253 """
00254 When controller type is changed clear the chosen file path and refresh the tree with the controller settings
00255 """
00256 if index == None:
00257 return
00258 self.reset_file_path()
00259 self.refresh_controller_tree_( self.controllers_in_dropdown[index] )
00260
00261 def reset_file_path(self):
00262 """
00263 Clear the chosen file path and disable the save button until user selects another path
00264 """
00265 self._widget.txt_file_path.setText("")
00266
00267 self._widget.btn_load.setEnabled(False)
00268
00269
00270
00271 self._widget.btn_save_all.setEnabled(False)
00272 self._widget.btn_save_selected.setEnabled(False)
00273
00274 def on_btn_select_file_path_clicked_(self):
00275 """
00276 Perform controller tuning and save settings to user specified file
00277 sr_config stack must be installed
00278 """
00279 path_to_config = "~"
00280 try:
00281 path_to_config = os.path.join(rospkg.RosPack().get_path('sr_ethercat_hand_config'))
00282 except:
00283 rospy.logwarn("couldn't find the sr_ethercat_hand_config package, do you have the sr_config stack installed?")
00284
00285
00286 config_subdir = rospy.get_param('config_dir', '')
00287 subpath = "/controls/host/" + config_subdir
00288 if self.sr_controller_tuner_app_.edit_only_mode:
00289 filter_files = "*.yaml"
00290 else:
00291 if self.controller_type == "Motor Force":
00292 filter_files = "*controllers.yaml"
00293 else:
00294 if self.sr_controller_tuner_app_.control_mode == "PWM":
00295 filter_files = "*s_PWM.yaml"
00296 else:
00297 filter_files = "*s.yaml"
00298
00299 if self.controller_type == "Motor Force":
00300 filter_files = "Config (*motor"+filter_files+")"
00301 subpath = "/controls/motors/" + config_subdir
00302 elif self.controller_type == "Position":
00303 filter_files = "Config (*position_controller"+filter_files+")"
00304 elif self.controller_type == "Muscle Position":
00305 filter_files = "Config (*muscle_joint_position_controller"+filter_files+")"
00306 elif self.controller_type == "Velocity":
00307 filter_files = "Config (*velocity_controller"+filter_files+")"
00308 elif self.controller_type == "Mixed Position/Velocity":
00309 filter_files = "Config (*position_velocity"+filter_files+")"
00310 elif self.controller_type == "Effort":
00311 filter_files = "Config (*effort_controller"+filter_files+")"
00312
00313 path_to_config += subpath
00314
00315 filename, _ = QFileDialog.getOpenFileName(self._widget.tree_ctrl_settings, self._widget.tr('Save Controller Settings'),
00316 self._widget.tr(path_to_config),
00317 self._widget.tr(filter_files))
00318
00319 if filename == "":
00320 return
00321
00322 self.file_to_save = filename
00323
00324 self._widget.txt_file_path.setText(filename)
00325
00326 self._widget.btn_load.setEnabled(True)
00327
00328
00329
00330 self._widget.btn_save_all.setEnabled(True)
00331 self._widget.btn_save_selected.setEnabled(True)
00332
00333 def on_btn_load_clicked_(self):
00334 """
00335 reload the parameters in rosparam, then refresh the tree widget
00336 """
00337 paramlist = rosparam.load_file( self.file_to_save )
00338 for params,ns in paramlist:
00339 rosparam.upload_params(ns, params)
00340
00341 self.refresh_controller_tree_( self.controllers_in_dropdown[self._widget.dropdown_ctrl.currentIndex()] )
00342
00343 def on_btn_save_selected_clicked_(self):
00344 """
00345 Save only the selected controllers
00346 """
00347 selected_items = self._widget.tree_ctrl_settings.selectedItems()
00348
00349 if len( selected_items ) == 0:
00350 QMessageBox.warning(self._widget.tree_ctrl_settings, "Warning", "No motors selected.")
00351
00352 for it in selected_items:
00353 if str(it.text(1)) != "":
00354 self.save_controller( str(it.text(1)) )
00355
00356 def on_btn_save_all_clicked_(self):
00357 """
00358 Save all controllers
00359 """
00360 for motor in self.ctrl_widgets.keys():
00361 self.save_controller( motor )
00362
00363 def on_btn_set_selected_clicked_(self):
00364 """
00365 Sets the current values for selected controllers using the ros service.
00366 """
00367 selected_items = self._widget.tree_ctrl_settings.selectedItems()
00368
00369 if len( selected_items ) == 0:
00370 QMessageBox.warning(self._widget.tree_ctrl_settings, "Warning", "No motors selected.")
00371
00372 for it in selected_items:
00373 if str(it.text(1)) != "":
00374 self.set_controller( str(it.text(1)) )
00375
00376 def on_btn_set_all_clicked_(self):
00377 """
00378 Sets the current values for all controllers using the ros service.
00379 """
00380 for motor in self.ctrl_widgets.keys():
00381 self.set_controller( motor )
00382
00383 def on_btn_refresh_ctrl_clicked_(self):
00384 """
00385 Calls refresh_controller_tree_ after preparing widgets
00386 """
00387 ctrls = self.sr_controller_tuner_app_.get_ctrls()
00388 self.sr_controller_tuner_app_.refresh_control_mode()
00389 self.controllers_in_dropdown = []
00390 self._widget.dropdown_ctrl.clear()
00391 for ctrl in ctrls:
00392 self._widget.dropdown_ctrl.addItem(ctrl)
00393 self.controllers_in_dropdown.append(ctrl)
00394
00395 self.refresh_controller_tree_()
00396
00397 def read_settings(self, joint_name):
00398 """
00399 retrive settings for joint with given name
00400 """
00401 dict_of_widgets = self.ctrl_widgets[joint_name]
00402
00403 settings = {}
00404 for item in dict_of_widgets.items():
00405 if item[0] == "sign":
00406 if item[1].checkState() == Qt.Checked:
00407 settings["sign"] = 1
00408 else:
00409 settings["sign"] = 0
00410 else:
00411 try:
00412 settings[item[0]] = item[1].value()
00413 except AttributeError:
00414 pass
00415
00416 return settings
00417
00418 def set_controller(self, joint_name):
00419 """
00420 Sets the current values for the given controller using the ros service.
00421 """
00422 settings = self.read_settings( joint_name )
00423
00424
00425 success = self.sr_controller_tuner_app_.set_controller(joint_name, self.controller_type, settings)
00426 if success == False:
00427 if self.controller_type == "Motor Force":
00428 QMessageBox.warning(self._widget.tree_ctrl_settings, "Warning", "Failed to set the PID values for joint "+ joint_name +". This won't work for Gazebo controllers as there are no force controllers yet.")
00429 else:
00430 QMessageBox.warning(self._widget.tree_ctrl_settings, "Warning", "Failed to set the PID values for joint "+ joint_name +".")
00431
00432
00433 def save_controller(self, joint_name):
00434 """
00435 Saves the current values for the given controller using the ros service.
00436 """
00437 settings = self.read_settings( joint_name )
00438
00439
00440 self.sr_controller_tuner_app_.save_controller(joint_name, self.controller_type, settings, self.file_to_save)
00441
00442
00443 def refresh_controller_tree_(self, controller_type = "Motor Force"):
00444 """
00445 Get the controller settings and their ranges and display them in the tree.
00446 Buttons and plots will be added unless in edit_only mode.
00447 Move button will be added if controller is position type
00448 Buttons "set all" "set selected" and "stop movements" are disabled in edit_only_mode
00449 Controller settings must exist for every motor of every finger in the yaml file.
00450 """
00451
00452 if self.sr_controller_tuner_app_.edit_only_mode:
00453 self._widget.btn_set_selected.setEnabled(False)
00454 self._widget.btn_set_all.setEnabled(False)
00455 self._widget.btn_stop_mvts.setEnabled(False)
00456 else:
00457 self._widget.btn_set_selected.setEnabled(True)
00458 self._widget.btn_set_all.setEnabled(True)
00459 self._widget.btn_stop_mvts.setEnabled(True)
00460
00461 self.controller_type = controller_type
00462 ctrl_settings = self.sr_controller_tuner_app_.get_controller_settings( controller_type )
00463
00464 self._widget.tree_ctrl_settings.clear()
00465 self._widget.tree_ctrl_settings.setColumnCount(ctrl_settings.nb_columns)
00466
00467 tmp_headers = []
00468 for header in ctrl_settings.headers:
00469 tmp_headers.append( header["name"] )
00470 self._widget.tree_ctrl_settings.setHeaderLabels( tmp_headers )
00471
00472 hand_item = QTreeWidgetItem(ctrl_settings.hand_item)
00473 self._widget.tree_ctrl_settings.addTopLevelItem(hand_item)
00474 for index_finger,finger_settings in enumerate(ctrl_settings.fingers):
00475 finger_item = QTreeWidgetItem( hand_item, finger_settings )
00476 self._widget.tree_ctrl_settings.addTopLevelItem(finger_item)
00477 for motor_settings in ctrl_settings.motors[index_finger]:
00478 motor_name = motor_settings[1]
00479
00480 motor_item = QTreeWidgetItem( finger_item, motor_settings )
00481 self._widget.tree_ctrl_settings.addTopLevelItem(motor_item)
00482
00483 parameter_values = self.sr_controller_tuner_app_.load_parameters( controller_type, motor_name )
00484 if parameter_values != -1:
00485
00486 self.ctrl_widgets[ motor_name ] = {}
00487
00488
00489 if not self.sr_controller_tuner_app_.edit_only_mode:
00490
00491 frame_buttons = QFrame()
00492 layout_buttons = QHBoxLayout()
00493 btn_plot = QPushButton("Plot")
00494 self.ctrl_widgets[ motor_name ]["btn_plot"] = btn_plot
00495 self.ctrl_widgets[ motor_name ]["btn_plot"].clicked.connect(partial(self.on_btn_plot_pressed_, motor_name, self.ctrl_widgets[ motor_name ]["btn_plot"]))
00496 layout_buttons.addWidget(btn_plot)
00497
00498 if self.controller_type in ["Position", "Muscle Position", "Mixed Position/Velocity"]:
00499
00500 btn_move = QPushButton("Move")
00501 self.ctrl_widgets[ motor_name ]["btn_move"] = btn_move
00502 self.ctrl_widgets[ motor_name ]["btn_move"].clicked.connect(partial(self.on_btn_move_pressed_,motor_name, self.ctrl_widgets[ motor_name ]["btn_move"]))
00503 layout_buttons.addWidget(btn_move)
00504 frame_buttons.setLayout(layout_buttons)
00505
00506 self._widget.tree_ctrl_settings.setItemWidget( motor_item, 0, frame_buttons )
00507
00508 for index_item,item in enumerate(ctrl_settings.headers):
00509 if item["type"] == "Bool":
00510 check_box = QCheckBox()
00511
00512 if parameter_values["sign"] == 1.0:
00513 check_box.setChecked(True)
00514
00515 check_box.setToolTip("Check if you want a negative sign\n(if the motor is being driven\n the wrong way around).")
00516 self._widget.tree_ctrl_settings.setItemWidget( motor_item, index_item, check_box )
00517
00518 self.ctrl_widgets[ motor_name ]["sign"] = check_box
00519
00520
00521 if item["type"] == "Int":
00522 spin_box = QSpinBox()
00523 spin_box.setRange(int( item["min"] ), int( item["max"] ))
00524
00525 param_name = item["name"].lower()
00526 spin_box.setValue( int(parameter_values[ param_name ] ) )
00527 self.ctrl_widgets[ motor_name ][param_name] = spin_box
00528
00529 self._widget.tree_ctrl_settings.setItemWidget( motor_item, index_item, spin_box )
00530
00531 if item["type"] == "Float":
00532 spin_box = QDoubleSpinBox()
00533 spin_box.setRange( -65535.0, 65535.0 )
00534 spin_box.setDecimals(3)
00535
00536 param_name = item["name"].lower()
00537 spin_box.setValue(float(parameter_values[param_name]))
00538 self.ctrl_widgets[ motor_name ][param_name] = spin_box
00539
00540 self._widget.tree_ctrl_settings.setItemWidget( motor_item, index_item, spin_box )
00541
00542 motor_item.setExpanded(True)
00543 else:
00544 motor_item.setText(1, "parameters not found - controller tuning disabled")
00545 finger_item.setExpanded(True)
00546 hand_item.setExpanded(True)
00547
00548 for col in range(0, self._widget.tree_ctrl_settings.columnCount()):
00549 self._widget.tree_ctrl_settings.resizeColumnToContents(col)
00550
00551
00552 def on_btn_stop_mvts_clicked_(self):
00553 for move_thread in self.move_threads:
00554 move_thread.__del__()
00555 self.move_threads = []
00556
00557
00558
00559
00560
00561 def _unregisterPublisher(self):
00562 if self._publisher is not None:
00563 self._publisher.unregister()
00564 self._publisher = None
00565
00566 def shutdown_plugin(self):
00567 self._unregisterPublisher()
00568
00569 def save_settings(self, global_settings, perspective_settings):
00570 pass
00571
00572 def restore_settings(self, global_settings, perspective_settings):
00573 pass