srh_muscle_joint_position_controller.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_mechanism_controllers/srh_muscle_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_EXPORT_CLASS( controller::SrhMuscleJointPositionController, pr2_controller_interface::Controller)
00037 
00038 using namespace std;
00039 
00040 namespace controller {
00041 
00042   SrhMuscleJointPositionController::SrhMuscleJointPositionController()
00043     : SrController(), position_deadband(0.015), command_acc_(0)
00044   {
00045   }
00046 
00047   SrhMuscleJointPositionController::~SrhMuscleJointPositionController()
00048   {
00049     sub_command_.shutdown();
00050   }
00051 
00052   bool SrhMuscleJointPositionController::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("SrhMuscleJointPositionController 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("SrhMuscleJointPositionController could not find joint named \"%s\"\n",
00083                   j2.c_str());
00084         return false;
00085       }
00086 //      if (!joint_state_2->calibrated_)
00087 //      {
00088 //        ROS_ERROR("Joint %s not calibrated for SrhMuscleJointPositionController", 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("SrhMuscleJointPositionController 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 SrhMuscleJointPositionController", 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", &SrhMuscleJointPositionController::setGains, this);
00117     serve_reset_gains_ = node_.advertiseService("reset_gains", &SrhMuscleJointPositionController::resetGains, this);
00118 
00119     after_init();
00120     return true;
00121   }
00122 
00123   bool SrhMuscleJointPositionController::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<sr_robot_msgs::JointMusclePositionControllerState>
00140       (node_, "state", 1));
00141 
00142     return init(robot, joint_name, pid_position);
00143   }
00144 
00145 
00146   void SrhMuscleJointPositionController::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 SrhMuscleJointPositionController::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 SrhMuscleJointPositionController::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 SrhMuscleJointPositionController::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 SrhMuscleJointPositionController::update()
00206   {
00207     //The valve commands can have values between -4 and 4
00208     int8_t valve[2];
00209 
00210 //    if( !has_j2)
00211 //    {
00212 //      if (!joint_state_->calibrated_)
00213 //        return;
00214 //    }
00215 
00216     assert(robot_ != NULL);
00217     ros::Time time = robot_->getTime();
00218     assert(joint_state_->joint_);
00219     dt_= time - last_time_;
00220 
00221     if (!initialized_)
00222     {
00223       initialized_ = true;
00224 
00225       if( has_j2 )
00226         command_ = joint_state_->position_ + joint_state_2->position_;
00227       else
00228         command_ = joint_state_->position_;
00229     }
00230 
00231     //IGNORE the following  lines if we don't want to use the pressure sensors data
00232     //We don't want to define a modified version of JointState, as that would imply using a modified version of robot.h, controller manager,
00233     //ethercat_hardware and pr2_etherCAT main loop
00234     // So we heve encoded the two uint16 that contain the data from the muscle pressure sensors into the double measured_effort_. (We don't
00235     // have any measured effort in the muscle hand anyway).
00236     // Here we extract the pressure values from joint_state_->measured_effort_ and decode that back into uint16.
00237     double pressure_0_tmp = fmod(joint_state_->measured_effort_, 0x10000);
00238     double pressure_1_tmp = (fmod(joint_state_->measured_effort_, 0x100000000) - pressure_0_tmp) / 0x10000;
00239     uint16_t pressure_0 = static_cast<uint16_t>(pressure_0_tmp + 0.5);
00240     uint16_t pressure_1 = static_cast<uint16_t>(pressure_1_tmp + 0.5);
00241 
00242     //****************************************
00243 
00244     command_ = clamp_command( command_ );
00245 
00246     //Compute position demand from position error:
00247     double error_position = 0.0;
00248 
00249     if( has_j2 )
00250       error_position = (joint_state_->position_ + joint_state_2->position_) - command_;
00251     else
00252       error_position = joint_state_->position_ - command_;
00253 
00254     bool in_deadband = hysteresis_deadband.is_in_deadband(command_, error_position, position_deadband);
00255 
00256     //don't compute the error if we're in the deadband.
00257     if( in_deadband )
00258       error_position = 0.0;
00259 
00260     //Run the PID loop to get a new command, we don't do this at the full rate
00261     //as that will drive the valves too hard with switch changes. Instead we
00262     //store a longer time in the command accumulator to keep using at the full
00263     //loop rate.
00264     if(loop_count_ % 50 == 0)
00265     {
00266       double commanded_effort = pid_controller_position_->computeCommand(-error_position, dt_);
00267 
00268       //clamp the result to max force
00269       commanded_effort = min( commanded_effort, max_force_demand );
00270       commanded_effort = max( commanded_effort, -max_force_demand );
00271 
00272       if( !in_deadband )
00273       {
00274         if( has_j2 )
00275           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 );
00276         else
00277           commanded_effort += friction_compensator->friction_compensation( joint_state_->position_ , joint_state_->velocity_, int(commanded_effort), friction_deadband );
00278       }
00279 
00280       command_acc_ = commanded_effort;
00281 
00282       //We want the last time we updated the PID loop not sent commands.
00283       last_time_ = time;
00284     }
00285 
00286     // Drive the joint from the accumulator. This runs at full update speed so
00287     // we can keep valves open continuously (with high enough P). A value of 4 fills
00288     // the valve for the whole of the next 1ms. 2 for half that time etc. Negative
00289     // values empty the valve.
00290     // The 2 involved muscles will act complementary for the moment, when one inflates,
00291     // the other deflates at the same rate
00292     double amt = abs(command_acc_) < 4 ? fabs(command_acc_) : 4;
00293     if (abs(command_acc_) == 0)
00294     {
00295       valve[0] = 0;
00296       valve[1] = 0;
00297     }
00298     else if(command_acc_ > 0)
00299     {
00300       command_acc_ -= amt;
00301       valve[0] = amt;
00302       valve[1] = -amt;
00303     }
00304     else
00305     {
00306       command_acc_ += amt;
00307       valve[0] = -amt;
00308       valve[1] = amt;
00309     }
00310 
00311 
00312     //************************************************
00313     // After doing any computation we consider, we encode the obtained valve commands into joint_state_->commanded_effort_
00314     //We don't want to define a modified version of JointState, as that would imply using a modified version of robot.h, controller manager,
00315     //ethercat_hardware and pr2_etherCAT main loop
00316     // So the controller encodes the two int8 (that are in fact int4) that contain the valve commands into the double commanded_effort_. (We don't
00317     // have any real commanded_effort_ in the muscle hand anyway).
00318 
00319     uint16_t valve_tmp[2];
00320     for(int i=0;i<2;++i)
00321     {
00322       //Check that the limits of the valve command are not exceded
00323       if (valve[i] > 4)
00324         valve[i] = 4;
00325       if (valve[i] < -4)
00326         valve[i] = -4;
00327       //encode
00328       if (valve[i] < 0)
00329         valve_tmp[i] = -valve[i] + 8;
00330       else
00331         valve_tmp[i] = valve[i];
00332     }
00333 
00334     //We encode the valve 0 command in the lowest "half byte" i.e. the lowest 16 integer values in the double var (see decoding in simple_transmission_for_muscle.cpp)
00335     //the valve 1 command is envoded in the next 4 bits
00336     joint_state_->commanded_effort_ = static_cast<double>(valve_tmp[0]) + static_cast<double>(valve_tmp[1] << 4);
00337 
00338     //*******************************************************************************
00339 
00340     // Send status msg
00341     if(loop_count_ % 10 == 0)
00342     {
00343       if(controller_state_publisher_ && controller_state_publisher_->trylock())
00344       {
00345         controller_state_publisher_->msg_.header.stamp = time;
00346         controller_state_publisher_->msg_.set_point = command_;
00347         if( has_j2 )
00348         {
00349           controller_state_publisher_->msg_.process_value = joint_state_->position_ + joint_state_2->position_;
00350           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_;
00351         }
00352         else
00353         {
00354           controller_state_publisher_->msg_.process_value = joint_state_->position_;
00355           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
00356         }
00357 
00358         controller_state_publisher_->msg_.error = error_position;
00359         controller_state_publisher_->msg_.time_step = dt_.toSec();
00360         controller_state_publisher_->msg_.pseudo_command = command_acc_;
00361         controller_state_publisher_->msg_.valve_muscle_0 = valve[0];
00362         controller_state_publisher_->msg_.valve_muscle_1 = valve[1];
00363         controller_state_publisher_->msg_.packed_valve = joint_state_->commanded_effort_;
00364         controller_state_publisher_->msg_.muscle_pressure_0 = pressure_0;
00365         controller_state_publisher_->msg_.muscle_pressure_1 = pressure_1;
00366 
00367 
00368         double dummy;
00369         getGains(controller_state_publisher_->msg_.p,
00370                  controller_state_publisher_->msg_.i,
00371                  controller_state_publisher_->msg_.d,
00372                  controller_state_publisher_->msg_.i_clamp,
00373                  dummy);
00374         controller_state_publisher_->unlockAndPublish();
00375       }
00376     }
00377     loop_count_++;
00378   }
00379 
00380   void SrhMuscleJointPositionController::read_parameters()
00381   {
00382     node_.param<double>("pid/max_force", max_force_demand, 1023.0);
00383     node_.param<double>("pid/position_deadband", position_deadband, 0.015);
00384     node_.param<int>("pid/friction_deadband", friction_deadband, 5);
00385   }
00386 }
00387 
00388 /* For the emacs weenies in the crowd.
00389 Local Variables:
00390    c-basic-offset: 2
00391 End:
00392 */
00393 
00394 


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