change_joint_controller_gains.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('pr2_object_manipulation_launch')
00004 import rospy
00005 
00006 from pr2_mechanism_msgs.srv import SwitchController, LoadController, UnloadController, ListControllers
00007 
00008 #load the params for the joint controllers onto the param server
00009 class JointParamChanger:
00010 
00011     def __init__(self, whicharm): #whicharm is 'r' or 'l'
00012         self.whicharm = whicharm
00013         self.default_p = [2400., 1200., 1000., 700., 300., 300., 300.]
00014         self.default_d = [18., 10., 6., 4., 6., 4., 4.]
00015         self.default_i = [800., 700., 600., 450., 300., 300., 300.]
00016         self.default_i_clamp = [4., 4., 4., 4., 2., 2., 2.]
00017         joint_names = ['_shoulder_pan_joint', '_shoulder_lift_joint',
00018                          '_upper_arm_roll_joint', '_elbow_flex_joint',
00019                          '_forearm_roll_joint', '_wrist_flex_joint',
00020                          '_wrist_roll_joint']
00021         self.controller_name = whicharm+"_arm_controller"
00022         self.joint_names = [whicharm+joint_name for joint_name in joint_names]
00023 
00024         rospy.loginfo("waiting for pr2_controller_manager services")
00025         rospy.wait_for_service('pr2_controller_manager/load_controller')
00026         rospy.wait_for_service('pr2_controller_manager/unload_controller')
00027         rospy.wait_for_service('pr2_controller_manager/switch_controller')
00028 
00029         self.load_controller_service = \
00030             rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)
00031         self.unload_controller_service = \
00032             rospy.ServiceProxy('pr2_controller_manager/unload_controller', UnloadController)
00033         self.switch_controller_service = \
00034             rospy.ServiceProxy('pr2_controller_manager/switch_controller', SwitchController)
00035         self.joint_controller = self.whicharm+'_arm_controller'
00036 
00037 
00038     #set gains to some fraction of the default
00039     def set_gains_fraction(self, fraction):
00040         for i in range(7):
00041             rospy.set_param(self.controller_name+'/gains/%s/p'%self.joint_names[i], self.default_p[i]*fraction)
00042             rospy.set_param(self.controller_name+'/gains/%s/d'%self.joint_names[i], self.default_d[i]*fraction)
00043             rospy.set_param(self.controller_name+'/gains/%s/i'%self.joint_names[i], self.default_i[i]*fraction)
00044             rospy.set_param(self.controller_name+'/gains/%s/i_clamp'%self.joint_names[i], self.default_i_clamp[i]*fraction)
00045             
00046 
00047     #unload and reload the joint controllers with the params on the server
00048     def reload_joint_controllers(self, fraction = 1.):
00049         self.set_gains_fraction(fraction)
00050 
00051         #unload the joint controller
00052         self.unload_joint_controllers()
00053   
00054         #re-load the joint controllers
00055         self.load_joint_controllers()
00056 
00057         #start the joint controllers
00058         self.start_joint_controllers()
00059 
00060 
00061     #load the joint controller with the current set of params on the param server
00062     def load_joint_controllers(self):
00063         success = self.load_controller_service(self.joint_controller)
00064         if not success:
00065             rospy.logerr("error in loading joint controller!")
00066         else:
00067             rospy.loginfo("loaded joint controller")
00068 
00069 
00070     #unload the joint controller 
00071     def unload_joint_controllers(self):
00072         self.stop_joint_controllers()
00073 
00074         success = self.unload_controller_service(self.joint_controller)
00075         if not success:
00076             rospy.logerr("error in unloading joint controller!")
00077         else:
00078             rospy.loginfo("unloaded joint controller")
00079 
00080 
00081     #stop controllers that are currently running
00082     def stop_joint_controllers(self):
00083         success = self.switch_controller_service([], [self.joint_controller,], 2)
00084         if success:
00085             rospy.loginfo("stopped joint controller successfully")    
00086         else:
00087             rospy.logerr("stopping joint controller failed")  
00088 
00089 
00090     #just start the joint controllers (need to be loaded already)
00091     def start_joint_controllers(self):
00092         success = self.switch_controller_service([self.joint_controller,], [], 2)
00093         if success:
00094             rospy.loginfo("started joint controller successfully")
00095         else:
00096             rospy.logerr("starting joint controller failed")
00097 
00098 
00099 if __name__ == "__main__":
00100     r_joint_params = JointParamChanger('r')
00101     l_joint_params = JointParamChanger('l')
00102 
00103     r_joint_params.reload_joint_controllers()
00104     l_joint_params.reload_joint_controllers()
00105 
00106 
00107 


pr2_object_manipulation_launch
Author(s): Matei Ciocarlie
autogenerated on Fri Jan 3 2014 11:56:01