srh_joint_position_controller.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_mechanism_controllers/srh_joint_position_controller.hpp"
00028 #include "angles/angles.h"
00029 #include "pluginlib/class_list_macros.h"
00030 #include <sstream>
00031 #include <math.h>
00032 #include "sr_utilities/sr_math_utils.hpp"
00033 
00034 #include <std_msgs/Float64.h>
00035 
00036 PLUGINLIB_DECLARE_CLASS(sr_mechanism_controllers, SrhJointPositionController, controller::SrhJointPositionController, pr2_controller_interface::Controller)
00037 
00038 using namespace std;
00039 
00040 namespace controller {
00041 
00042   SrhJointPositionController::SrhJointPositionController()
00043     : SrController(), position_deadband(0.015)
00044   {
00045   }
00046 
00047   SrhJointPositionController::~SrhJointPositionController()
00048   {
00049     sub_command_.shutdown();
00050   }
00051 
00052   bool SrhJointPositionController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name,
00053                                         boost::shared_ptr<control_toolbox::Pid> pid_position)
00054   {
00055     ROS_DEBUG(" --------- ");
00056     ROS_DEBUG_STREAM("Init: " << joint_name);
00057 
00058     joint_name_ = joint_name;
00059 
00060     assert(robot);
00061     robot_ = robot;
00062     last_time_ = robot->getTime();
00063 
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("SrhJointPositionController could not find joint named \"%s\"\n",
00075                   joint_name.c_str());
00076         return false;
00077       }
00078 
00079       joint_state_2 = robot_->getJointState(j2);
00080       if (!joint_state_2)
00081       {
00082         ROS_ERROR("SrhJointPositionController could not find joint named \"%s\"\n",
00083                   joint_name.c_str());
00084         return false;
00085       }
00086       if (!joint_state_2->calibrated_)
00087       {
00088         ROS_ERROR("Joint %s not calibrated for SrhJointPositionController", j2.c_str());
00089         return false;
00090       }
00091     }
00092     else
00093     {
00094       has_j2 = false;
00095       joint_state_ = robot_->getJointState(joint_name);
00096       if (!joint_state_)
00097       {
00098         ROS_ERROR("SrhJointPositionController could not find joint named \"%s\"\n",
00099                   joint_name.c_str());
00100         return false;
00101       }
00102       if (!joint_state_->calibrated_)
00103       {
00104         ROS_ERROR("Joint %s not calibrated for SrhJointPositionController", joint_name.c_str());
00105         return false;
00106       }
00107     }
00108 
00109     //get the min and max value for the current joint:
00110     get_min_max( robot_->model_->robot_model_, joint_name );
00111 
00112     friction_compensator = boost::shared_ptr<sr_friction_compensation::SrFrictionCompensator>(new sr_friction_compensation::SrFrictionCompensator(joint_name));
00113 
00114     pid_controller_position_ = pid_position;
00115 
00116     serve_set_gains_ = node_.advertiseService("set_gains", &SrhJointPositionController::setGains, this);
00117     serve_reset_gains_ = node_.advertiseService("reset_gains", &SrhJointPositionController::resetGains, this);
00118 
00119     after_init();
00120     return true;
00121   }
00122 
00123   bool SrhJointPositionController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00124   {
00125     assert(robot);
00126     node_ = n;
00127 
00128     std::string joint_name;
00129     if (!node_.getParam("joint", joint_name)) {
00130       ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00131       return false;
00132     }
00133 
00134     boost::shared_ptr<control_toolbox::Pid> pid_position = boost::shared_ptr<control_toolbox::Pid>( new control_toolbox::Pid() );
00135     if (!pid_position->init(ros::NodeHandle(node_, "pid")))
00136       return false;
00137 
00138     controller_state_publisher_.reset(
00139       new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointControllerState>
00140       (node_, "state", 1));
00141 
00142     return init(robot, joint_name, pid_position);
00143   }
00144 
00145 
00146   void SrhJointPositionController::starting()
00147   {
00148     if( has_j2 )
00149       command_ = joint_state_->position_ + joint_state_2->position_;
00150     else
00151       command_ = joint_state_->position_;
00152     pid_controller_position_->reset();
00153     read_parameters();
00154 
00155     ROS_WARN("Reseting PID");
00156   }
00157 
00158   bool SrhJointPositionController::setGains(sr_robot_msgs::SetPidGains::Request &req,
00159                                             sr_robot_msgs::SetPidGains::Response &resp)
00160   {
00161     ROS_INFO_STREAM("Setting new PID parameters. P:"<< req.p<< " / I:" << req.i <<
00162                     " / D:" << req.d << " / IClamp:"<<req.i_clamp<< ", max force: " <<req.max_force << ", friction deadband: "<< req.friction_deadband << " pos deadband: "<<req.deadband);
00163     pid_controller_position_->setGains(req.p,req.i,req.d,req.i_clamp,-req.i_clamp);
00164     max_force_demand = req.max_force;
00165     friction_deadband = req.friction_deadband;
00166     position_deadband = req.deadband;
00167 
00168     //Setting the new parameters in the parameter server
00169     node_.setParam("pid/p", req.p);
00170     node_.setParam("pid/i", req.i);
00171     node_.setParam("pid/d", req.d);
00172     node_.setParam("pid/i_clamp", req.i_clamp);
00173     node_.setParam("pid/max_force", max_force_demand);
00174     node_.setParam("pid/position_deadband", position_deadband);
00175     node_.setParam("pid/friction_deadband", friction_deadband);
00176 
00177     return true;
00178   }
00179 
00180   bool SrhJointPositionController::resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp)
00181   {
00182     if( has_j2 )
00183       command_ = joint_state_->position_ + joint_state_2->position_;
00184     else
00185       command_ = joint_state_->position_;
00186 
00187     if (!pid_controller_position_->init(ros::NodeHandle(node_, "pid")))
00188       return false;
00189 
00190     read_parameters();
00191 
00192     if( has_j2 )
00193       ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name << " and " << joint_state_2->joint_->name);
00194     else
00195       ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name);
00196 
00197     return true;
00198   }
00199 
00200   void SrhJointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00201   {
00202     pid_controller_position_->getGains(p,i,d,i_max,i_min);
00203   }
00204 
00205   void SrhJointPositionController::update()
00206   {
00207     if( !has_j2)
00208     {
00209       if (!joint_state_->calibrated_)
00210         return;
00211     }
00212 
00213     assert(robot_ != NULL);
00214     ros::Time time = robot_->getTime();
00215     assert(joint_state_->joint_);
00216     dt_= time - last_time_;
00217 
00218     if (!initialized_)
00219     {
00220       initialized_ = true;
00221 
00222       if( has_j2 )
00223         command_ = joint_state_->position_ + joint_state_2->position_;
00224       else
00225         command_ = joint_state_->position_;
00226     }
00227 
00228     command_ = clamp_command( command_ );
00229 
00230     //Compute position demand from position error:
00231     double error_position = 0.0;
00232     double commanded_effort = 0.0;
00233     if( has_j2 )
00234       error_position = (joint_state_->position_ + joint_state_2->position_) - command_;
00235     else
00236       error_position = joint_state_->position_ - command_;
00237 
00238     bool in_deadband = hysteresis_deadband.is_in_deadband(command_, error_position, position_deadband);
00239 
00240     //don't compute the error if we're in the deadband.
00241     if( in_deadband )
00242       error_position = 0.0;
00243 
00244     commanded_effort = pid_controller_position_->updatePid(error_position, dt_);
00245 
00246     //clamp the result to max force
00247     commanded_effort = min( commanded_effort, max_force_demand );
00248     commanded_effort = max( commanded_effort, -max_force_demand );
00249 
00250     if( !in_deadband )
00251     {
00252       if( has_j2 )
00253         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 );
00254       else
00255         commanded_effort += friction_compensator->friction_compensation( joint_state_->position_ , joint_state_->velocity_, int(commanded_effort), friction_deadband );
00256     }
00257 
00258     if( has_j2 )
00259       joint_state_2->commanded_effort_ = commanded_effort;
00260     else
00261       joint_state_->commanded_effort_ = commanded_effort;
00262 
00263     if(loop_count_ % 10 == 0)
00264     {
00265       if(controller_state_publisher_ && controller_state_publisher_->trylock())
00266       {
00267         controller_state_publisher_->msg_.header.stamp = time;
00268         controller_state_publisher_->msg_.set_point = command_;
00269         if( has_j2 )
00270         {
00271           controller_state_publisher_->msg_.process_value = joint_state_->position_ + joint_state_2->position_;
00272           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_;
00273         }
00274         else
00275         {
00276           controller_state_publisher_->msg_.process_value = joint_state_->position_;
00277           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
00278         }
00279 
00280         controller_state_publisher_->msg_.error = error_position;
00281         controller_state_publisher_->msg_.time_step = dt_.toSec();
00282         controller_state_publisher_->msg_.command = commanded_effort;
00283 
00284         double dummy;
00285         getGains(controller_state_publisher_->msg_.p,
00286                  controller_state_publisher_->msg_.i,
00287                  controller_state_publisher_->msg_.d,
00288                  controller_state_publisher_->msg_.i_clamp,
00289                  dummy);
00290         controller_state_publisher_->unlockAndPublish();
00291       }
00292     }
00293     loop_count_++;
00294 
00295     last_time_ = time;
00296   }
00297 
00298   void SrhJointPositionController::read_parameters()
00299   {
00300     node_.param<double>("pid/max_force", max_force_demand, 1023.0);
00301     node_.param<double>("pid/position_deadband", position_deadband, 0.015);
00302     node_.param<int>("pid/friction_deadband", friction_deadband, 5);
00303   }
00304 }
00305 
00306 /* For the emacs weenies in the crowd.
00307 Local Variables:
00308    c-basic-offset: 2
00309 End:
00310 */
00311 
00312 


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:06:50