sr_motor_hand_lib.cpp
Go to the documentation of this file.
00001 
00029 #include "sr_robot_lib/sr_motor_hand_lib.hpp"
00030 #include <algorithm>
00031 #include <utility>
00032 #include <string>
00033 #include <vector>
00034 #include <boost/foreach.hpp>
00035 #include <boost/algorithm/string.hpp>
00036 
00037 #include <sr_utilities/sr_math_utils.hpp>
00038 
00039 #include "sr_robot_lib/shadow_PSTs.hpp"
00040 
00041 using std::vector;
00042 using std::string;
00043 using std::pair;
00044 using std::ostringstream;
00045 using sr_actuator::SrMotorActuator;
00046 using shadow_joints::Joint;
00047 using shadow_joints::JointToSensor;
00048 using shadow_joints::MotorWrapper;
00049 using generic_updater::MotorUpdater;
00050 using generic_updater::MotorDataChecker;
00051 using boost::shared_ptr;
00052 using boost::static_pointer_cast;
00053 
00054 namespace shadow_robot
00055 {
00056   template<class StatusType, class CommandType>
00057   const int SrMotorHandLib<StatusType, CommandType>::nb_motor_data = 14;
00058 
00059   template<class StatusType, class CommandType>
00060   const char *SrMotorHandLib<StatusType,
00061           CommandType>::human_readable_motor_data_types[nb_motor_data] = {"sgl", "sgr",
00062                                                                           "pwm", "flags",
00063                                                                           "current",
00064                                                                           "voltage",
00065                                                                           "temperature",
00066                                                                           "can_num_received",
00067                                                                           "can_num_transmitted",
00068                                                                           "slow_data",
00069                                                                           "can_error_counters",
00070                                                                           "pterm",
00071                                                                           "iterm",
00072                                                                           "dterm"};
00073 
00074   template<class StatusType, class CommandType>
00075   const int32u SrMotorHandLib<StatusType,
00076           CommandType>::motor_data_types[nb_motor_data] = {MOTOR_DATA_SGL,
00077                                                            MOTOR_DATA_SGR,
00078                                                            MOTOR_DATA_PWM,
00079                                                            MOTOR_DATA_FLAGS,
00080                                                            MOTOR_DATA_CURRENT,
00081                                                            MOTOR_DATA_VOLTAGE,
00082                                                            MOTOR_DATA_TEMPERATURE,
00083                                                            MOTOR_DATA_CAN_NUM_RECEIVED,
00084                                                            MOTOR_DATA_CAN_NUM_TRANSMITTED,
00085                                                            MOTOR_DATA_SLOW_MISC,
00086                                                            MOTOR_DATA_CAN_ERROR_COUNTERS,
00087                                                            MOTOR_DATA_PTERM,
00088                                                            MOTOR_DATA_ITERM,
00089                                                            MOTOR_DATA_DTERM};
00090 
00091   template<class StatusType, class CommandType>
00092   SrMotorHandLib<StatusType, CommandType>::SrMotorHandLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh,
00093                                                           ros::NodeHandle nhtilde, string device_id,
00094                                                           string joint_prefix) :
00095           SrMotorRobotLib<StatusType, CommandType>(hw, nh, nhtilde, device_id, joint_prefix)
00096   {
00097     // read the motor polling frequency from the parameter server
00098     this->motor_update_rate_configs_vector = this->read_update_rate_configs("motor_data_update_rate/", nb_motor_data,
00099                                                                             human_readable_motor_data_types,
00100                                                                             motor_data_types);
00101     this->motor_updater_ = shared_ptr<MotorUpdater<CommandType> >(
00102             new MotorUpdater<CommandType>(this->motor_update_rate_configs_vector,
00103                                           operation_mode::device_update_state::INITIALIZATION));
00104 
00105     // @todo read this from config/EEProm?
00106     vector<JointToSensor> joint_to_sensor_vect = this->read_joint_to_sensor_mapping();
00107 
00108     // initializing the joints vector
00109     vector<string> joint_names_tmp;
00110     vector<int> motor_ids = read_joint_to_motor_mapping();
00111 
00112     ROS_ASSERT(motor_ids.size() == JOINTS_NUM_0220);
00113     ROS_ASSERT(joint_to_sensor_vect.size() == JOINTS_NUM_0220);
00114 
00115     for (unsigned int i = 0; i < JOINTS_NUM_0220; ++i)
00116     {
00117       joint_names_tmp.push_back(string(joint_names[i]));
00118     }
00119     initialize(joint_names_tmp, motor_ids, joint_to_sensor_vect);
00120     // Initialize the motor data checker
00121     this->motor_data_checker = shared_ptr<MotorDataChecker>(
00122             new MotorDataChecker(this->joints_vector, this->motor_updater_->initialization_configs_vector));
00123   }
00124 
00125   template<class StatusType, class CommandType>
00126   void SrMotorHandLib<StatusType, CommandType>::initialize(vector<string> joint_names,
00127                                                            vector<int> actuator_ids,
00128                                                            vector<JointToSensor> joint_to_sensors)
00129   {
00130     for (unsigned int index = 0; index < joint_names.size(); ++index)
00131     {
00132       // add the joint and the vector of joints.
00133       Joint joint;
00134       float tau;
00135 
00136       // read the parameters tau from the parameter server
00137       ostringstream full_param;
00138       string act_name = boost::to_lower_copy(joint_names[index]);
00139       full_param << act_name << "/pos_filter/tau";
00140       this->nodehandle_.template param<float>(full_param.str(), tau, 0.05);
00141       full_param.str("");
00142       joint.pos_filter = sr_math_utils::filters::LowPassFilter(tau);
00143 
00144       // update the joint variables
00145       joint.joint_name = joint_names[index];
00146       joint.joint_to_sensor = joint_to_sensors[index];
00147 
00148       if (actuator_ids[index] != -1)
00149       {
00150         joint.has_actuator = true;
00151         shared_ptr<MotorWrapper> motor_wrapper(new MotorWrapper());
00152         joint.actuator_wrapper = motor_wrapper;
00153         motor_wrapper->motor_id = actuator_ids[index];
00154         motor_wrapper->actuator = static_cast<SrMotorActuator *> (this->hw_->getActuator(
00155                 this->joint_prefix_ + joint.joint_name));
00156         if (motor_wrapper->actuator == NULL)
00157         {
00158           ROS_WARN_STREAM("No actuator found for joint " <<
00159                           this->joint_prefix_ + joint.joint_name <<
00160                           " (check the robot_description contains it)");
00161           joint.has_actuator = false;
00162           continue;
00163         }
00164 
00165         ostringstream ss;
00166         ss << "change_force_PID_" << joint_names[index];
00167         // initialize the force pid service
00168         // NOTE: the template keyword is needed to avoid a compiler complaint apparently due to the fact that
00169         // we are using an explicit template function inside this template class
00170         motor_wrapper->force_pid_service =
00171                 this->nh_tilde.template advertiseService<sr_robot_msgs::ForceController::Request,
00172                         sr_robot_msgs::ForceController::Response>(ss.str().c_str(),
00173                                                                   boost::bind(
00174                                                                           &SrMotorHandLib<StatusType,
00175                                                                                   CommandType>::force_pid_callback,
00176                                                                           this, _1, _2,
00177                                                                           motor_wrapper->motor_id));
00178 
00179         ss.str("");
00180         ss << "reset_motor_" << joint_names[index];
00181         // initialize the reset motor service
00182         motor_wrapper->reset_motor_service =
00183                 this->nh_tilde.template advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>(
00184                         ss.str().c_str(),
00185                         boost::bind(&SrMotorHandLib<StatusType, CommandType>::reset_motor_callback, this, _1, _2,
00186                                     pair<int, string>(motor_wrapper->motor_id, joint.joint_name)));
00187       }
00188       else
00189       { // no motor associated to this joint
00190         joint.has_actuator = false;
00191       }
00192 
00193       this->joints_vector.push_back(joint);
00194     }  // end for joints.
00195   }
00196 
00197   template<class StatusType, class CommandType>
00198   bool SrMotorHandLib<StatusType, CommandType>::reset_motor_callback(std_srvs::Empty::Request &request,
00199                                                                      std_srvs::Empty::Response &response,
00200                                                                      pair<int, string> joint)
00201   {
00202     ROS_INFO_STREAM(" resetting " << joint.second << " (" << joint.first << ")");
00203 
00204     this->reset_motors_queue.push(joint.first);
00205 
00206     // wait a few secs for the reset to be sent then resend the pids
00207     string joint_name = joint.second;
00208 
00209     pid_timers[joint_name] = this->nh_tilde.createTimer(ros::Duration(3.0),
00210                                                         boost::bind(&SrMotorHandLib::resend_pids, this, joint_name,
00211                                                                     joint.first),
00212                                                         true);
00213 
00214     return true;
00215   }
00216 
00217   template<class StatusType, class CommandType>
00218   void SrMotorHandLib<StatusType, CommandType>::resend_pids(string joint_name, int motor_index)
00219   {
00220     // read the parameters from the parameter server and set the pid
00221     // values.
00222     ostringstream full_param;
00223 
00224     int f, p, i, d, imax, max_pwm, sg_left, sg_right, deadband, sign;
00225     string act_name = boost::to_lower_copy(joint_name);
00226 
00227     full_param << act_name << "/pid/f";
00228     this->nodehandle_.template param<int>(full_param.str(), f, 0);
00229     full_param.str("");
00230     full_param << act_name << "/pid/p";
00231     this->nodehandle_.template param<int>(full_param.str(), p, 0);
00232     full_param.str("");
00233     full_param << act_name << "/pid/i";
00234     this->nodehandle_.template param<int>(full_param.str(), i, 0);
00235     full_param.str("");
00236     full_param << act_name << "/pid/d";
00237     this->nodehandle_.template param<int>(full_param.str(), d, 0);
00238     full_param.str("");
00239     full_param << act_name << "/pid/imax";
00240     this->nodehandle_.template param<int>(full_param.str(), imax, 0);
00241     full_param.str("");
00242     full_param << act_name << "/pid/max_pwm";
00243     this->nodehandle_.template param<int>(full_param.str(), max_pwm, 0);
00244     full_param.str("");
00245     full_param << act_name << "/pid/sgleftref";
00246     this->nodehandle_.template param<int>(full_param.str(), sg_left, 0);
00247     full_param.str("");
00248     full_param << act_name << "/pid/sgrightref";
00249     this->nodehandle_.template param<int>(full_param.str(), sg_right, 0);
00250     full_param.str("");
00251     full_param << act_name << "/pid/deadband";
00252     this->nodehandle_.template param<int>(full_param.str(), deadband, 0);
00253     full_param.str("");
00254     full_param << act_name << "/pid/sign";
00255     this->nodehandle_.template param<int>(full_param.str(), sign, 0);
00256     full_param.str("");
00257 
00258     sr_robot_msgs::ForceController::Request pid_request;
00259     pid_request.maxpwm = max_pwm;
00260     pid_request.sgleftref = sg_left;
00261     pid_request.sgrightref = sg_right;
00262     pid_request.f = f;
00263     pid_request.p = p;
00264     pid_request.i = i;
00265     pid_request.d = d;
00266     pid_request.imax = imax;
00267     pid_request.deadband = deadband;
00268     pid_request.sign = sign;
00269     sr_robot_msgs::ForceController::Response pid_response;
00270     bool pid_success = force_pid_callback(pid_request, pid_response, motor_index);
00271 
00272     // setting the backlash compensation (on or off)
00273     bool backlash_compensation;
00274     full_param << act_name << "/backlash_compensation";
00275     this->nodehandle_.template param<bool>(full_param.str(), backlash_compensation, true);
00276     full_param.str("");
00277     sr_robot_msgs::ChangeMotorSystemControls::Request backlash_request;
00278     sr_robot_msgs::MotorSystemControls motor_sys_ctrl;
00279     motor_sys_ctrl.motor_id = motor_index;
00280     motor_sys_ctrl.enable_backlash_compensation = backlash_compensation;
00281 
00282     if (!backlash_compensation)
00283       ROS_INFO_STREAM("Setting backlash compensation to OFF for joint " << act_name);
00284 
00285     backlash_request.motor_system_controls.push_back(motor_sys_ctrl);
00286     sr_robot_msgs::ChangeMotorSystemControls::Response backlash_response;
00287     bool backlash_success = this->motor_system_controls_callback_(backlash_request, backlash_response);
00288 
00289     if (!pid_success)
00290       ROS_WARN_STREAM("Didn't load the force pid settings for the motor in joint " << act_name);
00291     if (!backlash_success)
00292       ROS_WARN_STREAM("Didn't set the backlash compensation correctly for the motor in joint " << act_name);
00293   }
00294 
00295   template<class StatusType, class CommandType>
00296   bool SrMotorHandLib<StatusType, CommandType>::force_pid_callback(sr_robot_msgs::ForceController::Request &request,
00297                                                                    sr_robot_msgs::ForceController::Response &response,
00298                                                                    int motor_index)
00299   {
00300     ROS_INFO_STREAM("Received new force PID parameters for motor " << motor_index);
00301 
00302     // Check the parameters are in the correct ranges
00303     if (motor_index > 20)
00304     {
00305       ROS_WARN_STREAM(" Wrong motor index specified: " << motor_index);
00306       response.configured = false;
00307       return false;
00308     }
00309 
00310     if (!((request.maxpwm >= MOTOR_DEMAND_PWM_RANGE_MIN) &&
00311           (request.maxpwm <= MOTOR_DEMAND_PWM_RANGE_MAX))
00312             )
00313     {
00314       ROS_WARN_STREAM(" pid parameter maxpwm is out of range : " << request.maxpwm << " -> not in [" <<
00315                       MOTOR_DEMAND_PWM_RANGE_MIN << " ; " << MOTOR_DEMAND_PWM_RANGE_MAX << "]");
00316       response.configured = false;
00317       return false;
00318     }
00319 
00320     if (!((request.f >= MOTOR_CONFIG_F_RANGE_MIN) &&
00321           (request.maxpwm <= MOTOR_CONFIG_F_RANGE_MAX))
00322             )
00323     {
00324       ROS_WARN_STREAM(" pid parameter f is out of range : " << request.f << " -> not in [" <<
00325                       MOTOR_CONFIG_F_RANGE_MIN << " ; " << MOTOR_CONFIG_F_RANGE_MAX << "]");
00326       response.configured = false;
00327       return false;
00328     }
00329 
00330     if (!((request.p >= MOTOR_CONFIG_P_RANGE_MIN) &&
00331           (request.p <= MOTOR_CONFIG_P_RANGE_MAX))
00332             )
00333     {
00334       ROS_WARN_STREAM(" pid parameter p is out of range : " << request.p << " -> not in [" <<
00335                       MOTOR_CONFIG_P_RANGE_MIN << " ; " << MOTOR_CONFIG_P_RANGE_MAX << "]");
00336       response.configured = false;
00337       return false;
00338     }
00339 
00340     if (!((request.i >= MOTOR_CONFIG_I_RANGE_MIN) &&
00341           (request.i <= MOTOR_CONFIG_I_RANGE_MAX))
00342             )
00343     {
00344       ROS_WARN_STREAM(" pid parameter i is out of range : " << request.i << " -> not in [" <<
00345                       MOTOR_CONFIG_I_RANGE_MIN << " ; " << MOTOR_CONFIG_I_RANGE_MAX << "]");
00346       response.configured = false;
00347       return false;
00348     }
00349 
00350     if (!((request.d >= MOTOR_CONFIG_D_RANGE_MIN) &&
00351           (request.d <= MOTOR_CONFIG_D_RANGE_MAX))
00352             )
00353     {
00354       ROS_WARN_STREAM(" pid parameter d is out of range : " << request.d << " -> not in [" <<
00355                       MOTOR_CONFIG_D_RANGE_MIN << " ; " << MOTOR_CONFIG_D_RANGE_MAX << "]");
00356       response.configured = false;
00357       return false;
00358     }
00359 
00360     if (!((request.imax >= MOTOR_CONFIG_IMAX_RANGE_MIN) &&
00361           (request.imax <= MOTOR_CONFIG_IMAX_RANGE_MAX))
00362             )
00363     {
00364       ROS_WARN_STREAM(" pid parameter imax is out of range : " << request.imax << " -> not in [" <<
00365                       MOTOR_CONFIG_IMAX_RANGE_MIN << " ; " << MOTOR_CONFIG_IMAX_RANGE_MAX << "]");
00366       response.configured = false;
00367       return false;
00368     }
00369 
00370     if (!((request.deadband >= MOTOR_CONFIG_DEADBAND_RANGE_MIN) &&
00371           (request.deadband <= MOTOR_CONFIG_DEADBAND_RANGE_MAX))
00372             )
00373     {
00374       ROS_WARN_STREAM(" pid parameter deadband is out of range : " << request.deadband << " -> not in [" <<
00375                       MOTOR_CONFIG_DEADBAND_RANGE_MIN << " ; " << MOTOR_CONFIG_DEADBAND_RANGE_MAX << "]");
00376       response.configured = false;
00377       return false;
00378     }
00379 
00380     if (!((request.sign >= MOTOR_CONFIG_SIGN_RANGE_MIN) &&
00381           (request.sign <= MOTOR_CONFIG_SIGN_RANGE_MAX))
00382             )
00383     {
00384       ROS_WARN_STREAM(" pid parameter sign is out of range : " << request.sign << " -> not in [" <<
00385                       MOTOR_CONFIG_SIGN_RANGE_MIN << " ; " << MOTOR_CONFIG_SIGN_RANGE_MAX << "]");
00386       response.configured = false;
00387       return false;
00388     }
00389 
00390     // ok, the parameters sent are coherent, send the demand to the motor.
00391     this->generate_force_control_config(motor_index, request.maxpwm, request.sgleftref,
00392                                         request.sgrightref, request.f, request.p, request.i,
00393                                         request.d, request.imax, request.deadband, request.sign);
00394 
00395     update_force_control_in_param_server(find_joint_name(motor_index), request.maxpwm, request.sgleftref,
00396                                          request.sgrightref, request.f, request.p, request.i,
00397                                          request.d, request.imax, request.deadband, request.sign);
00398     response.configured = true;
00399 
00400     // Reinitialize motors information
00401     this->reinitialize_motors();
00402 
00403     return true;
00404   }
00405 
00406   template<class StatusType, class CommandType>
00407   string SrMotorHandLib<StatusType, CommandType>::find_joint_name(int motor_index)
00408   {
00409     for (vector<Joint>::iterator joint = this->joints_vector.begin();
00410          joint != this->joints_vector.end();
00411          ++joint)
00412     {
00413       if (joint->has_actuator && static_pointer_cast<MotorWrapper>(joint->actuator_wrapper)->motor_id == motor_index)
00414       {
00415         return joint->joint_name;
00416       }
00417     }
00418     ROS_ERROR("Could not find joint name for motor index: %d", motor_index);
00419     return "";
00420   }
00421 
00422   template<class StatusType, class CommandType>
00423   void SrMotorHandLib<StatusType, CommandType>::update_force_control_in_param_server(string joint_name, int max_pwm,
00424                                                                                      int sg_left, int sg_right, int f,
00425                                                                                      int p,
00426                                                                                      int i, int d, int imax,
00427                                                                                      int deadband, int sign)
00428   {
00429     if (joint_name != "")
00430     {
00431       ostringstream full_param;
00432       string act_name = boost::to_lower_copy(joint_name);
00433 
00434       full_param << act_name << "/pid/f";
00435       this->nodehandle_.setParam(full_param.str(), f);
00436       full_param.str("");
00437       full_param << act_name << "/pid/p";
00438       this->nodehandle_.setParam(full_param.str(), p);
00439       full_param.str("");
00440       full_param << act_name << "/pid/i";
00441       this->nodehandle_.setParam(full_param.str(), i);
00442       full_param.str("");
00443       full_param << act_name << "/pid/d";
00444       this->nodehandle_.setParam(full_param.str(), d);
00445       full_param.str("");
00446       full_param << act_name << "/pid/imax";
00447       this->nodehandle_.setParam(full_param.str(), imax);
00448       full_param.str("");
00449       full_param << act_name << "/pid/max_pwm";
00450       this->nodehandle_.setParam(full_param.str(), max_pwm);
00451       full_param.str("");
00452       full_param << act_name << "/pid/sgleftref";
00453       this->nodehandle_.setParam(full_param.str(), sg_left);
00454       full_param.str("");
00455       full_param << act_name << "/pid/sgrightref";
00456       this->nodehandle_.setParam(full_param.str(), sg_right);
00457       full_param.str("");
00458       full_param << act_name << "/pid/deadband";
00459       this->nodehandle_.setParam(full_param.str(), deadband);
00460       full_param.str("");
00461       full_param << act_name << "/pid/sign";
00462       this->nodehandle_.setParam(full_param.str(), sign);
00463     }
00464   }
00465 
00466   template<class StatusType, class CommandType>
00467   vector<int> SrMotorHandLib<StatusType, CommandType>::read_joint_to_motor_mapping()
00468   {
00469     vector<int> motor_ids;
00470     string param_name = "joint_to_motor_mapping";
00471 
00472     XmlRpc::XmlRpcValue mapping;
00473     this->nodehandle_.getParam(param_name, mapping);
00474     ROS_ASSERT(mapping.getType() == XmlRpc::XmlRpcValue::TypeArray);
00475     // iterate on all the joints
00476     for (int32_t i = 0; i < mapping.size(); ++i)
00477     {
00478       ROS_ASSERT(mapping[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
00479       motor_ids.push_back(static_cast<int> (mapping[i]));
00480     }
00481 
00482     return motor_ids;
00483   }  // end read_joint_to_motor_mapping
00484 
00485 
00486 #ifdef DEBUG_PUBLISHER
00487   template <class StatusType, class CommandType>
00488   bool SrMotorHandLib<StatusType,
00489           CommandType>::set_debug_data_to_publish(sr_robot_msgs::SetDebugData::Request& request,
00490                                                   sr_robot_msgs::SetDebugData::Response& response)
00491   {
00492     // check if the publisher_index is correct
00493     if (request.publisher_index < this->nb_debug_publishers_const)
00494     {
00495       if (request.motor_index > NUM_MOTORS)
00496       {
00497         response.success = false;
00498         return false;
00499       }
00500       if (request.motor_data_type > 0)
00501       {
00502         if ((request.motor_data_type < MOTOR_DATA_SGL) ||
00503             (request.motor_data_type > MOTOR_DATA_DTERM))
00504         {
00505           response.success = false;
00506           return false;
00507         }
00508       }
00509       if (!this->debug_mutex.timed_lock(boost::posix_time::microseconds(this->debug_mutex_lock_wait_time)))
00510       {
00511         response.success = false;
00512         return false;
00513       }
00514 
00515       this->debug_motor_indexes_and_data[request.publisher_index] = shared_ptr<pair<int, int> >(new pair<int, int>());
00516 
00517       this->debug_motor_indexes_and_data[request.publisher_index]->first = request.motor_index;
00518       this->debug_motor_indexes_and_data[request.publisher_index]->second = request.motor_data_type;
00519       this->debug_mutex.unlock();
00520     }
00521     else
00522     {
00523       response.success = false;
00524       return false;
00525     }
00526 
00527     response.success = true;
00528     return true;
00529   }
00530 #endif
00531 
00532   // Only to ensure that the template class is compiled for the types we are interested in
00533   template
00534   class SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
00535 
00536   template
00537   class SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
00538 }  // namespace shadow_robot
00539 
00540 /* For the emacs weenies in the crowd.
00541 Local Variables:
00542    c-basic-offset: 2
00543 End:
00544  */


sr_robot_lib
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Mon Jul 1 2019 20:06:26