sr_mechanism_controllers: srh_mixed_position_velocity

Go to the documentation of this file.
00001 
00029 #include "sr_mechanism_controllers/srh_mixed_position_velocity_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, SrhMixedPositionVelocityJointController, controller::SrhMixedPositionVelocityJointController, pr2_controller_interface::Controller)
00039 
00040 using namespace std;
00041 
00042 namespace controller {
00043 
00044 
00045   SrhMixedPositionVelocityJointController::SrhMixedPositionVelocityJointController()
00046     : SrController(), max_velocity_(1.0), min_velocity_(-1.0),
00047       position_deadband(0.05), motor_min_force_threshold(0)
00048   {
00049   }
00050 
00051   SrhMixedPositionVelocityJointController::~SrhMixedPositionVelocityJointController()
00052   {
00053     sub_command_.shutdown();
00054   }
00055 
00056   bool SrhMixedPositionVelocityJointController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name,
00057                                                      boost::shared_ptr<control_toolbox::Pid> pid_position,
00058                                                      boost::shared_ptr<control_toolbox::Pid> pid_velocity)
00059   {
00060     ROS_DEBUG(" --------- ");
00061     ROS_DEBUG_STREAM("Init: " << joint_name);
00062 
00063     assert(robot);
00064     robot_ = robot;
00065     last_time_ = robot->getTime();
00066 
00067     //joint 0s
00068     if( joint_name.substr(3,1).compare("0") == 0)
00069     {
00070       has_j2 = true;
00071       std::string j1 = joint_name.substr(0,3) + "1";
00072       std::string j2 = joint_name.substr(0,3) + "2";
00073       ROS_DEBUG_STREAM("Joint 0: " << j1 << " " << j2);
00074 
00075       joint_state_ = robot_->getJointState(j1);
00076       if (!joint_state_)
00077       {
00078         ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
00079                   joint_name.c_str());
00080         return false;
00081       }
00082 
00083       joint_state_2 = robot_->getJointState(j2);
00084       if (!joint_state_2)
00085       {
00086         ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
00087                   joint_name.c_str());
00088         return false;
00089       }
00090       if (!joint_state_2->calibrated_)
00091       {
00092         ROS_ERROR("Joint %s not calibrated for SrhMixedPositionVelocityJointController", j2.c_str());
00093         return false;
00094       }
00095     }
00096     else //"normal" joints
00097     {
00098       has_j2 = false;
00099 
00100       joint_state_ = robot_->getJointState(joint_name);
00101       if (!joint_state_)
00102       {
00103         ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
00104                   joint_name.c_str());
00105         return false;
00106       }
00107       if (!joint_state_->calibrated_)
00108       {
00109         ROS_ERROR("Joint %s not calibrated for SrhMixedPositionVelocityJointController", joint_name.c_str());
00110         return false;
00111       }
00112     }
00113     friction_compensator = boost::shared_ptr<sr_friction_compensation::SrFrictionCompensator>(new sr_friction_compensation::SrFrictionCompensator(joint_name));
00114 
00115     //get the min and max value for the current joint:
00116     get_min_max( robot_->model_->robot_model_, joint_name );
00117 
00118     pid_controller_position_ = pid_position;
00119     pid_controller_velocity_ = pid_velocity;
00120 
00121     serve_set_gains_ = node_.advertiseService("set_gains", &SrhMixedPositionVelocityJointController::setGains, this);
00122     serve_reset_gains_ = node_.advertiseService("reset_gains", &SrhMixedPositionVelocityJointController::resetGains, this);
00123 
00124     ROS_DEBUG_STREAM(" joint_state name: " << joint_state_->joint_->name);
00125     ROS_DEBUG_STREAM(" In Init: " << getJointName() << " This: " << this
00126                      << " joint_state: "<<joint_state_ );
00127 
00128 #ifdef DEBUG_PUBLISHER
00129     if( std::string("FFJ3").compare(getJointName()) == 0)
00130     {
00131       ROS_INFO("Publishing debug infor for FFJ3 mixed position/velocity controller");
00132       std::stringstream ss2;
00133       ss2 << getJointName() << "debug_velocity";
00134       debug_pub = n_tilde_.advertise<std_msgs::Float64>(ss2.str(), 2);
00135     }
00136 #endif
00137 
00138     after_init();
00139     return true;
00140   }
00141 
00142   bool SrhMixedPositionVelocityJointController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00143   {
00144     assert(robot);
00145     node_ = n;
00146 
00147     std::string joint_name;
00148     if (!node_.getParam("joint", joint_name)) {
00149       ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00150       return false;
00151     }
00152 
00153     boost::shared_ptr<control_toolbox::Pid> pid_position = boost::shared_ptr<control_toolbox::Pid>( new control_toolbox::Pid() );;
00154     if (!pid_position->init(ros::NodeHandle(node_, "position_pid")))
00155       return false;
00156 
00157     boost::shared_ptr<control_toolbox::Pid> pid_velocity = boost::shared_ptr<control_toolbox::Pid>( new control_toolbox::Pid() );;
00158     if (!pid_velocity->init(ros::NodeHandle(node_, "velocity_pid")))
00159       return false;
00160 
00161     controller_state_publisher_.reset(
00162       new realtime_tools::RealtimePublisher<sr_robot_msgs::JointControllerState>
00163       (node_, "state", 1));
00164 
00165     return init(robot, joint_name, pid_position, pid_velocity);
00166   }
00167 
00168 
00169   void SrhMixedPositionVelocityJointController::starting()
00170   {
00171     if( has_j2 )
00172       command_ = joint_state_->position_ + joint_state_2->position_;
00173     else
00174       command_ = joint_state_->position_;
00175 
00176     pid_controller_position_->reset();
00177     pid_controller_velocity_->reset();
00178     read_parameters();
00179     ROS_WARN("Reseting PID");
00180     last_time_ = robot_->getTime();
00181   }
00182 
00183   bool SrhMixedPositionVelocityJointController::setGains(sr_robot_msgs::SetMixedPositionVelocityPidGains::Request &req,
00184                                                          sr_robot_msgs::SetMixedPositionVelocityPidGains::Response &resp)
00185   {
00186     ROS_INFO_STREAM("New parameters: " << "PID pos: [" <<req.position_p << ", " <<req.position_i << ", " <<req.position_d << ", " <<req.position_i_clamp << "] PID vel: ["<< req.velocity_p << ", " <<req.velocity_i << ", " <<req.velocity_d << ", " <<req.velocity_i_clamp << "], max force: " <<req.max_force << ", friction deadband: "<< req.friction_deadband << " pos deadband: "<<req.position_deadband << " min and max vel: ["<<req.min_velocity << ", " << req.max_velocity <<"]");
00187 
00188     pid_controller_position_->setGains(req.position_p,req.position_i,req.position_d,req.position_i_clamp,-req.position_i_clamp);
00189 
00190     pid_controller_velocity_->setGains(req.velocity_p,req.velocity_i,req.velocity_d,req.velocity_i_clamp,-req.velocity_i_clamp);
00191     max_force_demand = req.max_force;
00192     friction_deadband = req.friction_deadband;
00193     position_deadband = req.position_deadband;
00194 
00195     //setting the position controller parameters
00196     min_velocity_ = req.min_velocity;
00197     max_velocity_ = req.max_velocity;
00198 
00199     //Setting the new parameters in the parameter server
00200     node_.setParam("position_pid/p", req.position_p);
00201     node_.setParam("position_pid/i", req.position_i);
00202     node_.setParam("position_pid/d", req.position_d);
00203     node_.setParam("position_pid/i_clamp", req.position_i_clamp);
00204 
00205     node_.setParam("velocity_pid/p", req.velocity_p);
00206     node_.setParam("velocity_pid/i", req.velocity_i);
00207     node_.setParam("velocity_pid/d", req.velocity_d);
00208     node_.setParam("velocity_pid/i_clamp", req.velocity_i_clamp);
00209 
00210     node_.setParam("position_pid/min_velocity", min_velocity_);
00211     node_.setParam("position_pid/max_velocity", max_velocity_);
00212     node_.setParam("position_pid/position_deadband", position_deadband);
00213 
00214     node_.setParam("velocity_pid/friction_deadband", friction_deadband);
00215     node_.setParam("velocity_pid/max_force", max_force_demand);
00216     node_.setParam("motor_min_force_threshold", motor_min_force_threshold);
00217 
00218     return true;
00219   }
00220 
00221   bool SrhMixedPositionVelocityJointController::resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp)
00222   {
00223     if( has_j2 )
00224       command_ = joint_state_->position_ + joint_state_2->position_;
00225     else
00226       command_ = joint_state_->position_;
00227 
00228     if (!pid_controller_position_->init(ros::NodeHandle(node_, "position_pid")))
00229       return false;
00230 
00231     if (!pid_controller_velocity_->init(ros::NodeHandle(node_, "velocity_pid")))
00232       return false;
00233 
00234     read_parameters();
00235 
00236     if( has_j2 )
00237       ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name << " and " << joint_state_2->joint_->name);
00238     else
00239       ROS_WARN_STREAM("Reseting controller gains: " << joint_state_->joint_->name);
00240 
00241     return true;
00242   }
00243 
00244   void SrhMixedPositionVelocityJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00245   {
00246     pid_controller_position_->getGains(p,i,d,i_max,i_min);
00247   }
00248   void SrhMixedPositionVelocityJointController::getGains_velocity(double &p, double &i, double &d, double &i_max, double &i_min)
00249   {
00250     pid_controller_velocity_->getGains(p,i,d,i_max,i_min);
00251   }
00252 
00253   void SrhMixedPositionVelocityJointController::update()
00254   {
00255     if( !has_j2)
00256     {
00257       if (!joint_state_->calibrated_)
00258         return;
00259     }
00260 
00261     assert(robot_ != NULL);
00262     ros::Time time = robot_->getTime();
00263     assert(joint_state_->joint_);
00264     dt_= time - last_time_;
00265 
00266     if (!initialized_)
00267     {
00268       initialized_ = true;
00269       if( has_j2 )
00270         command_ = joint_state_->position_ + joint_state_2->position_;
00271       else
00272         command_ = joint_state_->position_;
00273     }
00274 
00275     //Clamps the command to the correct range.
00276     command_ = clamp_command( command_ );
00277 
00279     // POSITION
00281     //Compute velocity demand from position error:
00282     double error_position = 0.0;
00283     if( has_j2 )
00284     {
00285       error_position = command_ - (joint_state_->position_ + joint_state_2->position_);
00286       ROS_DEBUG_STREAM("j0: " << joint_state_->position_ + joint_state_2->position_);
00287     }
00288     else
00289       error_position = command_ - joint_state_->position_;
00290 
00291     //are we in the deadband?
00292     bool in_deadband = hysteresis_deadband.is_in_deadband(command_, error_position, position_deadband);
00293 
00294     if( in_deadband ) //consider the error as 0 if we're in the deadband
00295     {
00296       error_position = 0.0;
00297       pid_controller_position_->reset();
00298     }
00299 
00300     double commanded_velocity = 0.0;
00301     double error_velocity = 0.0;
00302     double commanded_effort = 0.0;
00303 
00304     //compute the velocity demand using the position pid loop
00305     commanded_velocity = pid_controller_position_->updatePid(error_position, dt_);
00306     //saturate the velocity demand
00307     commanded_velocity = max( commanded_velocity, min_velocity_ );
00308     commanded_velocity = min( commanded_velocity, max_velocity_ );
00309 
00311     // VELOCITY
00313 
00314     //velocity loop:
00315     if( !in_deadband ) //don't compute the error if we're in the deadband
00316     {
00317       //we're not in the deadband, compute the error
00318       if( has_j2 )
00319         error_velocity = commanded_velocity - (joint_state_->velocity_ + joint_state_2->velocity_);
00320       else
00321         error_velocity = commanded_velocity - joint_state_->velocity_;
00322     }
00323     commanded_effort = pid_controller_velocity_->updatePid(error_velocity, dt_);
00324 
00325     //clamp the result to max force
00326     commanded_effort = min( commanded_effort, max_force_demand );
00327     commanded_effort = max( commanded_effort, -max_force_demand );
00328 
00329     //Friction compensation, only if we're not in the deadband.
00330     int friction_offset = 0;
00331     if( !in_deadband )
00332     {
00333       if( has_j2 )
00334         friction_offset = friction_compensator->friction_compensation( joint_state_->position_ + joint_state_2->position_ , joint_state_->velocity_ + joint_state_2->velocity_, int(commanded_effort), friction_deadband );
00335       else
00336         friction_offset = friction_compensator->friction_compensation( joint_state_->position_ , joint_state_->velocity_, int(commanded_effort), friction_deadband );
00337 
00338       commanded_effort += friction_offset;
00339     }
00340 
00341     //if the demand is too small to be executed by the motor, then we ask for a force
00342     // of 0
00343     if( fabs(commanded_effort) <= motor_min_force_threshold )
00344       commanded_effort = 0.0;
00345 
00346     if( has_j2 )
00347       joint_state_2->commanded_effort_ = commanded_effort;
00348     else
00349       joint_state_->commanded_effort_ = commanded_effort;
00350 
00351     if(loop_count_ % 10 == 0)
00352     {
00353       if(controller_state_publisher_ && controller_state_publisher_->trylock())
00354       {
00355         controller_state_publisher_->msg_.header.stamp = time;
00356         controller_state_publisher_->msg_.set_point = command_;
00357         if( has_j2 )
00358         {
00359           controller_state_publisher_->msg_.process_value = joint_state_->position_ + joint_state_2->position_;
00360           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_;
00361         }
00362         else
00363         {
00364           controller_state_publisher_->msg_.process_value = joint_state_->position_;
00365           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
00366         }
00367         controller_state_publisher_->msg_.commanded_velocity = commanded_velocity;
00368 
00369         controller_state_publisher_->msg_.error = error_position;
00370         controller_state_publisher_->msg_.time_step = dt_.toSec();
00371 
00372         controller_state_publisher_->msg_.command = commanded_effort;
00373         controller_state_publisher_->msg_.measured_effort = joint_state_->measured_effort_;
00374 
00375         controller_state_publisher_->msg_.friction_compensation = friction_offset;
00376 
00377         double dummy;
00378         getGains(controller_state_publisher_->msg_.position_p,
00379                  controller_state_publisher_->msg_.position_i,
00380                  controller_state_publisher_->msg_.position_d,
00381                  controller_state_publisher_->msg_.position_i_clamp,
00382                  dummy);
00383 
00384         getGains_velocity(controller_state_publisher_->msg_.velocity_p,
00385                  controller_state_publisher_->msg_.velocity_i,
00386                  controller_state_publisher_->msg_.velocity_d,
00387                  controller_state_publisher_->msg_.velocity_i_clamp,
00388                  dummy);
00389 
00390         controller_state_publisher_->unlockAndPublish();
00391       }
00392     }
00393     loop_count_++;
00394 
00395     last_time_ = time;
00396   }
00397 
00398   void SrhMixedPositionVelocityJointController::read_parameters()
00399   {
00400     node_.param<double>("position_pid/min_velocity", min_velocity_, -1.0);
00401     node_.param<double>("position_pid/max_velocity", max_velocity_, 1.0);
00402     node_.param<double>("position_pid/position_deadband", position_deadband, 0.015);
00403 
00404     node_.param<int>("velocity_pid/friction_deadband", friction_deadband, 5);
00405     node_.param<double>("velocity_pid/max_force", max_force_demand, 1023.0);
00406     node_.param<int>("motor_min_force_threshold", motor_min_force_threshold, 0);
00407   }
00408 }
00409 
00410 /* For the emacs weenies in the crowd.
00411 Local Variables:
00412    c-basic-offset: 2
00413 End:
00414 */
00415 
00416