PowerCubeSim.cpp
Go to the documentation of this file.
00001 
00060 #include <schunk_powercube_chain/PowerCubeSim.h>
00061 #include <string>
00062 #include <sstream>
00063 #ifdef PYTHON_THREAD
00064 #include <Python.h>
00065 #endif
00066 //#define __LINUX__
00067 
00068 #define DEG 57.295779524
00069 #define MANUAL_AXES0_OFFSET  1.8
00070 #define MANUAL_AXES6_OFFSET 1.5
00071 
00072 
00073 #define SIM_CLOCK_FREQUENCY 10.0 //ms
00074 using namespace std;
00075 
00076 #ifndef PCTRL_CHECK_INITIALIZED()
00077 #define PCTRL_CHECK_INITIALIZED() \
00078 if ( isInitialized()==false )                                                                                                   \
00079 {                                                                                                                                               \
00080     m_ErrorMessage.assign("Manipulator not initialized.");              \
00081         return false;                                                                                                           \
00082 }
00083 #endif
00084 
00085 
00086 PowerCubeSim::PowerCubeSim()
00087 {
00088         m_Dev=0;
00089         m_NumOfModules = 0;
00090         m_SimThreadArgs = NULL;
00091         m_SimThreadID = NULL;
00092 
00093         //pthreads variables
00094         
00095         
00096 }
00097 
00098 
00103 bool PowerCubeSim::Init(PowerCubeCtrlParams * params)
00104 {
00105         //std::cout << "---------- PowerCubes-Simulation Init -------------" << std::endl;
00106         m_DOF=params->GetNumberOfDOF();
00107 
00108         m_Dev=0;
00109         m_NumOfModules = m_DOF;
00110         
00111         m_maxVel = params->GetMaxVel();
00112         m_maxAcc = params->GetMaxAcc();
00113 
00114         // Make sure m_IdModules is clear of Elements:
00115         m_IdModules.clear();
00116                         
00117 
00118         m_CurrentAngles.resize(m_DOF);
00119         m_CurrentAngularMaxAccel.resize(m_DOF);
00120         m_CurrentAngularMaxVel.resize(m_DOF);
00121         m_CurrentAngularVel.resize(m_DOF);
00122 
00123         //m_AngleOffsets = m_Obj_Manipulator->GetThetaOffsets();
00124 
00125         for(int i=0; i<m_DOF; i++)
00126         {
00127                 std::ostringstream os;
00128                 os << "ID_Module_Number" << i+1;
00129                 
00130                 // Get the Module Id from the config file
00131                 
00132                 //set initial angles to zero
00133                 m_CurrentAngles[i] = 0.0;
00134                 m_CurrentAngularVel[i] =0.0;
00135                 m_CurrentAngularMaxVel[i] = m_maxVel[i];
00136                 m_CurrentAngularMaxAccel[i] =m_maxAcc[i];
00137                 
00138                 //printf("Offset Angle %d: %f\n",i,m_AngleOffsets[i]);
00139         }
00140         
00141         
00142         // Jetzt Winkelgrenzen setzen:
00143         //m_AngleLimits = m_Obj_Manipulator->GetLimitsTheta();
00144 
00145         // pthreads initialisation
00146         
00147         pthread_mutex_init(&m_Movement_Mutex,NULL);
00148         pthread_mutex_init(&m_Angles_Mutex,NULL);
00149         pthread_mutex_init(&m_AngularVel_Mutex,NULL);
00150 
00151         m_SimThreadID = (pthread_t*)malloc(m_DOF * sizeof(pthread_t));
00152         m_MovementInProgress.resize(m_DOF);
00153         m_SimThreadArgs = (SimThreadArgs**)malloc(m_DOF * sizeof(SimThreadArgs*));
00154         for (int i = 0; i < m_DOF; i++)
00155         {
00156                 m_MovementInProgress[i] = false;
00157                 m_SimThreadArgs[i] = new SimThreadArgs();
00158                 m_SimThreadArgs[i]->cubeSimPtr = this;
00159                 m_SimThreadArgs[i]->cubeID = i;
00160         }
00161         setMaxVelocity(MAX_VEL);
00162         setMaxAcceleration(MAX_ACC);
00163 
00164 
00165         //std::cout << "---------- PowerCubes Init fertig ----------" << std::endl;
00166         m_Initialized = true;
00167         return true;
00168 }
00169 
00170 
00173 PowerCubeSim::~PowerCubeSim()
00174 { 
00175         free(m_SimThreadID);
00176         for (int i = 0; i < m_DOF; i++)
00177         {
00178                 delete m_SimThreadArgs[i];
00179         }
00180         free(m_SimThreadArgs);
00181 
00182 }
00183 
00185 bool PowerCubeSim::getConfig(std::vector<double>& result)
00186 {
00187         PCTRL_CHECK_INITIALIZED();
00188         //lock mutex
00189         //
00190         //std::cout << "getConfig: Waiting for Current_Angles_Mutex ... ";
00191         pthread_mutex_lock(&m_Angles_Mutex);
00192         //m_Out << "locked"<<endl;
00193         result.resize(m_DOF);
00194         for(int i; i < m_DOF; i++)
00195                 result[i] = m_CurrentAngles[i]*DEG;
00196         //unlock mutex
00197         //
00198         //m_Out <<"getConfig: Unlocking Angles_Mutex ... ";
00199         pthread_mutex_unlock(&m_Angles_Mutex);
00200         //m_Out <<"unlocked "<<endl;
00201         
00202         return true;
00203 }
00204 
00205 
00206 
00208 bool PowerCubeSim::getJointVelocities(std::vector<double>& result)
00209 {
00210         PCTRL_CHECK_INITIALIZED();
00211         //lock mutex
00212         //
00213         //std::cout << "getConfig: Waiting for Current_Angles_Mutex ... ";
00214         pthread_mutex_lock(&m_AngularVel_Mutex);
00215         //m_Out << "locked"<<endl;
00216         result.resize(m_DOF);
00217         result = m_CurrentAngularVel;
00218         //unlock mutex
00219         //
00220         //m_Out <<"getConfig: Unlocking Angles_Mutex ... ";
00221         pthread_mutex_unlock(&m_AngularVel_Mutex);
00222         //m_Out <<"unlocked "<<endl;
00223 
00224         return true;
00225 }
00226 void PowerCubeSim::setCurrentAngles(std::vector<double> Angles)
00227 {
00228 
00229         //std::cout << "setCurrentAngles: " << Angles[0] << " " << Angles[1] << " " << Angles[2] << " " << Angles[3] << " " << Angles[4] << " " << Angles[5] << " \n";
00230         pthread_mutex_lock(&m_Angles_Mutex);
00231         //m_Out << "locked"<<endl;
00232         
00233         m_CurrentAngles = Angles;
00234         //unlock mutex
00235         //
00236         //m_Out <<"setCurrentAngles: Unlocking Angles_Mutex ... ";
00237         pthread_mutex_unlock(&m_Angles_Mutex);
00238         //m_Out <<"unlocked "<<endl;
00239         
00240 
00241 }
00242 
00243 void PowerCubeSim::setCurrentJointVelocities( std::vector<double> AngularVel)
00244 {
00245 
00246         //lock mutex
00247         //m_Out << "setCurrentJointVelocities: Waiting for AngularVel_Mutex ... ";
00248         pthread_mutex_lock(&m_AngularVel_Mutex);
00249         //m_Out << "locked"<<endl;
00250         
00251         m_CurrentAngularVel = AngularVel;
00252         //unlock mutex
00253         //
00254         //m_Out <<"setCurrentJointVelocities: Unlocking AngularVel_Mutex ... ";
00255         pthread_mutex_unlock(&m_AngularVel_Mutex);
00256         //m_Out <<"unlocked "<<endl;
00257 
00258 }
00259         
00260 
00261 
00263 bool PowerCubeSim::MoveJointSpaceSync(const std::vector<double>& target)
00264 {
00265         PCTRL_CHECK_INITIALIZED();
00266         std::cout << "Starting MoveJointSpaceSync(Jointd Angle) ... "<<endl;
00267         std::vector<double> targetRAD;
00268         targetRAD.resize(m_DOF);
00269         for(int i = 0; i<m_DOF; i++)
00270                 targetRAD[i] = target[i]/DEG;
00271 
00272         // Evtl. Fragen zur Rechnung / zum Verfahren an: Felix.Geibel@gmx.de
00273         std::vector<double> acc(m_DOF);
00274         std::vector<double> vel(m_DOF);
00275         
00276         double TG = 0;
00277         
00278         try
00279         {
00280                 
00281                 // Ermittle Joint, der bei max Geschw. und Beschl. am längsten braucht:
00282                 int DOF = m_DOF;
00283 
00284                 std::vector<double> posnow;
00285                 if ( getConfig(posnow) == false )
00286                     return false;
00287 
00288 
00289                 std::vector<double> velnow;
00290                 if ( getJointVelocities(velnow) == false )
00291                     return false;
00292 
00293                 std::vector<double> times(DOF);
00294 
00295                 for (int i=0; i < DOF; i++)
00296                 {
00297                         RampCommand rm(posnow[i], velnow[i], targetRAD[i], m_maxAcc[i], m_maxVel[i]);
00298                         times[i] = rm.getTotalTime();
00299                 }
00300                 
00301                 // determine the joint index that has the greates value for time
00302                 int furthest = 0;
00303 
00304                 double max = times[0];
00305 
00306             for (int i=1; i<m_DOF; i++)
00307             {
00308                     if (times[i] > max)
00309                     {
00310                             max = times[i];
00311                             furthest = i;
00312                     }
00313             }
00314 
00315                 RampCommand rm_furthest(posnow[furthest], velnow[furthest], targetRAD[furthest], m_maxAcc[furthest], m_maxVel[furthest]);
00316 
00317                 double T1 = rm_furthest.T1();
00318                 double T2 = rm_furthest.T2();
00319                 double T3 = rm_furthest.T3();
00320 
00321                 // Gesamtzeit:
00322                 TG = T1 + T2 + T3;
00323                 
00324                 // Jetzt Geschwindigkeiten und Beschl. für alle:
00325                 acc[furthest] = m_maxAcc[furthest];
00326                 vel[furthest] = m_maxVel[furthest];
00327                 
00328                 for (int i = 0; i < DOF; i++)
00329                 {
00330                         if (i != furthest)
00331                         {
00332                                 double a; double v;
00333                                 // a und v berechnen:
00334                                 RampCommand::calculateAV(
00335                                         posnow[i],
00336                                         velnow[i],
00337                                         targetRAD[i],
00338                                         TG, T3,
00339                                         m_maxAcc[i],
00340                                         m_maxVel[i],
00341                                         a,
00342                                         v);
00343 
00344                                 acc[i] = a;
00345                                 vel[i] = v;
00346                         }
00347                 }
00348         }
00349         catch(...)
00350         {
00351                 return false;
00352         }
00353 
00354         startSimulatedMovement(targetRAD);
00355         
00356         // Errechnete Gesamtzeit zurückgeben (könnte ja nützlich sein)
00357         return true;
00358                 
00359 }
00360 
00361 
00362                 
00363 
00365 double PowerCubeSim::timeRampMove(double dtheta, double vnow, double v, double a)
00366 {
00367         // die Zeiten T1, T2 und T3 errechnen
00368         // ACHTUNG: Hier wird vorläufig angenommen, dass die Bewegung groß ist, d.h. es eine Phase der Bewegung
00369         // mit konst. Geschw. gibt. Der andere Fall wird erst später hinzugefügt.
00370         //m_Out << "DEBUG: timeRampMove" << endl;
00371         //m_Out << "-------------------" << endl;
00372         //m_Out << "dtheta: " << dtheta << ", vnow: " << vnow << ", v: " << v << ", a: " << a << endl;
00373         //m_Out << endl;
00374         
00375         // Wird Joint mit +vmax oder -vmax drehen?
00376         double vm = (dtheta<0)?-v:v;
00377         double am = (dtheta<0)?-a:a;
00378         //m_Out << "vm: " << vm << endl;
00379         //m_Out << "am: " << am << endl;
00380         
00381         // Zeit bis vm erreicht:
00382         double T1 = (vm - vnow) / am;
00383         // Winkel, der hierbei zurückgelegt wird:
00384         double dtheta1 = vnow * T1 + 0.5 * am * T1 * T1;
00385         //m_Out << "T1: " << T1 << endl;
00386         //m_Out << "dtheta1: " << dtheta1 << endl;
00387         
00388         // Zeit zum Bremsen:
00389         double T3 = vm / am;
00390         // Winkel hierbei:
00391         double dtheta3 = 0.5 * vm * T3;
00392         
00393         // Verbleibender Winkel:
00394         double dtheta2 = dtheta - dtheta1 - dtheta3;
00395         // Also Restzeit (Bew. mit vm):
00396         double T2 = dtheta2 / vm;
00397         //m_Out << "T2: " << T2 << endl;
00398         //m_Out << "dtheta2: " << dtheta2 << endl;
00399         //m_Out << "T3: " << T3 << endl;
00400         //m_Out << "dtheta3: " << dtheta3 << endl;
00401 
00402         
00403         // Gesamtzeit zurückgeben:
00404         return T1 + T2 + T3;
00405 }
00406 
00407                 
00409 bool PowerCubeSim::MoveVel(const std::vector<double>& vel)
00410 {
00411         PCTRL_CHECK_INITIALIZED();
00412         /* TODO
00413         if (getStatus() != PC_CTRL_OK)
00414         {
00415                 printf("PowerCubeSim::MoveVel: canceled!\n");
00416                 return;
00417         }
00418         for(int i=0;i<m_NumOfModules;i++)
00419         {
00420                 PCube_moveVel(m_Dev,m_IdModules[i],AngularVelocity[i]);
00421         }
00422         PCube_startMotionAll(m_Dev);
00423         */
00424 }
00425                 
00426                                 
00428 bool PowerCubeSim::Stop()
00429 {
00430         for (int i=0; i < m_DOF; i++)
00431                 setStatusMoving(i, false);
00432         return true;
00433 }
00434 
00437 bool PowerCubeSim::setMaxVelocity(double radpersec)
00438 { 
00439         for (int i=0; i<m_DOF; i++)
00440         {
00441                 m_CurrentAngularMaxVel[i] = radpersec;
00442         }
00443         return true;
00444 }
00445 
00446 bool PowerCubeSim::setMaxVelocity(const std::vector<double>& radpersec)
00447 {
00448         for (int i=0; i<m_DOF; i++)
00449         {
00450                 m_CurrentAngularMaxVel[i] = radpersec[i];
00451         }
00452         return true;
00453 }
00454 
00455 
00458 bool PowerCubeSim::setMaxAcceleration(double radPerSecSquared)
00459 {
00460     PCTRL_CHECK_INITIALIZED();
00461 
00462         for (int i=0; i<m_DOF; i++)
00463         {
00464                 m_CurrentAngularMaxAccel[i] = radPerSecSquared;
00465         }
00466 
00467     return true;
00468 }
00469 bool PowerCubeSim::setMaxAcceleration(const std::vector<double>& radPerSecSquared)
00470 {
00471     PCTRL_CHECK_INITIALIZED();
00472 
00473         for (int i=0; i<m_DOF; i++)
00474         {
00475                 m_CurrentAngularMaxAccel[i] = radPerSecSquared[i];
00476         }
00477 
00478     return true;
00479 }
00480 
00481 
00482 
00484 bool PowerCubeSim::statusMoving()
00485 {
00486         PCTRL_CHECK_INITIALIZED();
00487         bool isMoving = false;
00488 
00489         //m_Out << "statusMoving: Waiting for Movement_Mutex ... ";
00490         pthread_mutex_lock(&m_Movement_Mutex);
00491         //m_Out << "locked"<<endl;
00492         for (int i = 0; i < m_DOF; i++)
00493         {
00494                 if (m_MovementInProgress[i])
00495                 {
00496                         isMoving = true;
00497                         break;
00498                 }
00499         }
00500 
00501         //unlock mutex
00502         //m_Out <<"statusMoving: Unlocking Movement_Mutex ... ";
00503         pthread_mutex_unlock(&m_Movement_Mutex);
00504         //m_Out <<"unlocked "<<endl;
00505 
00506         return isMoving;
00507 }
00508 
00509 bool PowerCubeSim::statusMoving(int cubeNo)
00510 {
00511         PCTRL_CHECK_INITIALIZED();
00512         bool isMoving = false;
00513 
00514         //m_Out << "statusMoving: Waiting for Movement_Mutex ... ";
00515         pthread_mutex_lock(&m_Movement_Mutex);
00516         //m_Out << "locked"<<endl;
00517         if (m_MovementInProgress[cubeNo])
00518         {
00519                 isMoving = true;
00520         }
00521 
00522         //unlock mutex
00523         //m_Out <<"statusMoving: Unlocking Movement_Mutex ... ";
00524         pthread_mutex_unlock(&m_Movement_Mutex);
00525         //m_Out <<"unlocked "<<endl;
00526 
00527         return isMoving;
00528 }
00529                 
00530 void PowerCubeSim::setStatusMoving (int cubeNo, bool moving)
00531 {
00532         //m_Out << "setStatusMoving: Waiting for Movement_Mutex ... ";
00533         pthread_mutex_lock(&m_Movement_Mutex);
00534         //m_Out << "locked"<<endl;
00535 
00536         m_MovementInProgress[cubeNo] = moving;
00537 
00538         //m_Out <<"setStatusMoving: Unlocking Movement_Mutex ... ";
00539         pthread_mutex_unlock(&m_Movement_Mutex);
00540         //m_Out <<"unlocked "<<endl;
00541 
00542 }
00543 
00545 bool PowerCubeSim::statusDec()
00546 {
00547         PCTRL_CHECK_INITIALIZED();
00548         /* TODO
00549         for (int i=0; i<m_NumOfModules; i++)
00550         {
00551                 unsigned long status;
00552                 PCube_getModuleState(m_Dev,m_IdModules[i], &status);
00553                 if (status & STATEID_MOD_RAMP_DEC)
00554                         return true;
00555         }
00556         */
00557         return false;
00558 }
00559 
00560 
00562 bool PowerCubeSim::statusAcc()
00563 {
00564         PCTRL_CHECK_INITIALIZED();
00565         /* TODO
00566         for (int i=0; i<m_NumOfModules; i++)
00567         {
00568                 unsigned long status;
00569                 PCube_getModuleState(m_Dev,m_IdModules[i], &status);
00570                 if (status & STATEID_MOD_RAMP_ACC)
00571                         return true;
00572         }
00573 
00574         */
00575         return false;
00576 }
00577 
00578 void  * SimThreadRoutine(void* threadArgs)
00579 {
00580         //get argument
00581         SimThreadArgs * args =  (SimThreadArgs*)threadArgs;
00582         PowerCubeSim* cubeSimPtr = args->cubeSimPtr;
00583         int cubeNo = args->cubeID;
00584         double targetAngle = args->targetAngle;
00585 
00586         //std::cout << "Thread started for cube no "<< cubeNo<<"["<<(cubeSimPtr->getModuleMap())[cubeNo]<<"]"<<endl;
00587         
00588         //calculate phases of movement
00589         float t1,t2,t,tges; //acceleration time t1/t , duration of maximum vel t2, total time tges
00590         double deltaT = SIM_CLOCK_FREQUENCY/1000; //clock period
00591         t1=t2=t=tges=0;
00592         float maxVel = (cubeSimPtr->getCurrentAngularMaxVel())[cubeNo];
00593         float maxAccel = (cubeSimPtr->getCurrentAngularMaxAccel())[cubeNo];
00594         std::vector<double> currAngles;
00595         cubeSimPtr->getConfig(currAngles);
00596         std::vector<double> currVels;
00597         cubeSimPtr->getJointVelocities(currVels);
00598 
00599         double deltaAngle = targetAngle - currAngles[cubeNo];
00600         
00601         //acceleration phase
00602         t1 = maxVel/maxAccel;
00603 
00604         //constant velocity phase
00605         t2 = abs(deltaAngle)/maxVel - t1;
00606 
00607         //cubeSimPtr->getOutputStream()<<" (abs(deltaAngle) >, maxVel*maxVel/maxAccel)?"<< abs(deltaAngle) <<"; "<< maxVel*maxVel/maxAccel<<endl;
00608         
00609         //is maxVel reached?
00610         if (abs(deltaAngle) > maxVel*maxVel/maxAccel)
00611         {
00612                 tges=2*t1+t2;
00613         }
00614         else
00615         {
00616 
00617                 //how long will we accelerate?
00618                 t = sqrt(abs(deltaAngle)/(maxAccel));
00619                 //what velocity will be reached?
00620                 //maxVel = maxAccel*t;
00621                 tges = 2*t;
00622         }
00623 
00624         //determine direction of movement
00625         if (deltaAngle<0)
00626         {
00627                 maxAccel = -maxAccel;
00628                 maxVel = -maxVel;
00629         }
00630         std::cout << "maxVel: "<<maxVel<<"; maxAccel: "<<maxAccel<< "; t1 [ms]: "<<t1<<"; t2 [ms]: "<<t2 <<"; deltaAngle[rad]" <<deltaAngle<<endl;
00631 
00632         //control loop
00633         
00634         double simulatedTime = 0.0; 
00635         int n = 0;  
00636         float currDeltaAngle = deltaAngle;
00637 
00638         if (abs(deltaAngle) > 0.001)
00639         {
00640                 while ((simulatedTime <= tges) && cubeSimPtr->getStatusMoving(cubeNo) /*||(abs(currDeltaAngle) > 0.001)*/)
00641                 //while (abs(deltaAngle)>0.005)
00642                 {
00643                         //advavnce in simulated time
00644                         cubeSimPtr->millisleep((int)SIM_CLOCK_FREQUENCY);
00645                         simulatedTime += SIM_CLOCK_FREQUENCY/1000; //=n*deltaT
00646                         n++;
00647                         cubeSimPtr->getJointVelocities(currVels);
00648                         //calculate delta phi
00649                         double deltaPhi = 0.0;
00650 
00651                         //is max Vel reached?
00652                         if (abs(deltaAngle) > maxVel*maxVel/abs(maxAccel))
00653                         {
00654                                 if (simulatedTime < t1)
00655                                 {
00656                                         deltaPhi = 0.5*maxAccel*(n*deltaT*n*deltaT - (n-1)*deltaT*(n-1)*deltaT);
00657                                         currVels[cubeNo] = maxAccel * n*deltaT;
00658                                         std::cout << "Phase 1, maxVel ->";
00659 
00660                                 }
00661                                 else if ((t1 < simulatedTime) && (simulatedTime < t1+t2))
00662                                 {
00663                                         deltaPhi = maxVel * deltaT;
00664                                         currVels[cubeNo] = maxVel;
00665                                         std::cout << "Phase 2, maxVel ->";
00666                                 }
00667                                 else if ((simulatedTime > t1+t2) && (simulatedTime < 2*t1+t2))
00668                                 {
00669                                         //deltaPhi = maxVel*simulatedTime - 0.5*maxAccel*(deltaT - (t1+t2))*(simulatedTime - (t1+t2));
00670                                         deltaPhi = maxVel*deltaT - 0.5*maxAccel*((n*deltaT-(t1+t2))*(n*deltaT-(t1+t2))-((n-1)*deltaT-(t1+t2))*((n-1)*deltaT-(t1+t2)));
00671                                         currVels[cubeNo] = maxVel - maxAccel * (simulatedTime -(t1+t2));
00672                                         std::cout << "Phase 3, maxVel ->";
00673                                 }
00674                                 else
00675                                 {
00676                                         deltaPhi = 0.0;
00677                                         currVels[cubeNo] = 0.0;
00678                                         std::cout << "Phase 4, maxVel ->";
00679                                 }
00680                         }
00681                         //no
00682                         else
00683                         {
00684                                 if (simulatedTime < t)
00685                                 {
00686                                         //deltaPhi = 0.5*maxAccel*simulatedTime*simulatedTime;
00687                                         deltaPhi = 0.5*maxAccel*(n*deltaT*n*deltaT - (n-1)*deltaT*(n-1)*deltaT);
00688                                         currVels[cubeNo] = maxAccel * simulatedTime;
00689                                         std::cout  << "Phase 1 ->";
00690                                 }
00691                                 else if ((simulatedTime > t) && (simulatedTime <= 2*t))
00692                                 {
00693                                         //deltaPhi = maxVel *simulatedTime - 0.5*maxAccel*(simulatedTime -t)*(simulatedTime-t); 
00694                                         deltaPhi = maxAccel*t*deltaT - 0.5*maxAccel*((n*deltaT-t)*(n*deltaT-t)-((n-1)*deltaT-t)*((n-1)*deltaT-t));
00695                                         currVels[cubeNo] = maxAccel*t - maxAccel * (simulatedTime -t);
00696                                         std::cout  << "Phase 2 ->";
00697                                 }
00698                                 else
00699                                 {
00700                                         deltaPhi = 0.0;
00701                                         currVels[cubeNo] = 0.0;
00702                                         std::cout  << "Phase 3 ->" << t << "\n";
00703                                 }
00704 
00705                         }
00706                         cubeSimPtr->getConfig(currAngles);
00707 
00708                         currAngles[cubeNo] = currAngles[cubeNo]+deltaPhi;
00709                         currDeltaAngle = targetAngle - currAngles[cubeNo];
00710                         //std::cout << "cube "<<cubeNo<<"["<<simulatedTime<<"]: deltaPhi: "<<deltaPhi<<"; deltaAngle:"<<currDeltaAngle<<endl;
00711 
00712                         //write new angle and velocity
00713                         cubeSimPtr->setCurrentAngles(currAngles);
00714                         cubeSimPtr->setCurrentJointVelocities(currVels);
00715                 }
00716         }
00717 
00718         //we have finished our move
00719         cubeSimPtr->setStatusMoving(cubeNo,false);
00720         currVels[cubeNo] = 0.0;
00721         cubeSimPtr->setCurrentJointVelocities(currVels);
00722 
00723         //cubeSimPtr->getOutputStream() << "Thread finished for cube ID "<< (cubeSimPtr->getModuleMap())[cubeNo]<<endl;
00724         pthread_exit(NULL);
00725 }
00726 
00727 
00728 int PowerCubeSim::startSimulatedMovement(std::vector<double> target)
00729 {
00730 
00731         if (statusMoving())
00732         {
00733                 std::cout << "startSimulatedMovement: Movement already in progress, preemption not implemented yet! Aborting .."<<endl;
00734         }
00735         else
00736         {
00737                 //create thread
00738 
00739                 for (int i = 0; i < m_DOF; i++)
00740                 {
00741                         setStatusMoving(i,true);
00742                         m_SimThreadArgs[i]->targetAngle = target[i];
00743                         pthread_create(&m_SimThreadID[i],NULL,SimThreadRoutine,(void*)m_SimThreadArgs[i]);
00744                 }
00745         }
00746         return 0;
00747 
00748 }
00749 void PowerCubeSim::millisleep(unsigned int milliseconds) const
00750 {
00751         timespec warten;
00752         // Millisekunden in Sekunden und Nanosekunden aufteilen
00753         warten.tv_sec = milliseconds / 1000;
00754         warten.tv_nsec = (milliseconds % 1000) * 1000000;
00755         timespec gewartet;
00756         nanosleep(&warten, &gewartet);
00757 }
00758 
00759 
00762 /* Not necessary
00763    void PowerCubeSim::HomingDone()
00764    {
00765    for(int i=0; i<m_NumOfModules; i++)
00766    {
00767    unsigned long int help;
00768    std::cout << "Warte auf Modul " << m_IdModules[i] << endl;
00769    do
00770    {
00771    PCube_getModuleState(m_Dev,m_IdModules[i],&help);
00772 //printf("Status: 0x%lx\n",help);
00773 millisleep(100);
00774 } while ( (help & STATEID_MOD_HOME) == 0 );
00775 }
00776 
00777 }
00778 
00779 */
00780 
00781 /*
00782    vector<int> PowerCubeSim::getModuleMap(int dev)
00783    {
00784    vector<int> mod_map;
00785    for( int i = 1; i < MAX_MODULES; i++ )
00786    {
00787    unsigned long serNo;
00788    int ret = PCube_getModuleSerialNo( dev, i, &serNo );
00789    if( ret == 0 )
00790    {
00791    std::cout << "Found module " << i << " with SerialNo " << serNo << "\n";
00792    mod_map.push_back(i);
00793    }
00794 //              else
00795 //              printf( "Module %d not found(%d)\n", i, ret);
00796 
00797 }
00798 return mod_map;
00799 }
00800 */
00801 
00802 /* TODO: necessary?
00803    void PowerCubeSim::waitForSync()
00804    {
00805    for (int i=0; i < m_DOF; i++)
00806    {
00807    unsigned long confword;
00808    millisleep(4);
00809    PCube_getConfig(m_Dev, m_IdModules[i], &confword );
00810    millisleep(4);
00811    PCube_setConfig(m_Dev, m_IdModules[i], confword | CONFIGID_MOD_SYNC_MOTION);
00812    }
00813    }
00814 
00815 */
00816 
00817 /* TODO: necessary?
00818    void PowerCubeSim::dontWaitForSync()
00819    {
00820    for (int i=0; i < m_DOF; i++)
00821    {
00822    unsigned long confword;
00823    millisleep(4);
00824    PCube_getConfig(m_Dev, m_IdModules[i], &confword );
00825    millisleep(4);
00826    PCube_setConfig(m_Dev, m_IdModules[i], confword & (~CONFIGID_MOD_SYNC_MOTION));
00827    }
00828    }
00829    */
00830 
00831 /*
00832    int PowerCubeSim::getStatus()
00833    {
00834 
00835    PC_CTRL_STATE error = PC_CTRL_OK;
00836 
00837    for(int i=0; i<m_NumOfModules; i++)
00838    {
00839    unsigned long int state;
00840    PCube_getModuleState(m_Dev,m_IdModules[i],&state);
00841 
00842    if (state & STATEID_MOD_POW_VOLT_ERR)
00843    {
00844    printf("Error in Module %d: Motor voltage below minimum value!\n",m_IdModules[i]);
00845    error = PC_CTRL_POW_VOLT_ERR;
00846    }
00847    else if (!(state & STATEID_MOD_HOME))
00848    {
00849    printf("Warning: Module %d is not referenced!\n",m_IdModules[i]);
00850    error = PC_CTRL_NOT_REFERENCED;
00851    }
00852    else if (state & STATEID_MOD_ERROR)
00853    {
00854    printf("Error in  Module %d: 0x%lx\n",m_IdModules[i],state);
00855    error = PC_CTRL_ERR;
00856    }
00857    }
00858         return error;
00859 
00860 }*/
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


schunk_powercube_chain
Author(s): Florian Weisshardt
autogenerated on Tue Mar 5 2013 14:44:47