pr2_gripper_reactive_approach: controller

Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('pr2_gripper_reactive_approach')
00002 import rospy
00003 import pdb
00004 from object_manipulation_msgs.msg import CartesianGains
00005 from std_msgs.msg import Float64MultiArray
00006 
00007 ##load the params for the joint controllers onto the param server
00008 class JointParams:
00009 
00010   def __init__(self, whicharm): #whicharm is 'r' or 'l'
00011     self.whicharm = whicharm
00012     self.default_p = [800., 800., 200., 100., 500., 400., 400.]
00013     self.default_d = [10., 10., 3., 3., 6., 4., 4.]
00014     joint_names = ['_shoulder_pan_joint', '_shoulder_lift_joint',
00015                      '_upper_arm_roll_joint', '_elbow_flex_joint',
00016                      '_forearm_roll_joint', '_wrist_flex_joint',
00017                      '_wrist_roll_joint']
00018     self.controller_name = whicharm+"_arm_controller"
00019     self.joint_names = [whicharm+joint_name for joint_name in joint_names]
00020 
00021   ##set gains to some fraction of the default
00022   def set_gains_fraction(self, fraction):
00023     for i in range(7):
00024       rospy.set_param(self.controller_name+'/gains/%s/p'%self.joint_names[i], self.default_p[i]*fraction)
00025       rospy.set_param(self.controller_name+'/gains/%s/d'%self.joint_names[i], self.default_d[i]*fraction)
00026 
00027 
00028 ##load the params for the Jtranspose Cartesian controllers (jt_task_controller) onto the param server
00029 class JTCartesianTaskParams:
00030 
00031   def __init__(self, whicharm, task_controller = 0): #whicharm is 'r' or 'l'
00032     self.whicharm = whicharm
00033     self.rot_d = [1.2]*3
00034     self.rot_p = [80.0]*3
00035     self.trans_d = [15.0]*3
00036     self.trans_p = [800.0]*3
00037     self.cname = self.whicharm+'_cart'
00038     self.gains_topic = self.cname+'/gains'
00039     self.tip_frame = self.whicharm+"_wrist_roll_link"
00040     self.base_frame = "/base_link"
00041     self.gains_frame = "/base_link"
00042     self.task_controller = task_controller
00043     print "task_controller: ", self.task_controller
00044 
00045     #publisher for setting gains
00046     if self.task_controller:
00047       self.set_gains_pub = rospy.Publisher(self.gains_topic, CartesianGains)
00048     else:
00049       self.set_gains_pub = rospy.Publisher(self.gains_topic, Float64MultiArray)
00050 
00051     #set the default values on the parameter server and send the default gains
00052     self.set_params()
00053     self.set_gains()
00054 
00055 
00056   ##set the parameter values back to their defaults
00057   def set_params_to_defaults(self, set_tip_frame = 1, set_params_on_server = 1, set_gains = 1):
00058     self.rot_d = [1.2]*3
00059     self.rot_p = [80.0]*3
00060     self.trans_d = [15.0]*3
00061     self.trans_p = [800.0]*3
00062     if set_tip_frame:
00063       self.tip_frame = self.whicharm+"_wrist_roll_link"
00064     if set_params_on_server:
00065       self.set_params()
00066     elif set_gains:
00067       self.set_gains()
00068 
00069 
00070   ##set the gains and max vels and accs lower, so the arm moves more gently/slowly
00071   def set_params_to_gentle(self, set_tip_frame = 1, set_params_on_server = 1, set_gains = 1):
00072     self.rot_d = [0.6]*3
00073     self.rot_p = [40.0]*3
00074     self.trans_d = [7.5]*3
00075     self.trans_p = [400.0]*3
00076     if set_tip_frame:
00077       self.tip_frame = self.whicharm+'_wrist_roll_link'
00078       rospy.loginfo("setting tip frame back to wrist_roll_link")
00079     if set_params_on_server:
00080       self.set_params()
00081     elif set_gains:
00082       self.set_gains()
00083 
00084 
00085   ##set the current parameter values on the parameter server
00086   def set_params(self):
00087     #disabled until controller's changing of tip frame is fixed (https://code.ros.org/trac/wg-ros-pkg/ticket/5134)
00088     #rospy.loginfo("setting Cartesian controller parameters")
00089     #rospy.set_param(self.cname+'/root_name', self.base_frame)
00090     #rospy.set_param(self.cname+'/tip_name', self.tip_frame)
00091     self.set_gains()
00092 
00093     
00094   ##set the current controller gains by calling the setGains service
00095   def set_gains(self, trans_p = None, trans_d = None, rot_p = None, rot_d = None, frame_id = None):
00096     if self.task_controller:
00097       gains = CartesianGains()
00098       if frame_id != None:
00099         gains.header.frame_id = frame_id
00100       else:
00101         gains.header.frame_id = self.gains_frame
00102     else:
00103       gains = Float64MultiArray()
00104     gains_array = [0]*12
00105     if trans_p != None:
00106         gains_array[0:3] = trans_p
00107     else:
00108         gains_array[0:3] = self.trans_p
00109     if rot_p != None:
00110         gains_array[3:6] = rot_p
00111     else:
00112         gains_array[3:6] = self.rot_p
00113     if trans_d != None:
00114         gains_array[6:9] = trans_d
00115     else:
00116         gains_array[6:9] = self.trans_d
00117     if rot_d != None:
00118         gains_array[9:12] = rot_d
00119     else:
00120         gains_array[9:12] = self.rot_d
00121     if self.task_controller:
00122       gains.gains = gains_array
00123     else:
00124       gains.data = gains_array
00125     self.set_gains_pub.publish(gains)
00126 
00127 
00128 
00129 ##load the params for the Jtranspose Cartesian controllers (simple_Jtranspose_controller) onto the param server
00130 class JTCartesianParams:
00131 
00132   def __init__(self, whicharm): #whicharm is 'r' or 'l'
00133     self.whicharm = whicharm
00134     self.max_vel_trans=.05 
00135     self.max_vel_rot=.1
00136     self.max_acc_trans=0.2
00137     self.max_acc_rot=0.3
00138     self.pose_fb_trans_p=400.
00139     self.pose_fb_trans_d=6.
00140     self.pose_fb_rot_p=40.
00141     self.pose_fb_rot_d=0.
00142 
00143     self.tcname = self.whicharm+'_arm_cartesian_trajectory_controller'
00144     self.pcname = self.whicharm+'_arm_cartesian_pose_controller'
00145     self.tip_frame = self.whicharm+"_wrist_roll_link"
00146     self.base_frame = "base_link"
00147 
00148     #set the default values on the parameter server
00149     self.set_params()
00150 
00151 
00152   ##set the parameter values back to their defaults
00153   def set_params_to_defaults(self, set_tip_frame = 1, set_params_on_server = 1):
00154     self.max_vel_trans=.15 
00155     self.max_vel_rot=.3
00156     self.max_acc_trans=0.4
00157     self.max_acc_rot=0.6
00158     self.pose_fb_trans_p=2000.
00159     self.pose_fb_trans_d=30.
00160     self.pose_fb_rot_p=200.
00161     self.pose_fb_rot_d=0.
00162     if set_tip_frame:
00163       self.tip_frame = self.whicharm+"_wrist_roll_link"
00164     if set_params_on_server:
00165       self.set_params()
00166 
00167 
00168   ##set the gains and max vels and accs lower, so the arm moves more gently/slowly
00169   def set_params_to_gentle(self, set_tip_frame = 1, set_params_on_server = 1):
00170     self.max_vel_trans=.05 
00171     self.max_vel_rot=.1
00172     self.max_acc_trans=0.2
00173     self.max_acc_rot=0.3
00174     self.pose_fb_trans_p=400.
00175     self.pose_fb_trans_d=6.
00176     self.pose_fb_rot_p=40.
00177     self.pose_fb_rot_d=0.
00178     if set_tip_frame:
00179       self.tip_frame = self.whicharm+'_wrist_roll_link'
00180       rospy.loginfo("setting tip frame back to wrist_roll_link")
00181     if set_params_on_server:
00182       self.set_params()
00183 
00184 
00185   ##set the current parameter values on the parameter server
00186   def set_params(self):
00187     rospy.loginfo("setting Cartesian controller parameters")
00188     rospy.set_param(self.tcname+'/type', "pr2_manipulation_controllers/CartesianTrajectoryController")
00189     rospy.set_param(self.tcname+'/root_name', self.base_frame)
00190     rospy.set_param(self.tcname+'/tip_name', self.tip_frame)
00191     rospy.set_param(self.tcname+'/output', self.pcname)
00192 
00193     rospy.set_param(self.pcname+'/type', "robot_mechanism_controllers/CartesianPoseController")
00194     rospy.set_param(self.pcname+'/root_name', self.base_frame)
00195     rospy.set_param(self.pcname+'/tip_name', self.tip_frame)
00196     rospy.set_param(self.pcname+'/output', self.whicharm+'_arm_cartesian_twist_controller')
00197 
00198     rospy.set_param(self.tcname+'/max_vel_trans', self.max_vel_trans)
00199     rospy.set_param(self.tcname+'/max_vel_rot', self.max_vel_rot)
00200     rospy.set_param(self.tcname+'/max_acc_trans', self.max_acc_trans)
00201     rospy.set_param(self.tcname+'/max_acc_rot', self.max_acc_rot)
00202     
00203     rospy.set_param(self.pcname+'/fb_trans/p', self.pose_fb_trans_p)
00204     rospy.set_param(self.pcname+'/fb_trans_d', self.pose_fb_trans_d)
00205     rospy.set_param(self.pcname+'/fb_rot/p', self.pose_fb_rot_p)
00206     rospy.set_param(self.pcname+'/fb_rot/d', self.pose_fb_rot_d)
00207 
00208     #tip_frame = rospy.get_param(self.tcname+'/tip_name')