TypeIIRMLStep1.cpp
Go to the documentation of this file.
1 // ---------------------- Doxygen info ----------------------
38 // ----------------------------------------------------------
39 // For a convenient reading of this file's source code,
40 // please use a tab width of four characters.
41 // ----------------------------------------------------------
42 
43 
44 #include <TypeIIRMLPosition.h>
45 #include <TypeIIRMLMath.h>
46 #include <TypeIIRMLStep1IntermediateProfiles.h>
47 #include <TypeIIRMLStep1Profiles.h>
48 #include <TypeIIRMLQuicksort.h>
49 #include <TypeIIRMLDecisions.h>
50 #include <TypeIIRMLDecisionTree1A.h>
51 #include <TypeIIRMLDecisionTree1B.h>
52 #include <TypeIIRMLDecisionTree1C.h>
53 #include <RMLPositionInputParameters.h>
54 #include <ReflexxesAPI.h>
55 
56 using namespace TypeIIRMLMath;
57 
58 
59 //*******************************************************************************************
60 // Step1
61 
62 void TypeIIRMLPosition::Step1(void)
63 {
64  double MaximalMinimalExecutionTime = 0.0
65  , VectorStretchFactorMaxAcceleration = 0.0
66  , VectorStretchFactorMaxVelocity = 0.0
67  , PhaseSyncTimeAverage = 0.0;
68 
69  unsigned int i = 0
70  , Counter = 0
71  , PhaseSyncDOFCounter = 0;
72 
73 
74  for(i = 0; i < this->NumberOfDOFs; i++)
75  {
76  if(this->CurrentInputParameters->SelectionVector->VecData[i])
77  {
78  TypeIIRMLDecisionTree1A( this->CurrentInputParameters->CurrentPositionVector->VecData [i]
79  , this->CurrentInputParameters->CurrentVelocityVector->VecData [i]
80  , this->CurrentInputParameters->TargetPositionVector->VecData [i]
81  , this->CurrentInputParameters->TargetVelocityVector->VecData [i]
82  , this->CurrentInputParameters->MaxVelocityVector->VecData [i]
83  , this->CurrentInputParameters->MaxAccelerationVector->VecData [i]
84  , &(this->UsedStep1AProfiles->VecData [i])
85  , &(this->MinimumExecutionTimes->VecData [i]));
86 
87  if((this->MinimumExecutionTimes->VecData)[i] > MaximalMinimalExecutionTime)
88  {
89  MaximalMinimalExecutionTime = (this->MinimumExecutionTimes->VecData)[i];
90  this->GreatestDOFForPhaseSynchronization = i;
91  }
92  }
93  }
94 
95  this->SetupModifiedSelectionVector();
96  // From now on, we only use the modified selection vector.
97 
98  if (this->CurrentTrajectoryIsNotSynchronized)
99  {
100  this->SynchronizationTime = MaximalMinimalExecutionTime;
101  return;
102  }
103 
104  // ******************************************************************
105  // phase-synchronization check
106 
107  if (this->CurrentTrajectoryIsPhaseSynchronized)
108  {
109  // check whether all vectors head into the same direction
110  this->CurrentTrajectoryIsPhaseSynchronized = IsPhaseSynchronizationPossible(this->PhaseSynchronizationReferenceVector);
111  }
112 
113  if ( (this->CurrentTrajectoryIsPhaseSynchronized)
114  && (fabs((this->PhaseSynchronizationReferenceVector->VecData)[this->GreatestDOFForPhaseSynchronization]) > ABSOLUTE_PHASE_SYNC_EPSILON) )
115  {
116  VectorStretchFactorMaxAcceleration = (this->CurrentInputParameters->MaxAccelerationVector->VecData)[this->GreatestDOFForPhaseSynchronization]
117  / fabs((this->PhaseSynchronizationReferenceVector->VecData)[this->GreatestDOFForPhaseSynchronization]);
118  VectorStretchFactorMaxVelocity = (this->CurrentInputParameters->MaxVelocityVector->VecData)[this->GreatestDOFForPhaseSynchronization]
119  / fabs((this->PhaseSynchronizationReferenceVector->VecData)[this->GreatestDOFForPhaseSynchronization]);
120 
121  for(i = 0; i < this->NumberOfDOFs; i++)
122  {
123  if((this->ModifiedSelectionVector->VecData)[i])
124  {
125  (this->PhaseSynchronizationTimeVector->VecData)[i] = 0.0;
126 
127  (this->PhaseSynchronizationMaxAccelerationVector->VecData)[i] = fabs(VectorStretchFactorMaxAcceleration
128  * (this->PhaseSynchronizationReferenceVector->VecData)[i]);
129  (this->PhaseSynchronizationMaxVelocityVector->VecData)[i] = fabs(VectorStretchFactorMaxVelocity
130  * (this->PhaseSynchronizationReferenceVector->VecData)[i]);
131 
132  if ( ( (this->PhaseSynchronizationMaxAccelerationVector->VecData)[i]
133  > ( (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i] * ( 1.0 + RELATIVE_PHASE_SYNC_EPSILON ) + ABSOLUTE_PHASE_SYNC_EPSILON ) )
134  || ( (this->PhaseSynchronizationMaxVelocityVector->VecData)[i]
135  > ( (this->CurrentInputParameters->MaxVelocityVector->VecData)[i] * ( 1.0 + RELATIVE_PHASE_SYNC_EPSILON ) + ABSOLUTE_PHASE_SYNC_EPSILON ) ) )
136  {
137  this->CurrentTrajectoryIsPhaseSynchronized = false;
138  break;
139  }
140  }
141  }
142  }
143  else
144  {
145  this->CurrentTrajectoryIsPhaseSynchronized = false;
146  }
147 
148  if (this->CurrentTrajectoryIsPhaseSynchronized)
149  {
150  *(this->PhaseSynchronizationCurrentPositionVector) = *(this->CurrentInputParameters->CurrentPositionVector);
151  *(this->PhaseSynchronizationCurrentVelocityVector) = *(this->CurrentInputParameters->CurrentVelocityVector);
152  *(this->PhaseSynchronizationTargetPositionVector) = *(this->CurrentInputParameters->TargetPositionVector);
153  *(this->PhaseSynchronizationTargetVelocityVector) = *(this->CurrentInputParameters->TargetVelocityVector);
154 
155  // check, whether all DOFs can be reached with the profile of the this->GreatestDOFForPhaseSynchronization
156 
157  for(i = 0; i < this->NumberOfDOFs; i++)
158  {
159  if (!Decision_1A__001(this->PhaseSynchronizationCurrentVelocityVector->VecData[i]))
160  {
161  NegateStep1( &(this->PhaseSynchronizationCurrentPositionVector->VecData [i] )
162  , &(this->PhaseSynchronizationCurrentVelocityVector->VecData [i] )
163  , &(this->PhaseSynchronizationTargetPositionVector->VecData [i] )
164  , &(this->PhaseSynchronizationTargetVelocityVector->VecData [i] ) );
165  }
166  if (!Decision_1A__002( this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
167  , this->PhaseSynchronizationMaxVelocityVector->VecData [i] ))
168  {
169  VToVMaxStep1( &(this->PhaseSynchronizationTimeVector->VecData [i] )
170  , &(this->PhaseSynchronizationCurrentPositionVector->VecData [i] )
171  , &(this->PhaseSynchronizationCurrentVelocityVector->VecData [i] )
172  , this->PhaseSynchronizationMaxVelocityVector->VecData [i]
173  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] );
174  }
175 
176  if ( (this->UsedStep1AProfiles->VecData[this->GreatestDOFForPhaseSynchronization] == Step1_Profile_NegLinHldPosLin )
177  || (this->UsedStep1AProfiles->VecData[this->GreatestDOFForPhaseSynchronization] == Step1_Profile_NegLinPosLin )
178  || (this->UsedStep1AProfiles->VecData[this->GreatestDOFForPhaseSynchronization] == Step1_Profile_NegTrapPosLin )
179  || (this->UsedStep1AProfiles->VecData[this->GreatestDOFForPhaseSynchronization] == Step1_Profile_NegTriPosLin ) )
180  {
181  VToZeroStep1( &(this->PhaseSynchronizationTimeVector->VecData [i] )
182  , &(this->PhaseSynchronizationCurrentPositionVector->VecData [i] )
183  , &(this->PhaseSynchronizationCurrentVelocityVector->VecData [i] )
184  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] );
185 
186  NegateStep1( &(this->PhaseSynchronizationCurrentPositionVector->VecData [i] )
187  , &(this->PhaseSynchronizationCurrentVelocityVector->VecData [i] )
188  , &(this->PhaseSynchronizationTargetPositionVector->VecData [i] )
189  , &(this->PhaseSynchronizationTargetVelocityVector->VecData [i] ) );
190  }
191  }
192 
193  switch((this->UsedStep1AProfiles->VecData)[this->GreatestDOFForPhaseSynchronization])
194  {
195  case Step1_Profile_PosLinHldNegLin:
196  case Step1_Profile_NegLinHldPosLin:
197  for(i = 0; i < this->NumberOfDOFs; i++)
198  {
199  if((this->ModifiedSelectionVector->VecData)[i])
200  {
201  if (!IsSolutionForProfile_PosLinHldNegLin_Possible( this->PhaseSynchronizationCurrentPositionVector->VecData [i]
202  , this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
203  , this->PhaseSynchronizationTargetPositionVector->VecData [i]
204  , this->PhaseSynchronizationTargetVelocityVector->VecData [i]
205  , this->PhaseSynchronizationMaxVelocityVector->VecData [i]
206  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] ))
207  {
208  this->CurrentTrajectoryIsPhaseSynchronized = false;
209  break;
210  }
211  }
212  }
213  break;
214  case Step1_Profile_PosLinNegLin:
215  case Step1_Profile_NegLinPosLin:
216  for(i = 0; i < this->NumberOfDOFs; i++)
217  {
218  if((this->ModifiedSelectionVector->VecData)[i])
219  {
220  if (!IsSolutionForProfile_PosLinNegLin_Possible( this->PhaseSynchronizationCurrentPositionVector->VecData [i]
221  , this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
222  , this->PhaseSynchronizationTargetPositionVector->VecData [i]
223  , this->PhaseSynchronizationTargetVelocityVector->VecData [i]
224  , this->PhaseSynchronizationMaxVelocityVector->VecData [i]
225  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] ))
226  {
227  this->CurrentTrajectoryIsPhaseSynchronized = false;
228  break;
229  }
230  }
231  }
232  break;
233  case Step1_Profile_PosTrapNegLin:
234  case Step1_Profile_NegTrapPosLin:
235  for(i = 0; i < this->NumberOfDOFs; i++)
236  {
237  if((this->ModifiedSelectionVector->VecData)[i])
238  {
239  if (!IsSolutionForProfile_PosTrapNegLin_Possible( this->PhaseSynchronizationCurrentPositionVector->VecData [i]
240  , this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
241  , this->PhaseSynchronizationTargetPositionVector->VecData [i]
242  , this->PhaseSynchronizationTargetVelocityVector->VecData [i]
243  , this->PhaseSynchronizationMaxVelocityVector->VecData [i]
244  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] ))
245  {
246  this->CurrentTrajectoryIsPhaseSynchronized = false;
247  break;
248  }
249  }
250  }
251  break;
252  case Step1_Profile_PosTriNegLin:
253  case Step1_Profile_NegTriPosLin:
254  for(i = 0; i < this->NumberOfDOFs; i++)
255  {
256  if((this->ModifiedSelectionVector->VecData)[i])
257  {
258  if (!IsSolutionForProfile_PosTriNegLin_Possible( this->PhaseSynchronizationCurrentPositionVector->VecData [i]
259  , this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
260  , this->PhaseSynchronizationTargetPositionVector->VecData [i]
261  , this->PhaseSynchronizationTargetVelocityVector->VecData [i]
262  , this->PhaseSynchronizationMaxVelocityVector->VecData [i]
263  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] ))
264  {
265  this->CurrentTrajectoryIsPhaseSynchronized = false;
266  break;
267  }
268  }
269  }
270  break;
271  default:
272  this->CurrentTrajectoryIsPhaseSynchronized = false;
273  break;
274  }
275  }
276 
277  if (this->CurrentTrajectoryIsPhaseSynchronized)
278  {
279  this->MotionProfileForPhaseSynchronization = (unsigned int)(this->UsedStep1AProfiles->VecData)[this->GreatestDOFForPhaseSynchronization];
280 
281  // Within the following
282  // switch/case part, the result value for
283  // ErrorDuringCalculationOfASynchronizedProfile
284  // does not have to be checked as the solution
285  // for the desired value of tmin will always be valid.
286 
287  switch(this->MotionProfileForPhaseSynchronization)
288  {
289  case Step1_Profile_PosLinHldNegLin:
290  case Step1_Profile_NegLinHldPosLin:
291  for(i = 0; i < this->NumberOfDOFs; i++)
292  {
293  if((this->ModifiedSelectionVector->VecData)[i])
294  {
295  this->PhaseSynchronizationTimeVector->VecData[i]
296  += ProfileStep1PosLinHldNegLin( this->PhaseSynchronizationCurrentPositionVector->VecData [i]
297  , this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
298  , this->PhaseSynchronizationTargetPositionVector->VecData [i]
299  , this->PhaseSynchronizationTargetVelocityVector->VecData [i]
300  , this->PhaseSynchronizationMaxVelocityVector->VecData [i]
301  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] );
302  }
303  }
304  break;
305  case Step1_Profile_PosLinNegLin:
306  case Step1_Profile_NegLinPosLin:
307  for(i = 0; i < this->NumberOfDOFs; i++)
308  {
309  if((this->ModifiedSelectionVector->VecData)[i])
310  {
311  this->PhaseSynchronizationTimeVector->VecData[i]
312  += ProfileStep1PosLinNegLin( this->PhaseSynchronizationCurrentPositionVector->VecData [i]
313  , this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
314  , this->PhaseSynchronizationTargetPositionVector->VecData [i]
315  , this->PhaseSynchronizationTargetVelocityVector->VecData [i]
316  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] );
317  }
318  }
319  break;
320  case Step1_Profile_PosTrapNegLin:
321  case Step1_Profile_NegTrapPosLin:
322  for(i = 0; i < this->NumberOfDOFs; i++)
323  {
324  if((this->ModifiedSelectionVector->VecData)[i])
325  {
326  this->PhaseSynchronizationTimeVector->VecData[i]
327  += ProfileStep1PosTrapNegLin( this->PhaseSynchronizationCurrentPositionVector->VecData [i]
328  , this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
329  , this->PhaseSynchronizationTargetPositionVector->VecData [i]
330  , this->PhaseSynchronizationTargetVelocityVector->VecData [i]
331  , this->PhaseSynchronizationMaxVelocityVector->VecData [i]
332  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] );
333  }
334  }
335  break;
336  case Step1_Profile_PosTriNegLin:
337  case Step1_Profile_NegTriPosLin:
338  for(i = 0; i < this->NumberOfDOFs; i++)
339  {
340  if((this->ModifiedSelectionVector->VecData)[i])
341  {
342  this->PhaseSynchronizationTimeVector->VecData[i]
343  += ProfileStep1PosTriNegLin( this->PhaseSynchronizationCurrentPositionVector->VecData [i]
344  , this->PhaseSynchronizationCurrentVelocityVector->VecData [i]
345  , this->PhaseSynchronizationTargetPositionVector->VecData [i]
346  , this->PhaseSynchronizationTargetVelocityVector->VecData [i]
347  , this->PhaseSynchronizationMaxAccelerationVector->VecData [i] );
348  }
349  }
350  break;
351  default:
352  this->CurrentTrajectoryIsPhaseSynchronized = false;
353  break;
354  }
355 
356  PhaseSyncTimeAverage = 0.0;
357  PhaseSyncDOFCounter = 0;
358 
359  for(i = 0; i < this->NumberOfDOFs; i++)
360  {
361  if((this->ModifiedSelectionVector->VecData)[i])
362  {
363  PhaseSyncTimeAverage += (this->PhaseSynchronizationTimeVector->VecData)[i];
364  PhaseSyncDOFCounter++;
365  }
366  }
367 
368  if (PhaseSyncDOFCounter == 0)
369  {
370  return;
371  }
372 
373  PhaseSyncTimeAverage /= ((double)PhaseSyncDOFCounter);
374 
375  for(i = 0; i < this->NumberOfDOFs; i++)
376  {
377  if ((this->ModifiedSelectionVector->VecData)[i])
378  {
379  if ( fabs((this->PhaseSynchronizationTimeVector->VecData)[i] - PhaseSyncTimeAverage)
380  > (ABSOLUTE_PHASE_SYNC_EPSILON + RELATIVE_PHASE_SYNC_EPSILON * PhaseSyncTimeAverage) )
381  {
382  this->CurrentTrajectoryIsPhaseSynchronized = false;
383  break;
384  }
385  }
386  }
387  }
388 
389  if (this->CurrentTrajectoryIsPhaseSynchronized)
390  {
391  this->SynchronizationTime = this->MinimumExecutionTimes->VecData[this->GreatestDOFForPhaseSynchronization];
392  return;
393  }
394 
395  // ******************************************************************
396  // Decision trees 1B and 1C
397 
398  for(i = 0; i < this->NumberOfDOFs; i++)
399  {
400  if ((this->ModifiedSelectionVector->VecData)[i])
401  {
402  TypeIIRMLDecisionTree1B( this->CurrentInputParameters->CurrentPositionVector->VecData [i]
403  , this->CurrentInputParameters->CurrentVelocityVector->VecData [i]
404  , this->CurrentInputParameters->TargetPositionVector->VecData [i]
405  , this->CurrentInputParameters->TargetVelocityVector->VecData [i]
406  , this->CurrentInputParameters->MaxVelocityVector->VecData [i]
407  , this->CurrentInputParameters->MaxAccelerationVector->VecData [i]
408  , &(this->BeginningsOfInoperativeTimeIntervals->VecData [i]));
409 
410  if (this->BeginningsOfInoperativeTimeIntervals->VecData[i] != RML_INFINITY)
411  {
412  TypeIIRMLDecisionTree1C( this->CurrentInputParameters->CurrentPositionVector->VecData [i]
413  , this->CurrentInputParameters->CurrentVelocityVector->VecData [i]
414  , this->CurrentInputParameters->TargetPositionVector->VecData [i]
415  , this->CurrentInputParameters->TargetVelocityVector->VecData [i]
416  , this->CurrentInputParameters->MaxVelocityVector->VecData [i]
417  , this->CurrentInputParameters->MaxAccelerationVector->VecData [i]
418  , &(this->EndingsOfInoperativeTimeIntervals->VecData [i]));
419  }
420  else
421  {
422  this->EndingsOfInoperativeTimeIntervals->VecData[i] = RML_INFINITY;
423  }
424  }
425  else
426  {
427  this->BeginningsOfInoperativeTimeIntervals->VecData [i] = RML_INFINITY;
428  this->EndingsOfInoperativeTimeIntervals->VecData [i] = RML_INFINITY;
429  }
430  }
431 
432  for(i = 0; i < this->NumberOfDOFs; i++)
433  {
434  if((this->ModifiedSelectionVector->VecData)[i])
435  {
436  if ( (this->BeginningsOfInoperativeTimeIntervals->VecData)[i] < (this->MinimumExecutionTimes->VecData)[i] )
437  {
438  (this->BeginningsOfInoperativeTimeIntervals->VecData)[i]
439  = (this->MinimumExecutionTimes->VecData)[i];
440  }
441 
442  if ( (this->EndingsOfInoperativeTimeIntervals->VecData)[i] < (this->BeginningsOfInoperativeTimeIntervals->VecData)[i] )
443  {
444  (this->EndingsOfInoperativeTimeIntervals->VecData)[i]
445  = (this->BeginningsOfInoperativeTimeIntervals->VecData)[i]
446  = ((this->BeginningsOfInoperativeTimeIntervals->VecData)[i]
447  + (this->EndingsOfInoperativeTimeIntervals->VecData)[i]) * 0.5;
448 
449  if ( (this->BeginningsOfInoperativeTimeIntervals->VecData)[i] < (this->MinimumExecutionTimes->VecData)[i] )
450  {
451  (this->MinimumExecutionTimes->VecData)[i]
452  = (this->EndingsOfInoperativeTimeIntervals->VecData)[i];
453  (this->BeginningsOfInoperativeTimeIntervals->VecData)[i] = RML_INFINITY;
454  (this->EndingsOfInoperativeTimeIntervals->VecData)[i] = RML_INFINITY;
455  }
456  }
457 
458  if ( (this->EndingsOfInoperativeTimeIntervals->VecData)[i] < (this->MinimumExecutionTimes->VecData)[i] )
459  {
460  (this->BeginningsOfInoperativeTimeIntervals->VecData)[i] = RML_INFINITY;
461  (this->EndingsOfInoperativeTimeIntervals->VecData)[i] = RML_INFINITY;
462  }
463  }
464  }
465 
466 
467  //***************
468  // Calculate tsync
469 
470  // Determine the maximum of the minimal execution times
471  for(i = 0; i < this->NumberOfDOFs; i++)
472  {
473  if(!(this->ModifiedSelectionVector->VecData)[i])
474  {
475  (this->BeginningsOfInoperativeTimeIntervals->VecData)[i] = RML_INFINITY;
476  (this->EndingsOfInoperativeTimeIntervals->VecData)[i] = RML_INFINITY;
477  }
478 
479  (this->ArrayOfSortedTimes->VecData)[i]
480  = (this->BeginningsOfInoperativeTimeIntervals->VecData)[i];
481 
482  (this->ArrayOfSortedTimes->VecData)[i + this->NumberOfDOFs]
483  = (this->EndingsOfInoperativeTimeIntervals->VecData)[i];
484  }
485 
486  // Quicksort sorts all values of AlternativeExecutuionTime independent of the SelectionVector
487  // because of that the alternative execution times of all no selected DOFs are set to infinity
488 
489  // Sort the alternative execution times
490  Quicksort(0, (2 * this->NumberOfDOFs - 1), &((this->ArrayOfSortedTimes->VecData)[0]));
491 
492  // Which alternative execution times are bigger than the maximum of the minimal execution times
493  for (Counter = 0; Counter < 2 * this->NumberOfDOFs; Counter++)
494  {
495  if ((this->ArrayOfSortedTimes->VecData)[Counter] > MaximalMinimalExecutionTime)
496  {
497  break;
498  }
499  }
500 
501  this->SynchronizationTime = MaximalMinimalExecutionTime;
502 
503  //calculate the minimal time, which is not in death-zone
504  while((IsWithinAnInoperativeTimeInterval( this->SynchronizationTime
505  , *(this->BeginningsOfInoperativeTimeIntervals)
506  , *(this->EndingsOfInoperativeTimeIntervals) ) )
507  && (Counter < 2 * this->NumberOfDOFs) )
508  {
509  this->SynchronizationTime = (this->ArrayOfSortedTimes->VecData)[Counter];
510  Counter++;
511  }
512 
513  return;
514 }
515 
516 
517 //************************************************************************************
518 // IsWithinAnInoperativeTimeInterval
519 
520 bool TypeIIRMLPosition::IsWithinAnInoperativeTimeInterval( const double &SynchronizationTimeCandidate
521  , const RMLDoubleVector &MaximalExecutionTime
522  , const RMLDoubleVector &AlternativeExecutionTime) const
523 {
524  unsigned int i;
525 
526  for (i = 0; i < this->NumberOfDOFs; i++)
527  {
528  if ((this->ModifiedSelectionVector->VecData)[i])
529  {
530  if ( ((MaximalExecutionTime.VecData)[i] < SynchronizationTimeCandidate)
531  && (SynchronizationTimeCandidate < (AlternativeExecutionTime.VecData)[i]) )
532  {
533  return(true);
534  }
535  }
536  }
537  return(false);
538 }
539 
ABSOLUTE_PHASE_SYNC_EPSILON
#define ABSOLUTE_PHASE_SYNC_EPSILON
Absolute epsilon to check whether all required vectors are collinear.
Definition: TypeIIRMLMath.h:179
ReflexxesAPI.h
Header file for the class ReflexxesAPI (API of the Reflexxes Motion Libraries)
TypeIIRMLStep1IntermediateProfiles.h
Header file for intermediate profile segments of Step 1.
TypeIIRMLMath::VToVMaxStep1
void VToVMaxStep1(double *TotalTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxVelocity, const double &MaxAcceleration)
One intermediate Step 1 trajectory segment: v -> +vmax (NegLin)
Definition: TypeIIRMLStep1IntermediateProfiles.cpp:72
TypeIIRMLMath::ProfileStep1PosTriNegLin
double ProfileStep1PosTriNegLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
Calculates the execution time of the PosTriNegLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:91
TypeIIRMLMath::TypeIIRMLDecisionTree1A
void TypeIIRMLDecisionTree1A(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, Step1_Profile *AppliedProfile, double *MinimalExecutionTime)
This function contains the decision tree 1A of the Step 1 of the Type II On-Line Trajectory Generatio...
Definition: TypeIIRMLDecisionTree1A.cpp:56
RMLVector::VecData
T * VecData
Pointer to the actual vector data, that is, an array of type T objects.
Definition: RMLVector.h:524
TypeIIRMLDecisions.h
Header file for decisions of the two decision trees of the Type II On-Line Trajectory Generation algo...
TypeIIRMLQuicksort.h
Header file for the Quicksort algorithm.
TypeIIRMLMath::IsSolutionForProfile_PosLinNegLin_Possible
bool IsSolutionForProfile_PosLinNegLin_Possible(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Checks whether a valid solution for the PosLinNegLin velocity profile would be possible.
Definition: TypeIIRMLStep1Profiles.cpp:180
TypeIIRMLMath::NegateStep1
void NegateStep1(double *ThisCurrentPosition, double *ThisCurrentVelocity, double *ThisTargetPosition, double *ThisTargetVelocity)
Negate input values during Step 1.
Definition: TypeIIRMLStep1IntermediateProfiles.cpp:55
TypeIIRMLMath.h
Header file for functions and definitions of constant values and macros.
TypeIIRMLMath::TypeIIRMLDecisionTree1B
void TypeIIRMLDecisionTree1B(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, double *MaximalExecutionTime)
This function contains the decision tree 1B of the Step 1 of the Type II On-Line Trajectory Generatio...
Definition: TypeIIRMLDecisionTree1B.cpp:55
RML_INFINITY
#define RML_INFINITY
A value for infinity .
Definition: TypeIIRMLMath.h:201
TypeIIRMLMath::ProfileStep1PosLinHldNegLin
double ProfileStep1PosLinHldNegLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Calculates the execution time of the PosLinHldNegLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:54
RMLPositionInputParameters.h
Header file for the class RMLPositionInputParameters.
TypeIIRMLMath::IsSolutionForProfile_PosTriNegLin_Possible
bool IsSolutionForProfile_PosTriNegLin_Possible(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Checks whether a valid solution for the PosTriNegLin velocity profile would be possible.
Definition: TypeIIRMLStep1Profiles.cpp:254
TypeIIRMLPosition.h
Header file for the class TypeIIRMLPosition, which constitutes the actual interface of the Type II Re...
TypeIIRMLDecisionTree1C.h
Header file for the Step 1 decision tree 1C of the Type II On-Line Trajectory Generation algorithm...
TypeIIRMLMath::VToZeroStep1
void VToZeroStep1(double *TotalTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxAcceleration)
One intermediate Step 1 trajectory segment: v -> 0 (NegLin)
Definition: TypeIIRMLStep1IntermediateProfiles.cpp:93
TypeIIRMLMath::ProfileStep1PosTrapNegLin
double ProfileStep1PosTrapNegLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Calculates the execution time of the PosTrapNegLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:109
TypeIIRMLDecisionTree1B.h
Header file for the Step 1 decision tree 1B of the Type II On-Line Trajectory Generation algorithm...
TypeIIRMLMath::Decision_1A__002
bool Decision_1A__002(const double &CurrentVelocity, const double &MaxVelocity)
Is (vi <= +vmax)?
Definition: TypeIIRMLDecisions.cpp:61
TypeIIRMLMath::IsSolutionForProfile_PosLinHldNegLin_Possible
bool IsSolutionForProfile_PosLinHldNegLin_Possible(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Checks whether a valid solution for the PosLinHldNegLin velocity profile would be possible...
Definition: TypeIIRMLStep1Profiles.cpp:144
TypeIIRMLDecisionTree1A.h
Header file for the Step 1 decision tree 1A of the Type II On-Line Trajectory Generation algorithm...
TypeIIRMLStep1Profiles.h
Header file for the Step 1 motion profiles.
TypeIIRMLMath::IsSolutionForProfile_PosTrapNegLin_Possible
bool IsSolutionForProfile_PosTrapNegLin_Possible(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration)
Checks whether a valid solution for the PosTrapNegLin velocity profile would be possible.
Definition: TypeIIRMLStep1Profiles.cpp:327
RELATIVE_PHASE_SYNC_EPSILON
#define RELATIVE_PHASE_SYNC_EPSILON
Relative epsilon to check whether all required vectors are collinear.
Definition: TypeIIRMLMath.h:192
TypeIIRMLMath::TypeIIRMLDecisionTree1C
void TypeIIRMLDecisionTree1C(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, double *AlternativeExecutionTime)
This function contains the decision tree 1C of the Step 1 of the Type II On-Line Trajectory Generatio...
Definition: TypeIIRMLDecisionTree1C.cpp:56
TypeIIRMLMath::ProfileStep1PosLinNegLin
double ProfileStep1PosLinNegLin(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
Calculates the execution time of the PosLinNegLin velocity profile.
Definition: TypeIIRMLStep1Profiles.cpp:73
TypeIIRMLMath::Decision_1A__001
bool Decision_1A__001(const double &CurrentVelocity)
Is (vi >= 0)?
Definition: TypeIIRMLDecisions.cpp:52
TypeIIRMLPosition::IsWithinAnInoperativeTimeInterval
bool IsWithinAnInoperativeTimeInterval(const double &SynchronizationTimeCandidate, const RMLDoubleVector &MaximalExecutionTime, const RMLDoubleVector &AlternativeExecutionTime) const
Checks, whether the value SynchronizationTimeCandidate lies within an inoperative timer interval...
Definition: TypeIIRMLStep1.cpp:520
RMLVector
This is a minimalistic dynamic vector class implementation used for the Reflexxes Motion Libraries...
Definition: RMLVector.h:77
TypeIIRMLPosition::Step1
void Step1(void)
Step 1 of the On-Line Trajectory Generation algorithm: Calculate the synchronization time ...
Definition: TypeIIRMLStep1.cpp:62
TypeIIRMLMath::Quicksort
void Quicksort(const int &LeftBound, const int &RightBound, double *ArrayOfValues)
Standard implementation of the Quicksort algorithm for double values.
Definition: TypeIIRMLQuicksort.cpp:52

libreflexxestype2
Author(s):
autogenerated on Sat Nov 21 2020 03:17:34