$search
00001 00029 #include "sr_mechanism_controllers/srh_joint_effort_controller.hpp" 00030 #include "angles/angles.h" 00031 #include "pluginlib/class_list_macros.h" 00032 #include <sstream> 00033 #include <math.h> 00034 #include "sr_utilities/sr_math_utils.hpp" 00035 00036 #include <std_msgs/Float64.h> 00037 00038 PLUGINLIB_DECLARE_CLASS(sr_mechanism_controllers, SrhEffortJointController, controller::SrhEffortJointController, pr2_controller_interface::Controller) 00039 00040 using namespace std; 00041 00042 namespace controller { 00043 00044 SrhEffortJointController::SrhEffortJointController() 00045 : SrController() 00046 { 00047 } 00048 00049 SrhEffortJointController::~SrhEffortJointController() 00050 { 00051 sub_command_.shutdown(); 00052 } 00053 00054 bool SrhEffortJointController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name) 00055 { 00056 ROS_DEBUG(" --------- "); 00057 ROS_DEBUG_STREAM("Init: " << joint_name); 00058 00059 assert(robot); 00060 robot_ = robot; 00061 last_time_ = robot->getTime(); 00062 00063 //joint 0s 00064 if( joint_name.substr(3,1).compare("0") == 0) 00065 { 00066 has_j2 = true; 00067 std::string j1 = joint_name.substr(0,3) + "1"; 00068 std::string j2 = joint_name.substr(0,3) + "2"; 00069 ROS_DEBUG_STREAM("Joint 0: " << j1 << " " << j2); 00070 00071 joint_state_ = robot_->getJointState(j1); 00072 if (!joint_state_) 00073 { 00074 ROS_ERROR("SrhEffortJointController could not find joint named \"%s\"\n", 00075 j1.c_str()); 00076 return false; 00077 } 00078 00079 joint_state_2 = robot_->getJointState(j2); 00080 if (!joint_state_2) 00081 { 00082 ROS_ERROR("SrhEffortJointController could not find joint named \"%s\"\n", 00083 j2.c_str()); 00084 return false; 00085 } 00086 } 00087 else 00088 { 00089 has_j2 = false; 00090 joint_state_ = robot_->getJointState(joint_name); 00091 if (!joint_state_) 00092 { 00093 ROS_ERROR("SrhJointPositionController could not find joint named \"%s\"\n", 00094 joint_name.c_str()); 00095 return false; 00096 } 00097 } 00098 00099 friction_compensator = boost::shared_ptr<sr_friction_compensation::SrFrictionCompensator>(new sr_friction_compensation::SrFrictionCompensator(joint_name)); 00100 00101 serve_set_gains_ = node_.advertiseService("set_gains", &SrhEffortJointController::setGains, this); 00102 serve_reset_gains_ = node_.advertiseService("reset_gains", &SrhEffortJointController::resetGains, this); 00103 00104 ROS_DEBUG_STREAM(" joint_state name: " << joint_state_->joint_->name); 00105 ROS_DEBUG_STREAM(" In Init: " << getJointName() << " This: " << this 00106 << " joint_state: "<<joint_state_ ); 00107 00108 after_init(); 00109 return true; 00110 } 00111 00112 bool SrhEffortJointController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) 00113 { 00114 assert(robot); 00115 node_ = n; 00116 00117 std::string joint_name; 00118 if (!node_.getParam("joint", joint_name)) { 00119 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str()); 00120 return false; 00121 } 00122 00123 controller_state_publisher_.reset( 00124 new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointControllerState> 00125 (node_, "state", 1)); 00126 00127 return init(robot, joint_name); 00128 } 00129 00130 00131 void SrhEffortJointController::starting() 00132 { 00133 command_ = 0.0; 00134 read_parameters(); 00135 } 00136 00137 bool SrhEffortJointController::setGains(sr_robot_msgs::SetEffortControllerGains::Request &req, 00138 sr_robot_msgs::SetEffortControllerGains::Response &resp) 00139 { 00140 max_force_demand = req.max_force; 00141 friction_deadband = req.friction_deadband; 00142 00143 //Setting the new parameters in the parameter server 00144 node_.setParam("max_force", max_force_demand); 00145 node_.setParam("friction_deadband", friction_deadband); 00146 00147 return true; 00148 } 00149 00150 bool SrhEffortJointController::resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp) 00151 { 00152 command_ = 0.0; 00153 00154 read_parameters(); 00155 00156 if( has_j2 ) 00157 ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name << " and " << joint_state_2->joint_->name); 00158 else 00159 ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name); 00160 00161 return true; 00162 } 00163 00164 void SrhEffortJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) 00165 { 00166 } 00167 00168 void SrhEffortJointController::update() 00169 { 00170 if( !has_j2) 00171 { 00172 if (!joint_state_->calibrated_) 00173 return; 00174 } 00175 00176 assert(robot_ != NULL); 00177 ros::Time time = robot_->getTime(); 00178 assert(joint_state_->joint_); 00179 dt_= time - last_time_; 00180 00181 if (!initialized_) 00182 { 00183 initialized_ = true; 00184 command_ = 0.0; 00185 } 00186 00187 //The commanded effort is the error directly: 00188 // the PID loop for the force controller is running on the 00189 // motorboard. 00190 double commanded_effort = command_; 00191 00192 //Clamps the effort 00193 commanded_effort = min( commanded_effort, max_force_demand ); 00194 commanded_effort = max( commanded_effort, -max_force_demand ); 00195 00196 //Friction compensation 00197 if( has_j2 ) 00198 commanded_effort += friction_compensator->friction_compensation( joint_state_->position_ + joint_state_2->position_ , joint_state_->velocity_ + joint_state_2->velocity_, int(commanded_effort), friction_deadband ); 00199 else 00200 commanded_effort += friction_compensator->friction_compensation( joint_state_->position_ , joint_state_->velocity_, int(commanded_effort), friction_deadband ); 00201 00202 if( has_j2 ) 00203 joint_state_2->commanded_effort_ = commanded_effort; 00204 else 00205 joint_state_->commanded_effort_ = commanded_effort; 00206 00207 if(loop_count_ % 10 == 0) 00208 { 00209 if(controller_state_publisher_ && controller_state_publisher_->trylock()) 00210 { 00211 controller_state_publisher_->msg_.header.stamp = time; 00212 controller_state_publisher_->msg_.set_point = command_; 00213 controller_state_publisher_->msg_.process_value = joint_state_->measured_effort_; 00214 //TODO: compute the derivative of the effort. 00215 controller_state_publisher_->msg_.process_value_dot = -1.0; 00216 controller_state_publisher_->msg_.error = commanded_effort - joint_state_->measured_effort_; 00217 controller_state_publisher_->msg_.time_step = dt_.toSec(); 00218 controller_state_publisher_->msg_.command = commanded_effort; 00219 00220 double dummy; 00221 getGains(controller_state_publisher_->msg_.p, 00222 controller_state_publisher_->msg_.i, 00223 controller_state_publisher_->msg_.d, 00224 controller_state_publisher_->msg_.i_clamp, 00225 dummy); 00226 controller_state_publisher_->unlockAndPublish(); 00227 } 00228 } 00229 loop_count_++; 00230 00231 last_time_ = time; 00232 } 00233 00234 void SrhEffortJointController::read_parameters() 00235 { 00236 node_.param<double>("max_force", max_force_demand, 1023.0); 00237 node_.param<int>("friction_deadband", friction_deadband, 5); 00238 } 00239 00240 } 00241 00242 /* For the emacs weenies in the crowd. 00243 Local Variables: 00244 c-basic-offset: 2 00245 End: 00246 */ 00247 00248