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


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