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


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:37:50