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
00008 class JointParams:
00009
00010 def __init__(self, whicharm):
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
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
00029 class JTCartesianTaskParams:
00030
00031 def __init__(self, whicharm, task_controller = 0):
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
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
00052 self.set_params()
00053 self.set_gains()
00054
00055
00056
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
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
00086 def set_params(self):
00087
00088
00089
00090
00091 self.set_gains()
00092
00093
00094
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
00130 class JTCartesianParams:
00131
00132 def __init__(self, whicharm):
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
00149 self.set_params()
00150
00151
00152
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
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
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