srh_joint_effort_controller.cpp
Go to the documentation of this file.
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_EXPORT_CLASS( 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 * max_force_factor_) );
00194     commanded_effort = max( commanded_effort, -(max_force_demand * max_force_factor_) );
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 */


sr_mechanism_controllers
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:09:56