PowerCubeCtrl.cpp
Go to the documentation of this file.
00001 
00060 // ROS includes
00061 #include <ros/ros.h>
00062 
00063 // own includes
00064 #include <schunk_powercube_chain/PowerCubeCtrl.h>
00065 
00066 #define PCTRL_CHECK_INITIALIZED()                               \
00067   if ( isInitialized()==false )                                 \
00068     {                                                           \
00069       m_ErrorMessage.assign("Manipulator not initialized.");    \
00070       return false;                                             \
00071     }
00072 
00073 #define Ready4MoveStep 4638
00074 
00075 /*
00076  * \brief Constructor
00077  */
00078 PowerCubeCtrl::PowerCubeCtrl(PowerCubeCtrlParams * params)
00079 {
00080   m_mutex = PTHREAD_MUTEX_INITIALIZER;
00081 
00082   m_CANDeviceOpened = false;
00083   m_Initialized = false;
00084 
00085   m_params = params;
00086 
00087   m_horizon = 0.01; // sec
00088 
00089   m_last_time_pub = ros::Time::now();
00090 
00091         m_pc_status = PC_CTRL_OK; 
00092 }
00093 
00094 /*
00095  * \brief Destructor
00096  */
00097 PowerCubeCtrl::~PowerCubeCtrl()
00098 {               
00099   // stop all components
00100   Stop(); 
00101         
00102   // close CAN device
00103   if (m_CANDeviceOpened)
00104     {
00105       pthread_mutex_lock(&m_mutex);
00106       PCube_closeDevice(m_DeviceHandle);
00107       pthread_mutex_unlock(&m_mutex);
00108     }
00109 }
00110 
00112 
00117 bool PowerCubeCtrl::Init(PowerCubeCtrlParams * params)
00118 {
00119   int ret = 0;
00120   int DOF = m_params->GetDOF();
00121   std::string CanModule = m_params->GetCanModule();
00122   std::string CanDevice = m_params->GetCanDevice();
00123   std::vector<int> ModulIDs = m_params->GetModuleIDs();
00124   int CanBaudrate = m_params->GetBaudrate();
00125   std::vector<double> MaxVel = m_params->GetMaxVel();
00126   std::vector<double> MaxAcc = m_params->GetMaxAcc();
00127   std::vector<double> Offsets = m_params->GetOffsets();
00128   std::vector<double> LowerLimits = m_params->GetLowerLimits();
00129   std::vector<double> UpperLimits = m_params->GetUpperLimits();
00130 
00132   std::cout << " D  O  F  :" << DOF << std::endl;
00133   m_status.resize(DOF);
00134   m_ModuleTypes.resize(DOF);
00135   m_version.resize(DOF); 
00136   m_dios.resize(DOF);
00137   m_positions.resize(DOF);
00138   m_velocities.resize(DOF);
00139 
00140   std::cout << "=========================================================================== " << std::endl;
00141   std::cout << "PowerCubeCtrl:Init: Trying to initialize with the following parameters: " << std::endl;
00142   std::cout << "DOF: " << DOF << std::endl;
00143   std::cout << "CanModule: " << CanModule << std::endl;
00144   std::cout << "CanDevice: " << CanDevice << std::endl;
00145   std::cout << "CanBaudrate: " << CanBaudrate << std::endl;
00146   std::cout << "ModulIDs: ";
00147   for (int i = 0; i < DOF; i++)
00148     {
00149       std::cout << ModulIDs[i] << " ";
00150     }
00151   std::cout << std::endl;
00152 
00153   std::cout << std::endl << "maxVel: ";
00154   for (int i = 0; i < DOF; i++)
00155     {
00156       std::cout << MaxVel[i] << " ";
00157     }
00158 
00159   std::cout << std::endl << "maxAcc: ";
00160   for (int i = 0; i < DOF; i++)
00161     {
00162       std::cout << MaxAcc[i] << " ";
00163     }
00164 
00165   std::cout << std::endl << "upperLimits: ";
00166   for (int i = 0; i < DOF; i++)
00167     {
00168       std::cout << UpperLimits[i] << " ";
00169     }
00170 
00171   std::cout << std::endl << "lowerLimits: ";
00172   for (int i = 0; i < DOF; i++)
00173     {
00174       std::cout << LowerLimits[i] << " ";
00175     }
00176 
00177   std::cout << std::endl << "offsets: ";
00178   for (int i = 0; i < DOF; i++)
00179     {
00180       std::cout << Offsets[i] << " ";
00181     }
00182 
00183   std::cout << std::endl << "=========================================================================== " << std::endl;
00184   std::ostringstream InitStr;
00185   InitStr << CanModule << ":" << CanDevice << "," << CanBaudrate;
00186   std::cout << "initstring = " << InitStr.str().c_str() << std::endl;
00187 
00189   pthread_mutex_lock(&m_mutex);
00190   ret = PCube_openDevice(&m_DeviceHandle, InitStr.str().c_str());
00191   pthread_mutex_unlock(&m_mutex);
00192   if (ret != 0)
00193     {
00194       std::ostringstream errorMsg;
00195       errorMsg << "Could not open device " << CanDevice << ", m5api error code: " << ret;
00196       m_ErrorMessage = errorMsg.str();
00197       return false;
00198     }
00199 
00200   m_CANDeviceOpened = true;
00201 
00203         int max_tries = 3; 
00204         for (int i = 0; i < DOF; i++)
00205         {       
00206                 for (int reset_try = 0; reset_try < max_tries; reset_try++)
00207                 {       
00208                         pthread_mutex_lock(&m_mutex);
00209                         ret =  PCube_resetModule(m_DeviceHandle, ModulIDs.at(i));
00210                         pthread_mutex_unlock(&m_mutex);
00211                         
00212                         if (ret == 0)
00213                         {       
00214                                 break; 
00215                         }
00216                         else if ((ret != 0) && (reset_try == (max_tries-1)))
00217                         {
00218                                 std::ostringstream errorMsg;
00219         errorMsg << "Could not reset module " << ModulIDs.at(i) << " during init. Errorcode during reset: " << ret << " Try to init once more.";
00220         m_ErrorMessage = errorMsg.str();
00221                                 return false;
00222                         }
00223                         else
00224                         {
00225                                 // little break
00226                                 usleep(1500000); 
00227                         }
00228                         
00229                 }
00230         }
00231         std::cout << "number of moduleIDs" << ModulIDs.size() << std::endl;
00232 
00234   pthread_mutex_lock(&m_mutex);
00235   int number_of_modules = PCube_getModuleCount(m_DeviceHandle);
00236   pthread_mutex_unlock(&m_mutex);
00237   std::cout << "found " << number_of_modules << " modules." << std::endl;
00238 
00240   for (int i = 0; i < DOF; i++)
00241   {
00242       unsigned long serNo;
00243       unsigned short verNo;
00244       unsigned long defConfig;
00245       std::vector<std::string> Module_Types; 
00246                 
00248       pthread_mutex_lock(&m_mutex);
00249       ret = PCube_getModuleSerialNo(m_DeviceHandle, ModulIDs[i], &serNo);
00250       pthread_mutex_unlock(&m_mutex);
00251       if (ret != 0)
00252                         {
00253                                 std::ostringstream errorMsg;
00254                                 errorMsg << "Could not find Module with ID " << ModulIDs[i] << ", m5api error code: " << ret;
00255                                 m_ErrorMessage = errorMsg.str();        
00256                                 return false;
00257                         }
00258                 
00260       pthread_mutex_lock(&m_mutex);
00261       ret = PCube_getModuleVersion(m_DeviceHandle, ModulIDs[i], &verNo);
00262       pthread_mutex_unlock(&m_mutex);
00263       if (ret != 0)
00264                         {
00265                                 std::ostringstream errorMsg;
00266                                 errorMsg << "Could not find Module with ID " << ModulIDs[i] << ", m5api error code: " << ret;
00267                                 m_ErrorMessage = errorMsg.str();        
00268                                 return false;
00269                         }
00270                         else
00271                         {
00272                           m_version[i] = verNo; 
00273                         }       
00274                         
00276                         float gear_ratio;       
00277       pthread_mutex_lock(&m_mutex);
00278       ret = PCube_getDefGearRatio(m_DeviceHandle, ModulIDs[i], &gear_ratio);
00279       pthread_mutex_unlock(&m_mutex);
00280       if (ret != 0)
00281                         {
00282                                 std::ostringstream errorMsg;
00283                                 errorMsg << "Could not get Module type with ID " << ModulIDs[i] << ", m5api error code: " << ret;
00284                                 m_ErrorMessage = errorMsg.str();        
00285                                 return false;
00286                         }
00287                         else
00288                         {
00289                                 if (true)
00290                                 {
00291                                 std::cout << "gear ratio: " << gear_ratio << std::endl; 
00292                                         //return false; 
00293                                 } 
00294                         }       
00295                         
00297                         unsigned char type;     
00298       pthread_mutex_lock(&m_mutex);
00299       ret = PCube_getModuleType(m_DeviceHandle, ModulIDs[i], &type);
00300       pthread_mutex_unlock(&m_mutex);
00301       if (ret != 0)
00302                         {
00303                                 std::ostringstream errorMsg;
00304                                 errorMsg << "Could not get Module type with ID " << ModulIDs[i] << ", m5api error code: " << ret;
00305                                 m_ErrorMessage = errorMsg.str();        
00306                                 return false;
00307                         }
00308                         else
00309                         {
00310                                 if (type != TYPEID_MOD_ROTARY)
00311                                 {
00312                                 std::cout << "wrong module type configured. Type must be rotary axis. Use Windows configuration software to change type." << std::endl; 
00313                                         return false; 
00314                                 } 
00315                         }       
00316 
00318                         // the typ -if PW or PRL- can be distinguished by the typ of encoder. 
00319                         pthread_mutex_lock(&m_mutex);
00320                         ret = PCube_getDefSetup(m_DeviceHandle, ModulIDs[i], &defConfig);
00321                         pthread_mutex_unlock(&m_mutex);
00322 
00323                         ROS_DEBUG("module type check: %li (std::dec)",defConfig); 
00324                         
00325                 if (ret != 0)
00326                         {
00327                         std::ostringstream errorMsg;
00328                                 errorMsg << "Error on communication with module " << ModulIDs[i] << ", m5api error code: " << ret;
00329                                 m_ErrorMessage = errorMsg.str();        
00330                                 return false;
00331                         }       
00332                 
00333                         // Firmware version 4634 of PRL modules replies ABSOULTE_FEEDBACK, firmware 4638 replies RESOLVER_FEEDBACK. 
00334                         // Both means the same: Module is PRL. PW modules have encoders (ENCODER_FEEDBACK, s.M5API), but this bit is not set is DefConfig word.
00335                         // For new firmware versions this needs to be evaluated. 
00336                         if (((defConfig & CONFIG_ABSOLUTE_FEEDBACK)==CONFIG_ABSOLUTE_FEEDBACK) || ((defConfig & CONFIG_RESOLVER_FEEDBACK)==CONFIG_RESOLVER_FEEDBACK))
00337                         {
00338                                 m_ModuleTypes[i] = "PRL"; 
00339                                 ROS_DEBUG("Module %i is from type: PRL", i);
00340                         }
00341                 else
00342                         {
00343                                 m_ModuleTypes[i] = "PW"; 
00344                                 ROS_DEBUG("Module %i is from type: PW", i);
00345                         }
00346 
00348                         std::cout << "Found module " << std::dec << ModulIDs[i] << " Serial: " << serNo << " Version: " << std::hex << verNo << std::endl;
00349 
00350                 }
00351         
00352   // modules should be initialized now
00353   m_pc_status = PC_CTRL_OK;
00354   m_Initialized = true; 
00355 
00357   std::vector<std::string> errorMessages;
00358   PC_CTRL_STATUS status;
00359 
00360   // update status variables
00361   updateStates();
00362 
00363   // grep updated status 
00364   getStatus(status, errorMessages);
00365         
00366   // homing dependant on moduletype and if already homed
00367   bool successful = false;
00368   successful = doHoming();
00369   if (!successful)
00370     {
00371       std::cout << "PowerCubeCtrl:Init: homing not successful, aborting ...\n";
00372       return false;
00373     }
00374 
00375   // All modules initialized successfully
00376   m_pc_status = PC_CTRL_OK;
00377   return true;
00378 }
00379 
00383 bool PowerCubeCtrl::Close()
00384 {
00385   if (m_CANDeviceOpened)
00386     {
00387       m_Initialized = false;
00388       m_CANDeviceOpened = false;
00389 
00390       pthread_mutex_lock(&m_mutex);
00391       PCube_closeDevice(m_DeviceHandle);
00392       pthread_mutex_unlock(&m_mutex);
00393 
00394       return true;
00395     }
00396 
00397   else
00398     {
00399       return false;
00400     }
00401 }
00402 
00403 /*
00404  * \brief Move joints with calculated velocities
00405  *
00406  * Calculating positions and times by desired value of the cob_trajectory_controller
00407  * \param velocities Vector
00408  */
00409 bool PowerCubeCtrl::MoveVel(const std::vector<double>& vel)
00410 {
00411   PCTRL_CHECK_INITIALIZED();
00412   
00413   //== init var ==================================================
00414 
00416         unsigned int DOF = m_params->GetDOF();
00417         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00418         
00419         std::vector<double> velocities; 
00420         velocities.resize(DOF); 
00421         velocities = vel;                               // temp var for velocity because of const reference of the function
00422         
00423         std::ostringstream errorMsg;                    // temp error msg for being copied to m_errorMessage
00424         
00425         std::vector<std::string> errorMessages;         // temp error msg for call of getStatus()
00426         PC_CTRL_STATUS status;                                                                          // PowerCube Ctrl status variable
00427 
00428         std::vector<float> delta_pos;                   // traviling distance for next command
00429         std::vector<float> delta_pos_horizon;
00430         delta_pos.resize(DOF);
00431         delta_pos_horizon.resize(DOF);
00432         
00433         std::vector<float> target_pos;          // absolute target postion that is desired with this command
00434         std::vector<float> target_pos_horizon;  
00435         
00436         target_pos.resize(DOF); 
00437         target_pos_horizon.resize(DOF);
00438         float target_time;      // time in milliseconds
00439         float target_time_horizon = 0;
00440   
00441         float delta_t;                  // time from the last moveVel cmd to now
00442 
00444         std::vector<double> LowerLimits = m_params->GetLowerLimits();
00445         std::vector<double> UpperLimits = m_params->GetUpperLimits();
00446         std::vector<double> maxVels = m_params->GetMaxVel();
00447 
00449         std::vector<double> Offsets = m_params->GetOffsets();
00450         
00451         int ret;                // temp return value holder
00452         float pos;      // temp position variable for PCube_move.. cmds 
00453 
00454         //== calculate destination position ============================
00455         // needed for limit handling and MoveStep command
00456         
00457   delta_t = ros::Time::now().toSec() - m_last_time_pub.toSec();
00458         
00459   m_last_time_pub = ros::Time::now();  
00460 
00461   std::vector<double> pos_temp;
00462   pos_temp.resize(DOF);
00463 
00464   for (unsigned int i = 0; i < DOF; i++)
00465   {
00466     // limit step time to 50msec
00467     //TODO: set value 0.05 as parameter
00468         if (delta_t >= 0.050)
00469           {
00470             target_time = 0.050; //sec
00471           }
00472         else
00473           {
00474             target_time = delta_t;      //sec
00475           }
00476         
00477         //add horizon to time before calculation of target position, to influence both time and position at the same time
00478         target_time_horizon = target_time + (float)m_horizon; //sec
00479         // calculate travel distance
00480         delta_pos_horizon[i] = target_time_horizon * velocities[i];
00481         delta_pos[i] = target_time * velocities[i];
00482         
00483                 ROS_DEBUG("delta_pos[%i]: %f target_time: %f velocitiy[%i]: %f",i ,delta_pos[i], target_time, i, velocities[i]);
00484  
00485                 // calculate target position   
00486                 target_pos_horizon[i] = m_positions[i] + delta_pos_horizon[i] - Offsets[i];
00487                 ROS_DEBUG("target_pos[%i]: %f m_position[%i]: %f",i ,target_pos[i], i, m_positions[i]);
00488         }
00489 
00490         //== check input parameter =====================================
00491   
00493   if (velocities.size() != DOF)
00494     {
00495       m_ErrorMessage = "Skipping command: Commanded velocities and DOF are not same dimension.";
00496       return false;
00497     }
00498 
00499   for (unsigned int i = 0; i < DOF; i++)
00500     {
00502       if(velocities[i] > maxVels[i])
00503                         { 
00504                                 // set velocities command to max value
00505                                 velocities[i] = maxVels[i];
00506                 
00507                                 //TODO: add ros_warn    
00508 
00509                                 ROS_INFO("Velocity %f exceeds limit %f for axis %i. moving with max velocity %f instead", velocities[i], maxVels[i], i, maxVels[i]);
00510                         }
00511 
00513                         // TODO: add second limit "safty limit" 
00514                         // if target position is outer limits and the command velocity is in in direction away from working range, skip command
00515       if ((target_pos[i] < LowerLimits[i]+Offsets[i]) && (velocities[i] < 0))
00516                         {       
00517                                 ROS_INFO("Skipping command: %f Target position exceeds lower limit (%f).", target_pos[i], LowerLimits[i]);              
00518                                 // target position is set to actual position and velocity to Null. So only movement in the non limit direction is possible.
00519         
00520                                 pthread_mutex_lock(&m_mutex);
00521                                 PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
00522                                 pthread_mutex_unlock(&m_mutex);
00523                                 
00524                                 return true; 
00525                         } 
00526                         
00527                         // if target position is outer limits and the command velocity is in in direction away from working range, skip command
00528                         if ((target_pos[i] > UpperLimits[i]+Offsets[i]) && (velocities[i] > 0))
00529                         {       
00530                                 ROS_INFO("Skipping command: %f Target position exceeds upper limit (%f).", target_pos[i], UpperLimits[i]);              
00531                                 // target position is set to actual position. So only movement in the non limit direction is possible.
00532                                 
00533                                 pthread_mutex_lock(&m_mutex);
00534                                 PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
00535                                 pthread_mutex_unlock(&m_mutex);
00536 
00537                                 return true; 
00538                         } 
00539                 }
00540 
00541   //== check system status ====================================== 
00542   
00543   getStatus(status, errorMessages);
00544         
00545   if ((status != PC_CTRL_OK))
00546   {
00547     m_ErrorMessage.assign("");
00548     for (unsigned int i = 0; i < DOF; i++)
00549                 {
00550                 m_ErrorMessage.append(errorMessages[i]);
00551                 }
00552                 ROS_INFO("Error during movement. Status: %i     \n", status);
00553     return false;
00554   }
00555 
00556   //== send velocity cmd to modules ============================== 
00557 
00558         //convert the time to int in [ms]
00559         unsigned short time4motion = (unsigned short)((target_time_horizon)*1000.0);
00560 
00561         for (unsigned int i = 0; i < DOF; i++)
00562         {
00563                 // check module type sending command (PRL-Modules can be driven with moveStepExtended(), PW-Modules can only be driven with less safe moveVelExtended())
00564                 if ((m_ModuleTypes.at(i) == "PRL") && (m_version[i] >= Ready4MoveStep) && !m_params->GetUseMoveVel())
00565                 {
00566                         //ROS_DEBUG("Modul_id = %i, ModuleType: %s, step=%f, time=%f", m_params->GetModuleID(i), m_ModuleTypes[i].c_str(), target_pos[i], target_time);
00567 
00568                         //ROS_INFO("Modul_id = %i, ModuleType: %s, step=%f, time4motion=%i [ms], target_time=%f, horizon: %f", m_params->GetModuleID(i), m_ModuleTypes[i].c_str(), delta_pos[i], time4motion, target_time, m_horizon);
00569                         
00570                         pthread_mutex_lock(&m_mutex);
00571                         ret = PCube_moveStepExtended(m_DeviceHandle, m_params->GetModuleID(i), target_pos_horizon[i], time4motion, &m_status[i], &m_dios[i], &pos);
00572                         pthread_mutex_unlock(&m_mutex);
00573                 }
00574                 else    
00575                 {               
00576                         pthread_mutex_lock(&m_mutex);
00577                         ret = PCube_moveVelExtended(m_DeviceHandle, m_params->GetModuleID(i), velocities[i], &m_status[i], &m_dios[i], &pos);
00578                         pthread_mutex_unlock(&m_mutex);
00579                 }
00580 
00582                 if (ret != 0)
00583                 {
00584                         ROS_DEBUG("Com Error: %i", ret);                  
00585                         pos = m_positions[i];
00586                         //m_pc_status = PC_CTRL_ERR;
00587                         //TODO: add error msg for diagnostics if error occours often
00588                 }
00589                 
00590                 // !!! Position in pos is position before moveStep movement, to get the expected position after the movement (required as input to the next moveStep command) we add the delta position (cmd_pos) !!!
00591                 m_positions[i] = (double)pos + delta_pos[i] + Offsets[i];
00592                 
00593                 pos_temp[i] = (double)pos;
00594                 //ROS_INFO("After cmd (%X) :m_positions[%i] %f = pos: %f + delta_pos[%i]: %f",m_status[i], i, m_positions[i], pos, i, delta_pos[i]);
00595         }
00596 
00597         updateVelocities(pos_temp, delta_t);
00598         
00599         //std::cout << "vel_com: " << velocities[1] << " vel_hori: " << delta_pos_horizon[1]/   target_time_horizon << " vel_real[1]: " << m_velocities.at(1) << std::endl;
00600 
00601         pthread_mutex_lock(&m_mutex);
00602         PCube_startMotionAll(m_DeviceHandle);
00603         pthread_mutex_unlock(&m_mutex);
00604         
00605         return true;
00606 }
00607 
00608 // Calculation of velocities based on vel = 1/(6*dt) * (-pos(t-3) - 3*pos(t-2) + 3*pos(t-1) + pos(t))
00609 void PowerCubeCtrl::updateVelocities(std::vector<double> pos_temp, double delta_t)
00610 {
00611   unsigned int DOF = m_params->GetDOF();
00612 
00613         if (m_cached_pos.empty())
00614         {       
00615                 std::vector<double> null; 
00616                 for (unsigned int i=0;i<DOF;i++){       null.push_back(0); }  
00617 
00618                 m_cached_pos.push_back(null);
00619                 m_cached_pos.push_back(null);
00620                 m_cached_pos.push_back(null);
00621                 m_cached_pos.push_back(null);
00622         }
00623 
00624         m_cached_pos.push_back(pos_temp);
00625         m_cached_pos.pop_front();
00626 
00627         std::vector<double> last_pos = m_cached_pos.front();
00628 
00629     for(unsigned int i = 0; i < DOF; i++)
00630         {
00631                 m_velocities[i] = 1/(6*delta_t) * (-m_cached_pos[0][i]-(3*m_cached_pos[1][i])+(3*m_cached_pos[2][i])+m_cached_pos[3][i]);
00632                 //m_velocities[i] = (m_cached_pos[3][i] - m_cached_pos[2][i])/delta_t;          
00633                 //m_velocities[i] = (pos_temp.at(i)-last_pos.at(i))/delta_t; 
00634         }   
00635 }
00636 
00637 
00638 
00642 bool PowerCubeCtrl::Stop()
00643 {       
00645   unsigned int DOF = m_params->GetDOF();
00646         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00647 
00649 
00650 
00651         for (unsigned int i = 0; i < DOF; i++)
00652   {  
00653                 pthread_mutex_lock(&m_mutex);
00654                 int ret =  PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
00655                 pthread_mutex_unlock(&m_mutex);
00656                 if (ret != 0)
00657                   {
00658                     std::ostringstream errorMsg;
00659                     errorMsg << "Could not reset all modules, m5api error code: " << ret;
00660                     m_ErrorMessage = errorMsg.str();
00661                     return false;
00662                   }
00663         }
00664 
00666   usleep(500000);
00667 
00669         for (unsigned int i = 0; i < DOF; i++)
00670   {  
00671                 pthread_mutex_lock(&m_mutex);
00672                 int ret =  PCube_resetModule(m_DeviceHandle, ModulIDs.at(i));
00673                 pthread_mutex_unlock(&m_mutex);
00674                 if (ret != 0)
00675                   {
00676                     std::ostringstream errorMsg;
00677                     errorMsg << "Could not reset all modules, m5api error code: " << ret;
00678                     m_ErrorMessage = errorMsg.str();
00679                     return false;
00680                   }
00681         }
00682   return true;
00683 }
00684 
00688 bool PowerCubeCtrl::Recover()
00689 {       
00690   unsigned int DOF = m_params->GetDOF();
00691         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00692   std::vector<double> MaxVel = m_params->GetMaxVel();
00693   std::vector<double> MaxAcc = m_params->GetMaxAcc();
00694   std::vector<double> Offsets = m_params->GetOffsets();
00695   
00696   std::vector<std::string> errorMessages;
00697   PC_CTRL_STATUS status;
00698         
00699   unsigned long state = PC_CTRL_OK;
00700   unsigned char dio;
00701   float position;
00702   int ret = 0;
00703         
00704   // check for each module if reset is necessary
00705   for (unsigned int i = 0; i < DOF; i++)
00706   {     
00707     pthread_mutex_lock(&m_mutex);
00708     ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
00709     pthread_mutex_unlock(&m_mutex);
00710     if (ret != 0)
00711                 {
00712                   m_pc_status = PC_CTRL_ERR;
00713                   std::cout << "State: Error com with Module: " << i << " Time: " << ros::Time::now() << std::endl;     
00714                   return false;                 
00715                 }
00716                 
00717       // if module is in error state --> reset
00718     if (state & STATEID_MOD_ERROR)
00719                 {       
00720                   pthread_mutex_lock(&m_mutex);
00721                   ret = PCube_resetModule(m_DeviceHandle, m_params->GetModuleID(i));
00722                   pthread_mutex_unlock(&m_mutex);
00723                   if (ret != 0)
00724             {
00725               m_pc_status = PC_CTRL_ERR;
00726               std::cout << "State: Error com with Module: " << i << " Time: " << ros::Time::now() << std::endl;         
00727               return false;             
00728             }
00729                 }
00730   }
00731         
00732   // time for reboot
00733   usleep(500000); 
00734 
00735   // check is everything is ok now
00736   updateStates(); 
00737 
00738   if (m_pc_status == PC_CTRL_NOT_HOMED)
00739   {
00740     if (!doHoming())
00741                 {
00742                   return false;
00743                 }
00744   }
00745         
00746   usleep(500000);
00747 
00748   // modules should be recovered now
00749   m_pc_status = PC_CTRL_OK;     
00750         
00751   updateStates(); 
00752   // check if modules are really back to normal state
00753   getStatus(status, errorMessages);
00754 
00755   if ((status != PC_CTRL_OK))
00756   {
00757           m_ErrorMessage.assign("");
00758 
00759     for (int i = 0; i < m_params->GetDOF(); i++)
00760                 {
00761                   m_ErrorMessage.append(errorMessages[i]);
00762                 }
00763     return false;
00764   }
00765 
00767   m_pc_status = PC_CTRL_OK;
00768   return true;
00769 }
00770 
00776 bool PowerCubeCtrl::setMaxVelocity(double maxVelocity)
00777 {
00778         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00779 
00780   PCTRL_CHECK_INITIALIZED();
00781   for (int i = 0; i < m_params->GetDOF(); i++)
00782     {
00783       pthread_mutex_lock(&m_mutex);
00784       int ret = PCube_setMaxVel(m_DeviceHandle, m_params->GetModuleID(i), maxVelocity);
00785       pthread_mutex_unlock(&m_mutex);
00786                         if (ret!=0)
00787                         {       
00788                                 std::ostringstream errorMsg;
00789                                 errorMsg << "Could not set MaxVelocity in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
00790                                 m_ErrorMessage = errorMsg.str();
00791                                 return false;
00792                         }
00793 
00794       std::vector<double> maxVelocities(maxVelocity);
00795       m_params->SetMaxVel(maxVelocities);
00796     }
00797 
00798   return true;
00799 }
00800 
00804 bool PowerCubeCtrl::setMaxVelocity(const std::vector<double>& maxVelocities)
00805 {       
00806   PCTRL_CHECK_INITIALIZED();
00807         
00808         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00809         
00810   for (int i = 0; i < m_params->GetDOF(); i++)
00811     {
00812       pthread_mutex_lock(&m_mutex);
00813       //std::cout << "------------------------------> PCube_setMaxVel()" << std::endl;
00814       int ret = PCube_setMaxVel(m_DeviceHandle, m_params->GetModuleID(i), maxVelocities[i]);
00815       pthread_mutex_unlock(&m_mutex);
00816                         if (ret!=0)
00817                         {       
00818                                 std::ostringstream errorMsg;
00819                                 errorMsg << "Could not set MaxVelocity in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
00820                                 m_ErrorMessage = errorMsg.str();
00821                                 return false;
00822                         }
00823 
00824       m_params->SetMaxVel(maxVelocities);
00825     }
00826 
00827   return true;
00828 }
00829 
00835 bool PowerCubeCtrl::setMaxAcceleration(double maxAcceleration)
00836 {
00837   PCTRL_CHECK_INITIALIZED();
00838         
00839         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00840 
00841   for (int i = 0; i < m_params->GetDOF(); i++)
00842     {
00843       pthread_mutex_lock(&m_mutex);
00844       //std::cout << "------------------------------> PCube_setMaxAcc()" << std::endl;
00845       int ret = PCube_setMaxAcc(m_DeviceHandle, m_params->GetModuleID(i), maxAcceleration);
00846       pthread_mutex_unlock(&m_mutex);
00847                         if (ret!=0)
00848                         {       
00849                                 std::ostringstream errorMsg;
00850                                 errorMsg << "Could not set MaxAcceleration in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
00851                                 m_ErrorMessage = errorMsg.str();
00852                                 return false;
00853                         }
00854 
00855       std::vector<double> maxAccelerations(maxAcceleration);
00856       m_params->SetMaxAcc(maxAccelerations);
00857     }
00858 
00859   return true;
00860 }
00861 
00865 bool PowerCubeCtrl::setMaxAcceleration(const std::vector<double>& maxAccelerations)
00866 {
00867   PCTRL_CHECK_INITIALIZED();
00868         
00869         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00870 
00871   for (int i = 0; i < m_params->GetDOF(); i++)
00872     {
00873       pthread_mutex_lock(&m_mutex);
00874       int ret = PCube_setMaxAcc(m_DeviceHandle, m_params->GetModuleID(i), maxAccelerations[i]);
00875       pthread_mutex_unlock(&m_mutex);
00876                         if (ret!=0)
00877                         {       
00878                                 std::ostringstream errorMsg;
00879                                 errorMsg << "Could not set MaxAcceleration in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
00880                                 m_ErrorMessage = errorMsg.str();
00881                                 return false;
00882                         }
00883 
00884       m_params->SetMaxAcc(maxAccelerations);
00885     }
00886 
00887   return true;
00888 }
00889 
00896 bool PowerCubeCtrl::setHorizon(double horizon)
00897 {
00898   m_horizon = horizon;
00899 
00900   return true;
00901 }
00902 
00909 double PowerCubeCtrl::getHorizon()
00910 {
00911   return m_horizon;
00912 }
00913 
00919 bool PowerCubeCtrl::setSyncMotion()
00920 {
00921         std::vector<int> ModulIDs = m_params->GetModuleIDs();
00922 
00923   if (m_CANDeviceOpened)
00924   {
00925                 for (int i = 0; i < m_params->GetDOF(); i++)
00926                 {
00927                 unsigned long confword;
00928 
00930                 pthread_mutex_lock(&m_mutex);
00931                 PCube_getConfig(m_DeviceHandle, m_params->GetModuleID(i), &confword);
00932                 pthread_mutex_unlock(&m_mutex);
00933 
00935                 pthread_mutex_lock(&m_mutex);
00936                 int ret = PCube_setConfig(m_DeviceHandle, m_params->GetModuleID(i), confword | CONFIGID_MOD_SYNC_MOTION);
00937                 pthread_mutex_unlock(&m_mutex);
00938                         if (ret!=0)
00939                         {       
00940                                 std::ostringstream errorMsg;
00941                                 errorMsg << "Could not set SyncMotion in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
00942                                 m_ErrorMessage = errorMsg.str();
00943                                 return false;
00944                         }
00945                 }
00946       return true;
00947     }
00948   else
00949     {
00950       return false;
00951     }
00952 }
00958 bool PowerCubeCtrl::setASyncMotion()
00959 {
00960   if (m_CANDeviceOpened)
00961     {
00962       for (int i = 0; i < m_params->GetDOF(); i++)
00963         {
00964           unsigned long confword;
00965 
00967           pthread_mutex_lock(&m_mutex);
00968           PCube_getConfig(m_DeviceHandle, m_params->GetModuleID(i), &confword);
00969           pthread_mutex_unlock(&m_mutex);
00970 
00972           pthread_mutex_lock(&m_mutex);
00973           PCube_setConfig(m_DeviceHandle, m_params->GetModuleID(i), confword & (~CONFIGID_MOD_SYNC_MOTION));
00974           pthread_mutex_unlock(&m_mutex);
00975         }
00976       return true;
00977     }
00978 
00979   else
00980     {
00981       return false;
00982     }
00983 }
00987 bool PowerCubeCtrl::updateStates()
00988 {       
00989   PCTRL_CHECK_INITIALIZED();
00990 
00991   unsigned int DOF = m_params->GetDOF();
00992   unsigned long state;
00993   PC_CTRL_STATUS pc_status = PC_CTRL_ERR; 
00994   std::vector<std::string> ErrorMessages;
00995   std::vector<double> Offsets = m_params->GetOffsets();
00996   std::ostringstream errorMsg;
00997 
00998   unsigned char dio;
00999   float position;
01000   int ret = 0;
01001 
01002   for (unsigned int i = 0; i < DOF; i++)
01003     {   
01004       state = m_status[i]; 
01005       pthread_mutex_lock(&m_mutex);
01006       ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
01007       pthread_mutex_unlock(&m_mutex);
01008                 
01009       if (ret != 0)
01010         {
01011           //m_pc_status = PC_CTRL_ERR;
01012           ROS_DEBUG("Error on com in UpdateStates");
01013           return true;
01014           //errorMsg << "State: Error com with Module [" <<  i << "]";
01015           //ErrorMessages[i] = errorMsg.str();                  
01016         }
01017       else
01018         {       
01019           ROS_DEBUG("Module %i, State: %li, Time: %f",i, state, ros::Time::now().toSec());
01020           
01021           m_status[i] = state;          
01022           m_dios[i] = dio;
01023           m_positions[i] = position + Offsets[i];
01024         }       
01025       
01026     }
01027 
01028         /*
01029   double delta_t = ros::Time::now().toSec() - m_last_time_pub.toSec();
01030   m_last_time_pub = ros::Time::now();  
01031 
01032   updateVelocities(m_positions, delta_t);
01033         */
01034 
01035   // evaluate state for translation for diagnostics msgs
01036   getStatus(pc_status, ErrorMessages);
01037 
01038   for (unsigned int i=0; i<ErrorMessages.size(); i++)
01039   {     
01040                 m_ErrorMessage.clear();
01041                 m_ErrorMessage.assign("");
01042     m_ErrorMessage.append(ErrorMessages[i]);
01043   }
01044 
01045   return true;
01046 }
01047 
01051 bool PowerCubeCtrl::getStatus(PC_CTRL_STATUS& status, std::vector<std::string>& errorMessages)
01052 {       
01053   unsigned int DOF = m_params->GetDOF();
01054   std::vector<int> ModuleIDs = m_params->GetModuleIDs();
01055         
01056         std::vector<PC_CTRL_STATUS> StatusArray;
01057         StatusArray.resize(DOF);        
01058 
01059   errorMessages.clear();
01060   errorMessages.resize(DOF);
01061 
01062   status = PC_CTRL_OK;
01063 
01064   for (unsigned int i = 0; i < DOF; i++)
01065   {
01066                 std::ostringstream errorMsg;
01067                 
01068                 if (m_status[i] & STATEID_MOD_POWERFAULT)
01069                 {       
01070                         if (m_status[i] & STATEID_MOD_POW_VOLT_ERR)
01071                         {
01072                                 errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
01073                                 errorMsg << "Motor voltage below minimum value! Check Emergency Stop!";
01074                                 errorMessages[i] = errorMsg.str();
01075                         }
01076                         else if (m_status[i] & STATEID_MOD_POW_FET_TEMP)
01077                         {
01078                                 errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
01079                                 errorMsg << "Overheated power transitors! Power must be switched of to reset this error.";
01080                                 errorMessages[i] = errorMsg.str();
01081                         }
01082                         else if (m_status[i] & STATEID_MOD_POW_INTEGRALERR)
01083                         {
01084                                 errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
01085                                 errorMsg << "The drive has been overloaded and the servo loop has been disabled. Power must be switched off to reset this error.";
01086                                 errorMessages[i] = errorMsg.str();
01087                         }
01088                         StatusArray[i] = PC_CTRL_POW_VOLT_ERR;
01089                 }
01090     else if (m_status[i] & STATEID_MOD_TOW_ERROR)
01091                 {
01092                         errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
01093                         errorMsg << "The servo loop was not able to follow the target position!";
01094                         errorMessages[i] = errorMsg.str();
01095                         StatusArray[i] = PC_CTRL_ERR;
01096                 }
01097     else if (m_status[i] & STATEID_MOD_ERROR)
01098                 {       
01099                         // STOP the motion for each module
01100                         pthread_mutex_lock(&m_mutex);
01101                         PCube_haltModule(m_DeviceHandle, m_params->GetModuleID(i));
01102                         pthread_mutex_unlock(&m_mutex);
01103                          
01104                         errorMsg << "Error in  Module " << ModuleIDs[i];
01105                         errorMsg << " : Status code: " << std::hex << m_status[i];
01106                         errorMessages[i] = errorMsg.str();
01107                         StatusArray[i] = PC_CTRL_ERR;
01108                 }
01109     else if (m_pc_status & PC_CTRL_ERR)
01110                 {       
01111                         Stop(); // stop all motion
01112                         errorMsg << "PowerCubeCtrl is in global error state";
01113                         errorMessages[i] = errorMsg.str();
01114                         StatusArray[i] =  PC_CTRL_ERR;
01115                 }
01116     else
01117                 {
01118                         errorMsg << "Module with Id " << ModuleIDs[i];
01119                         errorMsg << ": Status OK.";
01120                         errorMessages[i] = errorMsg.str();
01121                         StatusArray[i] = PC_CTRL_OK;
01122                 }
01123         }
01124         
01125         // search for worst status
01126         for (unsigned int i = 0; i < DOF; i++)
01127   {     
01128                 if ((int)StatusArray[i] <= (int)status)
01129                 {
01130                         status = StatusArray[i]; 
01131                 }       
01132         }
01133         
01134         m_pc_status = status;
01135 
01136   return true;
01137 }
01138 
01142 std::vector<unsigned long> PowerCubeCtrl::getVersion()
01143 {
01144   return m_version; 
01145 }
01149 bool PowerCubeCtrl::statusMoving()
01150 {
01151   PCTRL_CHECK_INITIALIZED();
01152 
01153   for (int i = 0; i < m_params->GetDOF(); i++)
01154     {
01155       if (m_status[i] & STATEID_MOD_MOTION)
01156         return true;
01157     }
01158   return false;
01159 }
01160 
01164 std::vector<double> PowerCubeCtrl::getPositions()
01165 {
01166   return m_positions;
01167 }
01168 
01172 std::vector<double> PowerCubeCtrl::getVelocities()
01173 {
01175   return m_velocities;
01176 }
01177 
01181 std::vector<double> PowerCubeCtrl::getAccelerations()
01182 {
01184   return m_accelerations;
01185 }
01186 
01192 bool PowerCubeCtrl::doHoming()
01193 {
01194         unsigned int DOF = m_params->GetDOF();
01195         std::vector<int> ModuleIDs = m_params->GetModuleIDs();
01196         std::vector<double> LowerLimits = m_params->GetLowerLimits();
01197         std::vector<double> UpperLimits = m_params->GetUpperLimits();
01198 
01200         std::vector<double> Offsets = m_params->GetOffsets();
01201 
01203   double max_homing_time = 15.0; // seconds   
01204   double homing_time = 999.0;   // set to 0 if any module is homed
01205   double intervall = 0.1;
01206         
01207   unsigned long state = PC_CTRL_ERR;
01208   unsigned char dio;
01209   float position;
01210   int ret = 0;
01211         
01213   for (unsigned int i = 0; i < DOF; i++)
01214     {   
01215         pthread_mutex_lock(&m_mutex);
01216         ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
01217         pthread_mutex_unlock(&m_mutex);
01218                 
01219         // check and init m_position variable for trajectory controller
01220         if ((position > UpperLimits[i] + Offsets[i]) || (position < LowerLimits[i] + Offsets[i]))
01221         {       
01222                 std::ostringstream errorMsg;
01223                 errorMsg << "Module " << ModuleIDs[i] << " has position " << position << " that is outside limits (" << UpperLimits[i] + Offsets[i] << " <-> " << LowerLimits[i] + Offsets[i] << std::endl; 
01224                 if ((m_ModuleTypes.at(i)=="PW") || (m_ModuleTypes.at(i) == "other"))
01225                 {       std::cout << "Position error for PW-Module. Init is aborted. Try to reboot the robot." << std::endl;
01226                         m_ErrorMessage = errorMsg.str(); 
01227                         m_pc_status = PC_CTRL_ERR;
01228                         return false;
01229                 }
01230                 else if (m_ModuleTypes.at(i)=="PRL") 
01231                 {       
01232                         ROS_INFO("Position of Module %i is outside limits. Module can only be moved in opposite direction.",i );
01233                         ROS_INFO("Homing for Module: %i not necessary", m_params->GetModuleID(i));
01234                 }       
01235                 else 
01236                 {       
01237                         ROS_INFO("Module type incorrect. (in func. PowerCubeCtrl::doHoming();)");
01238                         return false;
01239                 }
01240         }
01241         else
01242         {
01243                 m_positions[i] = position + Offsets[i]; 
01244         }
01245 
01246         // check module type before homing (PRL-Modules need not to be homed by ROS)
01247         if ( (m_ModuleTypes.at(i) == "PW") || (m_ModuleTypes.at(i) == "other") )
01248         {       
01249                 pthread_mutex_lock(&m_mutex);
01250                 ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
01251                 pthread_mutex_unlock(&m_mutex);
01252         
01253                 if (ret != 0)
01254                 {       
01255                         ROS_INFO("Error on communication with Module: %i.", m_params->GetModuleID(i)); 
01256                         m_pc_status = PC_CTRL_ERR;
01257                         return false;
01258                 }
01259                         
01260                 // only do homing when necessary 
01261                 if (!(state & STATEID_MOD_HOME))
01262                 {
01263                         // homing timer
01264                         homing_time = 0.0;
01265 
01266                           pthread_mutex_lock(&m_mutex);
01267                           ret = PCube_homeModule(m_DeviceHandle, ModuleIDs[i]);
01268                           pthread_mutex_unlock(&m_mutex);
01269 
01270                           ROS_INFO("Homing started at: %f", ros::Time::now().toSec()); 
01271 
01272                           if (ret != 0)
01273                         {        
01274                           ROS_INFO("Error while sending homing command to Module: %i. I try to reset the module.", i); 
01275 
01276                           // reset module with the hope that homing works afterwards
01277                           pthread_mutex_lock(&m_mutex);
01278                           ret = PCube_resetModule(m_DeviceHandle, ModuleIDs[i]);
01279                           pthread_mutex_unlock(&m_mutex);
01280                           if (ret != 0)
01281                                 {       
01282                                   std::ostringstream errorMsg;
01283                                   errorMsg << "Can't reset module after homing error" << ModuleIDs[i] << ", m5api error code: " << ret;
01284                                   m_ErrorMessage = errorMsg.str();
01285                                 }
01286 
01287                           // little break for reboot
01288                           usleep(200000); 
01289 
01290                           pthread_mutex_lock(&m_mutex);
01291                           ret = PCube_homeModule(m_DeviceHandle, ModuleIDs[i]);
01292                           pthread_mutex_unlock(&m_mutex);
01293                           if (ret != 0)
01294                                 {       
01295                                   std::ostringstream errorMsg;
01296                                   errorMsg << "Can't start homing for module " << ModuleIDs[i] << ", tried reset with no success, m5api error code: " << ret;
01297                                   m_ErrorMessage = errorMsg.str();
01298                                   return false;                         
01299                                 }
01300 
01301                         }
01302             }else
01303             {
01304               ROS_INFO("Homing for Module: %i not necessary", m_params->GetModuleID(i));
01305             } 
01306         }else
01307         {
01308           ROS_INFO("Homing for PRL-Module %i not necessary", m_params->GetModuleID(i));
01309         } 
01310     }
01311 
01312   for (unsigned int i = 0; i < DOF; i++)
01313     {   
01314       unsigned long int help; 
01315       do
01316                         {
01317                                 pthread_mutex_lock(&m_mutex);
01318                                 PCube_getModuleState(m_DeviceHandle, ModuleIDs[i], &help);
01319                                 pthread_mutex_unlock(&m_mutex);
01320                                 ROS_DEBUG("Homing active for Module: %i State: %li", ModuleIDs.at(i), help);
01321 
01323                                 usleep(intervall * 1000000);    // convert sec to usec
01324                                 homing_time += intervall; 
01325                                 if (homing_time >= max_homing_time) {Stop(); break;} 
01326 
01327                         } while ((help & STATEID_MOD_HOME) == 0);
01328      
01329                         m_status[i] = help;
01330       ROS_DEBUG("State of Module %i : %li", ModuleIDs.at(i), help);
01331     }
01332 
01333   for (unsigned int i = 0; i < DOF; i++)
01334     {   
01336       if (!(m_status[i] & STATEID_MOD_HOME) || (m_status[i] & STATEID_MOD_ERROR) )
01337                         {
01338                                 std::cout << "Homing failed: Error in  Module " << ModuleIDs[i] << std::endl;
01339                                 m_pc_status = PC_CTRL_NOT_HOMED;
01340                                 return false;
01341                         }
01342                 
01343       ROS_INFO("Homing for Modul %i done.", ModuleIDs.at(i)); 
01344     }
01345 
01346   // modules successfully homed
01347   m_pc_status = PC_CTRL_OK;
01348   return true;
01349 }
01350 
01354 bool m_TranslateError2Diagnosics(std::ostringstream* errorMsg)
01355 {
01356   return true;
01357 }


schunk_powercube_chain
Author(s): Florian Weisshardt
autogenerated on Thu Aug 27 2015 15:06:58