PowerCubeSim.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
19 #include <string>
20 #include <sstream>
21 #ifdef PYTHON_THREAD
22 #include <Python.h>
23 #endif
24 //#define __LINUX__
25 
26 #define DEG 57.295779524
27 #define MANUAL_AXES0_OFFSET 1.8
28 #define MANUAL_AXES6_OFFSET 1.5
29 
30 #define SIM_CLOCK_FREQUENCY 10.0 // ms
31 using namespace std;
32 
33 #ifndef PCTRL_CHECK_INITIALIZED()
34 #define PCTRL_CHECK_INITIALIZED() \
35  if (isInitialized() == false) \
36  { \
37  m_ErrorMessage.assign("Manipulator not initialized."); \
38  return false; \
39  }
40 #endif
41 
43 {
44  m_Dev = 0;
45  m_NumOfModules = 0;
46  m_SimThreadArgs = NULL;
47  m_SimThreadID = NULL;
48 
49  // pthreads variables
50 }
51 
59 {
60  // std::cout << "---------- PowerCubes-Simulation Init -------------" << std::endl;
61  m_DOF = params->GetNumberOfDOF();
62 
63  m_Dev = 0;
64  m_NumOfModules = m_DOF;
65 
66  m_maxVel = params->GetMaxVel();
67  m_maxAcc = params->GetMaxAcc();
68 
69  // Make sure m_IdModules is clear of Elements:
70  m_IdModules.clear();
71 
72  m_CurrentAngles.resize(m_DOF);
73  m_CurrentAngularMaxAccel.resize(m_DOF);
74  m_CurrentAngularMaxVel.resize(m_DOF);
75  m_CurrentAngularVel.resize(m_DOF);
76 
77  // m_AngleOffsets = m_Obj_Manipulator->GetThetaOffsets();
78 
79  for (int i = 0; i < m_DOF; i++)
80  {
81  std::ostringstream os;
82  os << "ID_Module_Number" << i + 1;
83 
84  // Get the Module Id from the config file
85 
86  // set initial angles to zero
87  m_CurrentAngles[i] = 0.0;
88  m_CurrentAngularVel[i] = 0.0;
89  m_CurrentAngularMaxVel[i] = m_maxVel[i];
90  m_CurrentAngularMaxAccel[i] = m_maxAcc[i];
91 
92  // printf("Offset Angle %d: %f\n",i,m_AngleOffsets[i]);
93  }
94 
95  // Jetzt Winkelgrenzen setzen:
96  // m_AngleLimits = m_Obj_Manipulator->GetLimitsTheta();
97 
98  // pthreads initialisation
99 
100  pthread_mutex_init(&m_Movement_Mutex, NULL);
101  pthread_mutex_init(&m_Angles_Mutex, NULL);
102  pthread_mutex_init(&m_AngularVel_Mutex, NULL);
103 
104  m_SimThreadID = (pthread_t*)malloc(m_DOF * sizeof(pthread_t));
105  m_MovementInProgress.resize(m_DOF);
106  m_SimThreadArgs = (SimThreadArgs**)malloc(m_DOF * sizeof(SimThreadArgs*));
107  for (int i = 0; i < m_DOF; i++)
108  {
109  m_MovementInProgress[i] = false;
110  m_SimThreadArgs[i] = new SimThreadArgs();
111  m_SimThreadArgs[i]->cubeSimPtr = this;
112  m_SimThreadArgs[i]->cubeID = i;
113  }
114  setMaxVelocity(MAX_VEL);
115  setMaxAcceleration(MAX_ACC);
116 
117  // std::cout << "---------- PowerCubes Init fertig ----------" << std::endl;
118  m_Initialized = true;
119  return true;
120 }
121 
125 {
126  free(m_SimThreadID);
127  for (int i = 0; i < m_DOF; i++)
128  {
129  delete m_SimThreadArgs[i];
130  }
131  free(m_SimThreadArgs);
132 }
133 
135 bool PowerCubeSim::getConfig(std::vector<double>& result)
136 {
138  // lock mutex
139  //
140  // std::cout << "getConfig: Waiting for Current_Angles_Mutex ... ";
141  pthread_mutex_lock(&m_Angles_Mutex);
142  // m_Out << "locked"<<endl;
143  result.resize(m_DOF);
144  for (int i; i < m_DOF; i++)
145  result[i] = m_CurrentAngles[i] * DEG;
146  // unlock mutex
147  //
148  // m_Out <<"getConfig: Unlocking Angles_Mutex ... ";
149  pthread_mutex_unlock(&m_Angles_Mutex);
150  // m_Out <<"unlocked "<<endl;
151 
152  return true;
153 }
154 
156 bool PowerCubeSim::getJointVelocities(std::vector<double>& result)
157 {
159  // lock mutex
160  //
161  // std::cout << "getConfig: Waiting for Current_Angles_Mutex ... ";
162  pthread_mutex_lock(&m_AngularVel_Mutex);
163  // m_Out << "locked"<<endl;
164  result.resize(m_DOF);
165  result = m_CurrentAngularVel;
166  // unlock mutex
167  //
168  // m_Out <<"getConfig: Unlocking Angles_Mutex ... ";
169  pthread_mutex_unlock(&m_AngularVel_Mutex);
170  // m_Out <<"unlocked "<<endl;
171 
172  return true;
173 }
174 void PowerCubeSim::setCurrentAngles(std::vector<double> Angles)
175 {
176  // std::cout << "setCurrentAngles: " << Angles[0] << " " << Angles[1] << " " << Angles[2] << " " << Angles[3] << " "
177  // << Angles[4] << " " << Angles[5] << " \n";
178  pthread_mutex_lock(&m_Angles_Mutex);
179  // m_Out << "locked"<<endl;
180 
181  m_CurrentAngles = Angles;
182  // unlock mutex
183  //
184  // m_Out <<"setCurrentAngles: Unlocking Angles_Mutex ... ";
185  pthread_mutex_unlock(&m_Angles_Mutex);
186  // m_Out <<"unlocked "<<endl;
187 }
188 
189 void PowerCubeSim::setCurrentJointVelocities(std::vector<double> AngularVel)
190 {
191  // lock mutex
192  // m_Out << "setCurrentJointVelocities: Waiting for AngularVel_Mutex ... ";
193  pthread_mutex_lock(&m_AngularVel_Mutex);
194  // m_Out << "locked"<<endl;
195 
196  m_CurrentAngularVel = AngularVel;
197  // unlock mutex
198  //
199  // m_Out <<"setCurrentJointVelocities: Unlocking AngularVel_Mutex ... ";
200  pthread_mutex_unlock(&m_AngularVel_Mutex);
201  // m_Out <<"unlocked "<<endl;
202 }
203 
205 bool PowerCubeSim::MoveJointSpaceSync(const std::vector<double>& target)
206 {
208  std::cout << "Starting MoveJointSpaceSync(Jointd Angle) ... " << endl;
209  std::vector<double> targetRAD;
210  targetRAD.resize(m_DOF);
211  for (int i = 0; i < m_DOF; i++)
212  targetRAD[i] = target[i] / DEG;
213 
214  // Evtl. Fragen zur Rechnung / zum Verfahren an: Felix.Geibel@gmx.de
215  std::vector<double> acc(m_DOF);
216  std::vector<double> vel(m_DOF);
217 
218  double TG = 0;
219 
220  try
221  {
222  // Ermittle Joint, der bei max Geschw. und Beschl. am längsten braucht:
223  int DOF = m_DOF;
224 
225  std::vector<double> posnow;
226  if (getConfig(posnow) == false)
227  return false;
228 
229  std::vector<double> velnow;
230  if (getJointVelocities(velnow) == false)
231  return false;
232 
233  std::vector<double> times(DOF);
234 
235  for (int i = 0; i < DOF; i++)
236  {
237  RampCommand rm(posnow[i], velnow[i], targetRAD[i], m_maxAcc[i], m_maxVel[i]);
238  times[i] = rm.getTotalTime();
239  }
240 
241  // determine the joint index that has the greates value for time
242  int furthest = 0;
243 
244  double max = times[0];
245 
246  for (int i = 1; i < m_DOF; i++)
247  {
248  if (times[i] > max)
249  {
250  max = times[i];
251  furthest = i;
252  }
253  }
254 
255  RampCommand rm_furthest(posnow[furthest], velnow[furthest], targetRAD[furthest], m_maxAcc[furthest],
256  m_maxVel[furthest]);
257 
258  double T1 = rm_furthest.T1();
259  double T2 = rm_furthest.T2();
260  double T3 = rm_furthest.T3();
261 
262  // Gesamtzeit:
263  TG = T1 + T2 + T3;
264 
265  // Jetzt Geschwindigkeiten und Beschl. für alle:
266  acc[furthest] = m_maxAcc[furthest];
267  vel[furthest] = m_maxVel[furthest];
268 
269  for (int i = 0; i < DOF; i++)
270  {
271  if (i != furthest)
272  {
273  double a;
274  double v;
275  // a und v berechnen:
276  RampCommand::calculateAV(posnow[i], velnow[i], targetRAD[i], TG, T3, m_maxAcc[i], m_maxVel[i], a, v);
277 
278  acc[i] = a;
279  vel[i] = v;
280  }
281  }
282  }
283  catch (...)
284  {
285  return false;
286  }
287 
288  startSimulatedMovement(targetRAD);
289 
290  // Errechnete Gesamtzeit zurückgeben (könnte ja nützlich sein)
291  return true;
292 }
293 
296 double PowerCubeSim::timeRampMove(double dtheta, double vnow, double v, double a)
297 {
298  // die Zeiten T1, T2 und T3 errechnen
299  // ACHTUNG: Hier wird vorläufig angenommen, dass die Bewegung groß ist, d.h. es eine Phase der Bewegung
300  // mit konst. Geschw. gibt. Der andere Fall wird erst später hinzugefügt.
301  // m_Out << "DEBUG: timeRampMove" << endl;
302  // m_Out << "-------------------" << endl;
303  // m_Out << "dtheta: " << dtheta << ", vnow: " << vnow << ", v: " << v << ", a: " << a << endl;
304  // m_Out << endl;
305 
306  // Wird Joint mit +vmax oder -vmax drehen?
307  double vm = (dtheta < 0) ? -v : v;
308  double am = (dtheta < 0) ? -a : a;
309  // m_Out << "vm: " << vm << endl;
310  // m_Out << "am: " << am << endl;
311 
312  // Zeit bis vm erreicht:
313  double T1 = (vm - vnow) / am;
314  // Winkel, der hierbei zurückgelegt wird:
315  double dtheta1 = vnow * T1 + 0.5 * am * T1 * T1;
316  // m_Out << "T1: " << T1 << endl;
317  // m_Out << "dtheta1: " << dtheta1 << endl;
318 
319  // Zeit zum Bremsen:
320  double T3 = vm / am;
321  // Winkel hierbei:
322  double dtheta3 = 0.5 * vm * T3;
323 
324  // Verbleibender Winkel:
325  double dtheta2 = dtheta - dtheta1 - dtheta3;
326  // Also Restzeit (Bew. mit vm):
327  double T2 = dtheta2 / vm;
328  // m_Out << "T2: " << T2 << endl;
329  // m_Out << "dtheta2: " << dtheta2 << endl;
330  // m_Out << "T3: " << T3 << endl;
331  // m_Out << "dtheta3: " << dtheta3 << endl;
332 
333  // Gesamtzeit zurückgeben:
334  return T1 + T2 + T3;
335 }
336 
338 bool PowerCubeSim::MoveVel(const std::vector<double>& vel)
339 {
341  /* TODO
342  if (getStatus() != PC_CTRL_OK)
343  {
344  printf("PowerCubeSim::MoveVel: canceled!\n");
345  return;
346  }
347  for(int i=0;i<m_NumOfModules;i++)
348  {
349  PCube_moveVel(m_Dev,m_IdModules[i],AngularVelocity[i]);
350  }
351  PCube_startMotionAll(m_Dev);
352  */
353 }
354 
357 {
358  for (int i = 0; i < m_DOF; i++)
359  setStatusMoving(i, false);
360  return true;
361 }
362 
365 bool PowerCubeSim::setMaxVelocity(double radpersec)
366 {
367  for (int i = 0; i < m_DOF; i++)
368  {
369  m_CurrentAngularMaxVel[i] = radpersec;
370  }
371  return true;
372 }
373 
374 bool PowerCubeSim::setMaxVelocity(const std::vector<double>& radpersec)
375 {
376  for (int i = 0; i < m_DOF; i++)
377  {
378  m_CurrentAngularMaxVel[i] = radpersec[i];
379  }
380  return true;
381 }
382 
385 bool PowerCubeSim::setMaxAcceleration(double radPerSecSquared)
386 {
388 
389  for (int i = 0; i < m_DOF; i++)
390  {
391  m_CurrentAngularMaxAccel[i] = radPerSecSquared;
392  }
393 
394  return true;
395 }
396 bool PowerCubeSim::setMaxAcceleration(const std::vector<double>& radPerSecSquared)
397 {
399 
400  for (int i = 0; i < m_DOF; i++)
401  {
402  m_CurrentAngularMaxAccel[i] = radPerSecSquared[i];
403  }
404 
405  return true;
406 }
407 
410 {
412  bool isMoving = false;
413 
414  // m_Out << "statusMoving: Waiting for Movement_Mutex ... ";
415  pthread_mutex_lock(&m_Movement_Mutex);
416  // m_Out << "locked"<<endl;
417  for (int i = 0; i < m_DOF; i++)
418  {
419  if (m_MovementInProgress[i])
420  {
421  isMoving = true;
422  break;
423  }
424  }
425 
426  // unlock mutex
427  // m_Out <<"statusMoving: Unlocking Movement_Mutex ... ";
428  pthread_mutex_unlock(&m_Movement_Mutex);
429  // m_Out <<"unlocked "<<endl;
430 
431  return isMoving;
432 }
433 
435 {
437  bool isMoving = false;
438 
439  // m_Out << "statusMoving: Waiting for Movement_Mutex ... ";
440  pthread_mutex_lock(&m_Movement_Mutex);
441  // m_Out << "locked"<<endl;
442  if (m_MovementInProgress[cubeNo])
443  {
444  isMoving = true;
445  }
446 
447  // unlock mutex
448  // m_Out <<"statusMoving: Unlocking Movement_Mutex ... ";
449  pthread_mutex_unlock(&m_Movement_Mutex);
450  // m_Out <<"unlocked "<<endl;
451 
452  return isMoving;
453 }
454 
455 void PowerCubeSim::setStatusMoving(int cubeNo, bool moving)
456 {
457  // m_Out << "setStatusMoving: Waiting for Movement_Mutex ... ";
458  pthread_mutex_lock(&m_Movement_Mutex);
459  // m_Out << "locked"<<endl;
460 
461  m_MovementInProgress[cubeNo] = moving;
462 
463  // m_Out <<"setStatusMoving: Unlocking Movement_Mutex ... ";
464  pthread_mutex_unlock(&m_Movement_Mutex);
465  // m_Out <<"unlocked "<<endl;
466 }
467 
470 {
472  /* TODO
473  for (int i=0; i<m_NumOfModules; i++)
474  {
475  unsigned long status;
476  PCube_getModuleState(m_Dev,m_IdModules[i], &status);
477  if (status & STATEID_MOD_RAMP_DEC)
478  return true;
479  }
480  */
481  return false;
482 }
483 
486 {
488  /* TODO
489  for (int i=0; i<m_NumOfModules; i++)
490  {
491  unsigned long status;
492  PCube_getModuleState(m_Dev,m_IdModules[i], &status);
493  if (status & STATEID_MOD_RAMP_ACC)
494  return true;
495  }
496 
497  */
498  return false;
499 }
500 
501 void* SimThreadRoutine(void* threadArgs)
502 {
503  // get argument
504  SimThreadArgs* args = (SimThreadArgs*)threadArgs;
505  PowerCubeSim* cubeSimPtr = args->cubeSimPtr;
506  int cubeNo = args->cubeID;
507  double targetAngle = args->targetAngle;
508 
509  // std::cout << "Thread started for cube no "<< cubeNo<<"["<<(cubeSimPtr->getModuleMap())[cubeNo]<<"]"<<endl;
510 
511  // calculate phases of movement
512  float t1, t2, t, tges; // acceleration time t1/t , duration of maximum vel t2, total time tges
513  double deltaT = SIM_CLOCK_FREQUENCY / 1000; // clock period
514  t1 = t2 = t = tges = 0;
515  float maxVel = (cubeSimPtr->getCurrentAngularMaxVel())[cubeNo];
516  float maxAccel = (cubeSimPtr->getCurrentAngularMaxAccel())[cubeNo];
517  std::vector<double> currAngles;
518  cubeSimPtr->getConfig(currAngles);
519  std::vector<double> currVels;
520  cubeSimPtr->getJointVelocities(currVels);
521 
522  double deltaAngle = targetAngle - currAngles[cubeNo];
523 
524  // acceleration phase
525  t1 = maxVel / maxAccel;
526 
527  // constant velocity phase
528  t2 = abs(deltaAngle) / maxVel - t1;
529 
530  // cubeSimPtr->getOutputStream()<<" (abs(deltaAngle) >, maxVel*maxVel/maxAccel)?"<< abs(deltaAngle) <<"; "<<
531  // maxVel*maxVel/maxAccel<<endl;
532 
533  // is maxVel reached?
534  if (abs(deltaAngle) > maxVel * maxVel / maxAccel)
535  {
536  tges = 2 * t1 + t2;
537  }
538  else
539  {
540  // how long will we accelerate?
541  t = sqrt(abs(deltaAngle) / (maxAccel));
542  // what velocity will be reached?
543  // maxVel = maxAccel*t;
544  tges = 2 * t;
545  }
546 
547  // determine direction of movement
548  if (deltaAngle < 0)
549  {
550  maxAccel = -maxAccel;
551  maxVel = -maxVel;
552  }
553  std::cout << "maxVel: " << maxVel << "; maxAccel: " << maxAccel << "; t1 [ms]: " << t1 << "; t2 [ms]: " << t2
554  << "; deltaAngle[rad]" << deltaAngle << endl;
555 
556  // control loop
557 
558  double simulatedTime = 0.0;
559  int n = 0;
560  float currDeltaAngle = deltaAngle;
561 
562  if (abs(deltaAngle) > 0.001)
563  {
564  while ((simulatedTime <= tges) && cubeSimPtr->getStatusMoving(cubeNo) /*||(abs(currDeltaAngle) > 0.001)*/)
565  // while (abs(deltaAngle)>0.005)
566  {
567  // advavnce in simulated time
568  cubeSimPtr->millisleep((int)SIM_CLOCK_FREQUENCY);
569  simulatedTime += SIM_CLOCK_FREQUENCY / 1000; //=n*deltaT
570  n++;
571  cubeSimPtr->getJointVelocities(currVels);
572  // calculate delta phi
573  double deltaPhi = 0.0;
574 
575  // is max Vel reached?
576  if (abs(deltaAngle) > maxVel * maxVel / abs(maxAccel))
577  {
578  if (simulatedTime < t1)
579  {
580  deltaPhi = 0.5 * maxAccel * (n * deltaT * n * deltaT - (n - 1) * deltaT * (n - 1) * deltaT);
581  currVels[cubeNo] = maxAccel * n * deltaT;
582  std::cout << "Phase 1, maxVel ->";
583  }
584  else if ((t1 < simulatedTime) && (simulatedTime < t1 + t2))
585  {
586  deltaPhi = maxVel * deltaT;
587  currVels[cubeNo] = maxVel;
588  std::cout << "Phase 2, maxVel ->";
589  }
590  else if ((simulatedTime > t1 + t2) && (simulatedTime < 2 * t1 + t2))
591  {
592  // deltaPhi = maxVel*simulatedTime - 0.5*maxAccel*(deltaT - (t1+t2))*(simulatedTime - (t1+t2));
593  deltaPhi = maxVel * deltaT -
594  0.5 * maxAccel * ((n * deltaT - (t1 + t2)) * (n * deltaT - (t1 + t2)) -
595  ((n - 1) * deltaT - (t1 + t2)) * ((n - 1) * deltaT - (t1 + t2)));
596  currVels[cubeNo] = maxVel - maxAccel * (simulatedTime - (t1 + t2));
597  std::cout << "Phase 3, maxVel ->";
598  }
599  else
600  {
601  deltaPhi = 0.0;
602  currVels[cubeNo] = 0.0;
603  std::cout << "Phase 4, maxVel ->";
604  }
605  }
606  // no
607  else
608  {
609  if (simulatedTime < t)
610  {
611  // deltaPhi = 0.5*maxAccel*simulatedTime*simulatedTime;
612  deltaPhi = 0.5 * maxAccel * (n * deltaT * n * deltaT - (n - 1) * deltaT * (n - 1) * deltaT);
613  currVels[cubeNo] = maxAccel * simulatedTime;
614  std::cout << "Phase 1 ->";
615  }
616  else if ((simulatedTime > t) && (simulatedTime <= 2 * t))
617  {
618  // deltaPhi = maxVel *simulatedTime - 0.5*maxAccel*(simulatedTime -t)*(simulatedTime-t);
619  deltaPhi = maxAccel * t * deltaT - 0.5 * maxAccel * ((n * deltaT - t) * (n * deltaT - t) - ((n - 1) * deltaT - t) * ((n - 1) * deltaT - t));
620  currVels[cubeNo] = maxAccel * t - maxAccel * (simulatedTime - t);
621  std::cout << "Phase 2 ->";
622  }
623  else
624  {
625  deltaPhi = 0.0;
626  currVels[cubeNo] = 0.0;
627  std::cout << "Phase 3 ->" << t << "\n";
628  }
629  }
630  cubeSimPtr->getConfig(currAngles);
631 
632  currAngles[cubeNo] = currAngles[cubeNo] + deltaPhi;
633  currDeltaAngle = targetAngle - currAngles[cubeNo];
634  // std::cout << "cube "<<cubeNo<<"["<<simulatedTime<<"]: deltaPhi: "<<deltaPhi<<";
635  // deltaAngle:"<<currDeltaAngle<<endl;
636 
637  // write new angle and velocity
638  cubeSimPtr->setCurrentAngles(currAngles);
639  cubeSimPtr->setCurrentJointVelocities(currVels);
640  }
641  }
642 
643  // we have finished our move
644  cubeSimPtr->setStatusMoving(cubeNo, false);
645  currVels[cubeNo] = 0.0;
646  cubeSimPtr->setCurrentJointVelocities(currVels);
647 
648  // cubeSimPtr->getOutputStream() << "Thread finished for cube ID "<< (cubeSimPtr->getModuleMap())[cubeNo]<<endl;
649  pthread_exit(NULL);
650 }
651 
652 int PowerCubeSim::startSimulatedMovement(std::vector<double> target)
653 {
654  if (statusMoving())
655  {
656  std::cout << "startSimulatedMovement: Movement already in progress, preemption not implemented yet! Aborting .."
657  << endl;
658  }
659  else
660  {
661  // create thread
662 
663  for (int i = 0; i < m_DOF; i++)
664  {
665  setStatusMoving(i, true);
666  m_SimThreadArgs[i]->targetAngle = target[i];
667  pthread_create(&m_SimThreadID[i], NULL, SimThreadRoutine, (void*)m_SimThreadArgs[i]);
668  }
669  }
670  return 0;
671 }
672 void PowerCubeSim::millisleep(unsigned int milliseconds) const
673 {
674  timespec warten;
675  // Millisekunden in Sekunden und Nanosekunden aufteilen
676  warten.tv_sec = milliseconds / 1000;
677  warten.tv_nsec = (milliseconds % 1000) * 1000000;
678  timespec gewartet;
679  nanosleep(&warten, &gewartet);
680 }
681 
684 /* Not necessary
685  void PowerCubeSim::HomingDone()
686  {
687  for(int i=0; i<m_NumOfModules; i++)
688  {
689  unsigned long int help;
690  std::cout << "Warte auf Modul " << m_IdModules[i] << endl;
691  do
692  {
693  PCube_getModuleState(m_Dev,m_IdModules[i],&help);
694 //printf("Status: 0x%lx\n",help);
695 millisleep(100);
696 } while ( (help & STATEID_MOD_HOME) == 0 );
697 }
698 
699 }
700 
701 */
702 
703 /*
704  vector<int> PowerCubeSim::getModuleMap(int dev)
705  {
706  vector<int> mod_map;
707  for( int i = 1; i < MAX_MODULES; i++ )
708  {
709  unsigned long serNo;
710  int ret = PCube_getModuleSerialNo( dev, i, &serNo );
711  if( ret == 0 )
712  {
713  std::cout << "Found module " << i << " with SerialNo " << serNo << "\n";
714  mod_map.push_back(i);
715  }
716 // else
717 // printf( "Module %d not found(%d)\n", i, ret);
718 
719 }
720 return mod_map;
721 }
722 */
723 
724 /* TODO: necessary?
725  void PowerCubeSim::waitForSync()
726  {
727  for (int i=0; i < m_DOF; i++)
728  {
729  unsigned long confword;
730  millisleep(4);
731  PCube_getConfig(m_Dev, m_IdModules[i], &confword );
732  millisleep(4);
733  PCube_setConfig(m_Dev, m_IdModules[i], confword | CONFIGID_MOD_SYNC_MOTION);
734  }
735  }
736 
737 */
738 
739 /* TODO: necessary?
740  void PowerCubeSim::dontWaitForSync()
741  {
742  for (int i=0; i < m_DOF; i++)
743  {
744  unsigned long confword;
745  millisleep(4);
746  PCube_getConfig(m_Dev, m_IdModules[i], &confword );
747  millisleep(4);
748  PCube_setConfig(m_Dev, m_IdModules[i], confword & (~CONFIGID_MOD_SYNC_MOTION));
749  }
750  }
751  */
752 
753 /*
754  int PowerCubeSim::getStatus()
755  {
756 
757  PC_CTRL_STATE error = PC_CTRL_OK;
758 
759  for(int i=0; i<m_NumOfModules; i++)
760  {
761  unsigned long int state;
762  PCube_getModuleState(m_Dev,m_IdModules[i],&state);
763 
764  if (state & STATEID_MOD_POW_VOLT_ERR)
765  {
766  printf("Error in Module %d: Motor voltage below minimum value!\n",m_IdModules[i]);
767  error = PC_CTRL_POW_VOLT_ERR;
768  }
769  else if (!(state & STATEID_MOD_HOME))
770  {
771  printf("Warning: Module %d is not referenced!\n",m_IdModules[i]);
772  error = PC_CTRL_NOT_REFERENCED;
773  }
774  else if (state & STATEID_MOD_ERROR)
775  {
776  printf("Error in Module %d: 0x%lx\n",m_IdModules[i],state);
777  error = PC_CTRL_ERR;
778  }
779  }
780  return error;
781 
782 }*/
bool getConfig(std::vector< double > &result)
Returns the current Joint Angles.
bool statusDec()
Returns true if any of the Joints are decelerating.
void setCurrentAngles(std::vector< double > Angles)
double targetAngle
Definition: PowerCubeSim.h:59
void setCurrentJointVelocities(std::vector< double > Angles)
bool setMaxVelocity(double radpersec)
Sets the maximum angular velocity (rad/s) for the Joints, use with care! A Value of 0...
bool MoveJointSpaceSync(const std::vector< double > &Angle)
same as MoveJointSpace, but final angles should by reached simultaniously! Returns the time that the ...
void millisleep(unsigned int milliseconds) const
double timeRampMove(double dtheta, double vnow, double v, double a)
Tells the Modules not to start moving until PCubel_startMotionAll is called.
#define PCTRL_CHECK_INITIALIZED()
virtual double T3()
Definition: moveCommand.h:163
bool statusMoving()
Returns true if any of the Joints are still moving Should also return true if Joints are accelerating...
bool setMaxAcceleration(double radPerSecSquared)
Sets the maximum angular acceleration (rad/s^2) for the Joints, use with care! A Value of 0...
std::vector< double > GetMaxAcc()
Gets the max. angular accelerations (rad/s^2) for the joints.
virtual double getTotalTime()
returns the planned total time for the movement (in seconds)
std::vector< double > getCurrentAngularMaxAccel()
Definition: PowerCubeSim.h:166
static void calculateAV(double x0, double v0, double xtarget, double time, double T3, double amax, double vmax, double &a, double &v)
Calculate the necessary a and v of a rampmove, so that the move will take the desired time...
int startSimulatedMovement(std::vector< double > target)
std::vector< double > getCurrentAngularMaxVel()
Definition: PowerCubeSim.h:162
bool Init(PowerCubeCtrlParams *params)
void * SimThreadRoutine(void *threadArgs)
#define MAX_VEL
Definition: PowerCubeSim.h:35
void setStatusMoving(int cubeNo, bool moving)
bool Stop()
Stops the Manipulator immediately.
PowerCubeSim * cubeSimPtr
Definition: PowerCubeSim.h:57
virtual double T1()
Return the times of the different phases of the ramp move.
Definition: moveCommand.h:155
virtual double T2()
Definition: moveCommand.h:159
bool getJointVelocities(std::vector< double > &result)
Returns the current Angular velocities (Rad/s)
#define DEG
std::vector< double > GetMaxVel()
Gets the max. angular velocities (rad/s) for the joints.
bool MoveVel(const std::vector< double > &vel)
Moves all cubes by the given velocities.
long abs(long iValue)
bool statusAcc()
Returs true if any of the Joints are accelerating.
Parameters for cob_powercube_chain.
#define SIM_CLOCK_FREQUENCY
#define MAX_ACC
Definition: PowerCubeSim.h:36
bool getStatusMoving(int cubeNo) const
Definition: PowerCubeSim.h:153


schunk_powercube_chain
Author(s): Florian Weisshardt
autogenerated on Mon Nov 25 2019 03:48:21