sr_controller_tuner.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2011 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 #
00018 
00019 import roslib
00020 roslib.load_manifest('sr_gui_controller_tuner')
00021 import rospy
00022 
00023 from xml.etree import ElementTree as ET
00024 from pr2_mechanism_msgs.srv import ListControllers
00025 
00026 from sr_robot_msgs.srv import ForceController, SetEffortControllerGains, SetMixedPositionVelocityPidGains, SetPidGains
00027 from sr_gui_controller_tuner.pid_loader_and_saver import PidLoader, PidSaver
00028 import unicodedata
00029 
00030 class CtrlSettings(object):
00031     """
00032     """
00033 
00034     def __init__(self, xml_path, controller_type):
00035         """
00036         """
00037         self.headers = []
00038 
00039         #open and parses the xml config file
00040         xml_file = open(xml_path)
00041         xml_tree = ET.parse(xml_file)
00042         xml_file.close()
00043 
00044         #read the settings from the xml file
00045         ctrl_tree = None
00046         for ctrl in xml_tree.findall("controller"):
00047             ctrl_name = ctrl.attrib['name']
00048             if ctrl_name == controller_type:
00049                 ctrl_tree = ctrl
00050                 break
00051 
00052         if ctrl_tree == None:
00053             rospy.logerr("Couldn't find the settings for the controller " + controller_type)
00054 
00055         #read the headers settings
00056         xml_headers = ctrl.find("headers")
00057         for header in xml_headers.findall("item"):
00058             self.headers.append( header.attrib )
00059 
00060         self.nb_columns = len(self.headers)
00061 
00062         self.hand_item = ["Hand"]
00063         for i in range(0, self.nb_columns - 1):
00064             self.hand_item.append("")
00065 
00066         #read the fingers and the motors from the xml file
00067         self.fingers = []
00068         self.motors = []
00069         all_fingers = xml_tree.find("fingers")
00070         for finger in all_fingers.findall("finger"):
00071             finger_row = [ finger.attrib['name'] ]
00072             for i in range(0, self.nb_columns - 1):
00073                 finger_row.append("")
00074             self.fingers.append( finger_row )
00075 
00076             motors_for_finger = []
00077             for motor in finger.findall("motor"):
00078                 motor_row = [ "", motor.attrib['name'] ]
00079                 for i in range(0, self.nb_columns - 2):
00080                     motor_row.append("")
00081                 motors_for_finger.append( motor_row )
00082 
00083             self.motors.append( motors_for_finger )
00084 
00085 
00086 class SrControllerTunerApp(object):
00087     """
00088     """
00089     CONTROLLER_MANAGER_DETECTION_TIMEOUT = 3.0
00090     
00091     def __init__(self, xml_path):
00092         """
00093         """
00094         self.xml_path = xml_path
00095         self.all_controller_types = ["Motor Force", "Position", "Velocity",
00096                                      "Mixed Position/Velocity", "Effort"]
00097         self.pid_loader = PidLoader()
00098         
00099         self.edit_only_mode = False
00100         self.control_mode = "FORCE"
00101         
00102 
00103     def get_ctrls(self):
00104         #return ["Motor Force", "Position"]
00105         #
00106         running_ctrls = []
00107 
00108         try:
00109             rospy.wait_for_service('/pr2_controller_manager/list_controllers', self.CONTROLLER_MANAGER_DETECTION_TIMEOUT)
00110         
00111             controllers = rospy.ServiceProxy('/pr2_controller_manager/list_controllers', ListControllers)
00112             resp = None
00113             try:
00114                 resp = controllers()
00115             except rospy.ServiceException, e:
00116                 rospy.logerr( "Service did not process request: %s"%str(e) )
00117             
00118             running_ctrls.append("Motor Force")
00119             if resp != None:
00120                 for state,controller in zip(resp.state, resp.controllers):
00121                     if state == "running":
00122                         split = controller.split("_")
00123                         ctrl_type_tmp = split[2]
00124                         for defined_ctrl_type in self.all_controller_types:
00125                             if ctrl_type_tmp.lower() in defined_ctrl_type.lower():
00126                                 running_ctrls.append(defined_ctrl_type)
00127                                 self.edit_only_mode = False
00128                                 return running_ctrls
00129             rospy.loginfo( "No controllers currently running" )
00130             rospy.loginfo( "Running controller tuner in edit-only mode" )
00131             self.edit_only_mode = True
00132             del running_ctrls[:]
00133             #In edit_only_mode all the controllers are available for edition
00134             for defined_ctrl_type in self.all_controller_types:
00135                 running_ctrls.append(defined_ctrl_type)
00136         except rospy.ROSException, e:
00137             rospy.loginfo( "Controller manager not running: %s"%str(e) )
00138             rospy.loginfo( "Running controller tuner in edit-only mode" )
00139             self.edit_only_mode = True
00140             #In edit_only_mode all the controllers are available for edition
00141             for defined_ctrl_type in self.all_controller_types:
00142                 running_ctrls.append(defined_ctrl_type)
00143         
00144         return running_ctrls
00145     
00146     def refresh_control_mode(self):
00147         self.control_mode = rospy.get_param('/realtime_loop/default_control_mode', 'FORCE')
00148 
00149     def get_controller_settings( self, controller_type ):
00150         """
00151         Parses a file containing the controller settings
00152         and their min and max values, and return them.
00153         """
00154         ctrl_settings = CtrlSettings(self.xml_path, controller_type)
00155 
00156         return ctrl_settings
00157 
00158     def load_parameters(self, controller_type, joint_name):
00159         """
00160         Load the parameters from the yaml file.
00161         """
00162         param_name = ""
00163         if controller_type == "Motor Force":
00164             param_name = "/"+ joint_name.lower() +"/pid"
00165         elif controller_type == "Position":
00166             param_name =  "/sh_"+ joint_name.lower()+"_position_controller/pid"
00167         elif controller_type == "Velocity":
00168             param_name =  "/sh_"+ joint_name.lower()+"_velocity_controller/pid"
00169         elif controller_type == "Mixed Position/Velocity":
00170             param_name = ["/sh_"+ joint_name.lower()+"_mixed_position_velocity_controller/position_pid",
00171                           "/sh_"+ joint_name.lower()+"_mixed_position_velocity_controller/velocity_pid" ]
00172         elif controller_type == "Effort":
00173             param_name =  "/sh_"+ joint_name.lower()+"_effort_controller"
00174 
00175         return self.pid_loader.get_settings( param_name )
00176 
00177     def set_controller(self, joint_name, controller_type, controller_settings):
00178         """
00179         Sets the controller calling the proper service with the correct syntax.
00180         """
00181         pid_service = None
00182         service_name = ""
00183         if controller_type == "Motor Force":
00184             #/realtime_loop/change_force_PID_FFJ0
00185             service_name =  "/realtime_loop/change_force_PID_"+joint_name.upper()
00186             pid_service = rospy.ServiceProxy(service_name, ForceController)
00187 
00188         elif controller_type == "Position":
00189             #/sh_ffj3_position_controller/set_gains
00190             service_name =  "/sh_"+joint_name.lower()+"_position_controller/set_gains"
00191             pid_service = rospy.ServiceProxy(service_name, SetPidGains)
00192 
00193         elif controller_type == "Velocity":
00194             #/sh_ffj3_velocity_controller/set_gains
00195             service_name =  "/sh_"+joint_name.lower()+"_velocity_controller/set_gains"
00196             pid_service = rospy.ServiceProxy(service_name, SetPidGains)
00197 
00198         elif controller_type == "Mixed Position/Velocity":
00199             #/sh_ffj3_mixed_position_velocity_controller/set_gains
00200             service_name =  "/sh_"+joint_name.lower()+"_mixed_position_velocity_controller/set_gains"
00201             pid_service = rospy.ServiceProxy(service_name, SetMixedPositionVelocityPidGains)
00202 
00203         elif controller_type == "Effort":
00204             #/sh_ffj3_effort_controller/set_gains
00205             service_name =  "/sh_"+joint_name.lower()+"_effort_controller/set_gains"
00206             pid_service = rospy.ServiceProxy(service_name, SetEffortControllerGains)
00207 
00208         else:
00209             rospy.logerr( "", controller_type, " is not a recognized controller type." )
00210 
00211         contrlr_settings_converted = {}
00212         for param in controller_settings.items():
00213             contrlr_settings_converted[ param[0] ] = float(param[1])
00214 
00215         if controller_type == "Motor Force":
00216             try:
00217                 pid_service(int(contrlr_settings_converted["max_pwm"]),
00218                             int(contrlr_settings_converted["sgleftref"]),
00219                             int(contrlr_settings_converted["sgrightref"]),
00220                             int(contrlr_settings_converted["f"]),
00221                             int(contrlr_settings_converted["p"]), int(contrlr_settings_converted["i"]),
00222                             int(contrlr_settings_converted["d"]), int(contrlr_settings_converted["imax"]),
00223                             int(contrlr_settings_converted["deadband"]), int(contrlr_settings_converted["sign"]) )
00224             except:
00225                 return False
00226 
00227         elif controller_type == "Position":
00228             try:
00229                 pid_service(float(contrlr_settings_converted["p"]), float(contrlr_settings_converted["i"]),
00230                             float(contrlr_settings_converted["d"]), float(contrlr_settings_converted["i_clamp"]),
00231                             float(contrlr_settings_converted["max_force"]), float(contrlr_settings_converted["position_deadband"]),
00232                             int(contrlr_settings_converted["friction_deadband"]) )
00233             except:
00234                 return False
00235 
00236         elif controller_type == "Velocity":
00237             try:
00238                 pid_service(float(contrlr_settings_converted["p"]), float(contrlr_settings_converted["i"]),
00239                             float(contrlr_settings_converted["d"]), float(contrlr_settings_converted["i_clamp"]),
00240                             float(contrlr_settings_converted["max_force"]), float(contrlr_settings_converted["velocity_deadband"]),
00241                             int(contrlr_settings_converted["friction_deadband"]) )
00242             except:
00243                 return False
00244 
00245         elif controller_type == "Mixed Position/Velocity":
00246             try:
00247                 pid_service(float(contrlr_settings_converted["pos/p"]), float(contrlr_settings_converted["pos/i"]),
00248                             float(contrlr_settings_converted["pos/d"]), float(contrlr_settings_converted["pos/i_clamp"]),
00249                             float(contrlr_settings_converted["pos/min_velocity"]), float(contrlr_settings_converted["pos/max_velocity"]),
00250                             float(contrlr_settings_converted["pos/position_deadband"]),
00251                             float(contrlr_settings_converted["vel/p"]), float(contrlr_settings_converted["vel/i"]),
00252                             float(contrlr_settings_converted["vel/d"]), float(contrlr_settings_converted["vel/i_clamp"]),
00253                             float(contrlr_settings_converted["vel/max_force"]),
00254                             int(contrlr_settings_converted["vel/friction_deadband"]) )
00255             except:
00256                 return False
00257 
00258         elif controller_type == "Effort":
00259             try:
00260                 pid_service(int(contrlr_settings_converted["max_force"]), int(contrlr_settings_converted["friction_deadband"]) )
00261             except:
00262                 return False
00263         else:
00264             rospy.logerr( "", controller_type, " is not a recognized controller type." )
00265             return False
00266         return True
00267 
00268 
00269     def save_controller(self, joint_name, controller_type, controller_settings, filename):
00270         """
00271         Sets the controller calling the proper service with the correct syntax.
00272         """
00273         param_name = []
00274         if controller_type == "Motor Force":
00275             param_name = [""+joint_name.lower() ,"pid"]
00276         elif controller_type == "Position":
00277             param_name =  ["sh_"+joint_name.lower()+"_position_controller" , "pid"]
00278         elif controller_type == "Velocity":
00279             param_name =  ["sh_"+joint_name.lower()+"_velocity_controller" , "pid"]
00280         elif controller_type == "Mixed Position/Velocity":
00281             param_name =  ["sh_"+joint_name.lower()+"_mixed_position_velocity_controller" , "pid"]
00282         elif controller_type == "Effort":
00283             param_name =  ["sh_"+joint_name.lower()+"_effort_controller"]
00284 
00285         pid_saver = PidSaver(filename)
00286         pid_saver.save_settings(param_name, controller_settings)


sr_gui_controller_tuner
Author(s): Ugo Cupcic
autogenerated on Fri Jan 3 2014 12:04:50