simulatedArm.cpp
Go to the documentation of this file.
00001 
00060 #include <schunk_powercube_chain/simulatedArm.h>
00061 #include <schunk_powercube_chain/simulatedMotor.h>
00062 #include <math.h>
00063 
00064 #define PSIM_CHECK_INITIALIZED() \
00065 if ( isInitialized()==false )                                                                                   \
00066 {                                                                                                                                               \
00067     m_ErrorMessage.assign("Manipulator not initialized.");              \
00068         return false;                                                                                                           \
00069 }
00070 
00071 simulatedArm::simulatedArm()
00072 {
00073         std::cerr << "==============Starting Simulated Powercubes\n";
00074         m_DOF = 0;
00075         m_Initialized = false;
00076         m_motors.clear();
00077                         
00078 }
00079 
00080 simulatedArm::~simulatedArm()
00081 {
00082         // nothing to do...
00083         ;
00084 }
00085 
00086 bool simulatedArm::Init(PowerCubeCtrlParams * params)
00087 {
00088         m_DOF = params->GetNumberOfDOF();;
00089         
00090         double vmax = 1.0;
00091         double amax = 1.5;
00092         
00093         setMaxVelocity(vmax);
00094         setMaxAcceleration(amax);
00095         m_maxVel.resize(m_DOF);
00096         m_maxAcc.resize(m_DOF);
00097         
00098         std::vector<double> ul(m_DOF);
00099         std::vector<double> ll(m_DOF);
00100         ul = params->GetUpperLimits();
00101         ll = params->GetLowerLimits();
00102         m_maxVel = params->GetMaxVel();
00103         m_maxAcc =  params->GetMaxAcc();
00104         
00105 
00106         for (int i=0; i < m_DOF; i++)
00107         {
00108                 m_motors.push_back( simulatedMotor(ll[i], ul[i], amax, vmax) );
00109         }
00110         std::cerr << "===========Initializing Simulated Powercubes\n";
00111         m_Initialized = true;
00112         return true;
00113 }
00114 
00116 bool simulatedArm::Stop()
00117 {
00118         for (int i=0; i < m_DOF; i++)
00119                 m_motors[i].stop();
00120         return true;
00121 }
00122 
00123 bool simulatedArm::MoveJointSpaceSync(const std::vector<double>& target)
00124 {
00125         std::cerr << "======================TUTUTUTUT\n";
00126     PSIM_CHECK_INITIALIZED();
00127 
00128 
00129         std::vector<double> acc(m_DOF);
00130         std::vector<double> vel(m_DOF);
00131         
00132         double TG = 0;
00133         
00134 
00135         try 
00136         {               
00137                 // Ermittle Joint, der bei max Geschw. und Beschl. am längsten braucht:
00138 
00139                 std::vector<double> posnow;
00140                 posnow.resize(m_DOF);
00141                 if ( getConfig(posnow) == false )
00142                     return false;
00143                     
00144                 std::vector<double> velnow;
00145                 velnow.resize(m_DOF);
00146                 if ( getJointVelocities(velnow) == false )
00147                     return false;
00148                         
00149                 std::vector<double> times(m_DOF);
00150 
00151                 for (int i=0; i < m_DOF; i++)
00152                 {
00153                         RampCommand rm(posnow[i], velnow[i], target[i], m_maxAcc[i], m_maxVel[i]);
00154                         times[i] = rm.getTotalTime();
00155                 }
00156                 
00157                 // determine the joint index that has the greates value for time
00158                 int furthest = 0;
00159                 
00160                 double max = times[0];
00161         
00162             for (int i=1; i<m_DOF; i++)
00163             {
00164                     if (times[i] > max)
00165                     {
00166                             max = times[i];
00167                             furthest = i;
00168                     }
00169             }
00170                 
00171                 RampCommand rm_furthest(posnow[furthest], velnow[furthest], target[furthest], m_maxAcc[furthest], m_maxVel[furthest]);
00172                 
00173                 double T1 = rm_furthest.T1();
00174                 double T2 = rm_furthest.T2();
00175                 double T3 = rm_furthest.T3();
00176                 
00177                 // Gesamtzeit:
00178                 TG = T1 + T2 + T3;
00179                 
00180                 // Jetzt Geschwindigkeiten und Beschl. für alle: 
00181                 acc[furthest] = m_maxAcc[furthest];
00182                 vel[furthest] = m_maxVel[furthest];
00183                 
00184                 for (int i = 0; i < m_DOF; i++)
00185                 {
00186                         if (i != furthest)
00187                         {
00188                                 double a; double v;
00189                                 // a und v berechnen:
00190                                 RampCommand::calculateAV(
00191                                         posnow[i], 
00192                                         velnow[i], 
00193                                         target[i], 
00194                                         TG, T3, 
00195                                         m_maxAcc[i],
00196                                         m_maxVel[i],
00197                                         a, 
00198                                         v);
00199                                                         
00200                                 acc[i] = a;
00201                                 vel[i] = v;
00202                         }
00203                 }
00204         } 
00205         catch(...) 
00206         {
00207                 m_ErrorMessage.assign("Problem during calculation of a and av.");
00208                 return false;
00209         }
00210                 
00211         // Jetzt Bewegung starten:      
00212         for (int i=0; i < m_DOF; i++)
00213         {
00214                 std::cout << "moving motor " << i << ": " << target[i] << ": " << vel[i] << ": " << acc[i] << "\n";
00215                 m_motors[i].moveRamp(target[i], vel[i], acc[i]);
00216         }
00217         
00218         return true;    
00219 }
00220 
00221 bool simulatedArm::MoveVel(const std::vector<double>& Vel)
00222 {
00223     PSIM_CHECK_INITIALIZED();
00224 
00225 //      std::cerr << ".";
00226 //      std::cerr << "Vels: ";
00227         for (int i=0; i < m_DOF; i++)
00228         {
00229 //              std::cerr << Vel[i] << " ";
00230                 m_motors[i].moveVel(Vel[i]);
00231         }
00232 //      std::cerr << "\n";
00233         return true;
00234 }       
00235 
00236 
00237 bool simulatedArm::MovePos(const std::vector<double>& Pos)
00238 {
00239     PSIM_CHECK_INITIALIZED();
00240 
00241         for (int i=0; i < m_DOF; i++)
00242                 m_motors[i].movePos(Pos[i]);
00243         
00244         return true;
00245 }       
00246 
00247         
00249 // Funktionen zum setzen von Parametern: //
00251 
00253 bool simulatedArm::setMaxVelocity(double radpersec)
00254 {
00255     PSIM_CHECK_INITIALIZED();
00256     
00257     m_maxVel.resize(m_DOF);
00258 
00259         for (int i=0; i < m_DOF; i++)
00260         {
00261                 m_maxAcc[i] = radpersec;
00262                 m_motors[i].setMaxVelocity(radpersec);
00263         }
00264         
00265         return true;
00266 }
00267 
00268 bool simulatedArm::setMaxVelocity(const std::vector<double>& radpersec)
00269 {
00270     PSIM_CHECK_INITIALIZED();
00271     
00272     m_maxAcc = radpersec;
00273 
00274         for (int i=0; i < m_DOF; i++)
00275                 m_motors[i].setMaxVelocity(radpersec[i]);
00276         
00277         return true;
00278 }
00279 
00281 bool simulatedArm::setMaxAcceleration(double radPerSecSquared)
00282 {
00283     PSIM_CHECK_INITIALIZED();
00284 
00285         for (int i=0; i < m_DOF; i++)
00286                 m_motors[i].setMaxAcceleration(radPerSecSquared);
00287         
00288         return true;
00289 }
00290 
00291 bool simulatedArm::setMaxAcceleration(const std::vector<double>& radPerSecSquared)
00292 {
00293     PSIM_CHECK_INITIALIZED();
00294 
00295         for (int i=0; i < m_DOF; i++)
00296                 m_motors[i].setMaxAcceleration(radPerSecSquared[i]);
00297         
00298         return true;
00299 }
00300 
00301 
00303 // hier die Funktionen zur Statusabfrage: //
00305                 
00306                 
00308 bool simulatedArm::getConfig(std::vector<double>& result)
00309 {
00310     PSIM_CHECK_INITIALIZED();
00311 
00312         result.resize(m_DOF);
00313         for (int i=0; i < m_DOF; i++)
00314         {
00315                 result[i] = m_motors[i].getAngle();
00316         }
00317         
00318         return true;
00319 }       
00320 
00322 bool simulatedArm::getJointVelocities(std::vector<double>& result)
00323 {
00324     PSIM_CHECK_INITIALIZED();
00325 
00326         result.resize(m_DOF);
00327         for (int i=0; i < m_DOF; i++)
00328         {
00329                 result[i] = m_motors[i].getVelocity();
00330         }
00331         
00332         return true;
00333 }       
00334 
00337 bool simulatedArm::statusMoving()
00338 {
00339     PSIM_CHECK_INITIALIZED();
00340 
00341         for(int i=0; i<m_DOF; i++)
00342         {       
00343                 if (m_motors[i].statusMoving())
00344                         return true;
00345         }
00346         return false;
00347 }
00348                 
00349 
00351 bool simulatedArm::statusDec()
00352 {
00353     PSIM_CHECK_INITIALIZED();
00354 
00355         for(int i=0; i<m_DOF; i++)
00356         {       
00357                 if (m_motors[i].statusDec())
00358                         return true;
00359         }
00360         return false;
00361 }
00362 
00364 bool simulatedArm::statusAcc()
00365 {
00366     PSIM_CHECK_INITIALIZED();
00367 
00368         for(int i=0; i<m_DOF; i++)
00369         {       
00370                 if (m_motors[i].statusAcc())
00371                         return true;
00372         }
00373         return false;
00374 }
00375 


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