TypeIIRMLDecisionTree2.cpp
Go to the documentation of this file.
1 // ---------------------- Doxygen info ----------------------
39 // ----------------------------------------------------------
40 // For a convenient reading of this file's source code,
41 // please use a tab width of four characters.
42 // ----------------------------------------------------------
43 
44 
45 #include <TypeIIRMLDecisionTree2.h>
46 #include <TypeIIRMLStep2Profiles.h>
47 #include <TypeIIRMLMath.h>
48 #include <TypeIIRMLDecisions.h>
49 #include <TypeIIRMLStep2IntermediateProfiles.h>
50 #include <TypeIIRMLStep2Profiles.h>
51 
52 
53 //************************************************************************************
54 // TypeIIRMLDecisionTree2()
55 
56 void TypeIIRMLMath::TypeIIRMLDecisionTree2( const double &CurrentPosition
57  , const double &CurrentVelocity
58  , const double &TargetPosition
59  , const double &TargetVelocity
60  , const double &MaxVelocity
61  , const double &MaxAcceleration
62  , const double &SynchronizationTime
63  , MotionPolynomials *PolynomialsInternal)
64 {
65  bool Inverted = false ;
66 
67  double CurrentTime = 0.0
68  , ThisCurrentPosition = CurrentPosition
69  , ThisCurrentVelocity = CurrentVelocity
70  , ThisTargetPosition = TargetPosition
71  , ThisTargetVelocity = TargetVelocity ;
72 
73  // ********************************************************************
74  if (Decision_2___001(ThisCurrentVelocity))
75  {
76  goto MDecision_2___002;
77  }
78  else
79  {
80  NegateStep2( &ThisCurrentPosition
81  , &ThisCurrentVelocity
82  , &ThisTargetPosition
83  , &ThisTargetVelocity
84  , &Inverted );
85 
86  goto MDecision_2___002;
87  }
88  // ********************************************************************
89 MDecision_2___002:
90  if (Decision_2___002( ThisCurrentVelocity
91  , MaxVelocity ))
92  {
93  goto MDecision_2___003;
94  }
95  else
96  {
97  VToVMaxStep2( &CurrentTime
98  , &ThisCurrentPosition
99  , &ThisCurrentVelocity
100  , MaxVelocity
101  , MaxAcceleration
102  , &(*PolynomialsInternal)
103  , Inverted );
104 
105  goto MDecision_2___003;
106  }
107  // ********************************************************************
108 MDecision_2___003:
109  if (Decision_2___003( ThisCurrentVelocity
110  , ThisTargetVelocity ))
111  {
112  goto MDecision_2___004;
113  }
114  else
115  {
116  goto MDecision_2___007;
117  }
118  // ********************************************************************
119 MDecision_2___004:
120  if (Decision_2___004( ThisCurrentPosition
121  , ThisCurrentVelocity
122  , ThisTargetPosition
123  , ThisTargetVelocity
124  , MaxAcceleration
125  , CurrentTime
126  , SynchronizationTime ))
127  {
128  ProfileStep2PosLinHldNegLin( CurrentTime
129  , SynchronizationTime
130  , ThisCurrentPosition
131  , ThisCurrentVelocity
132  , ThisTargetPosition
133  , ThisTargetVelocity
134  , MaxAcceleration
135  , &(*PolynomialsInternal)
136  , Inverted );
137  goto END_OF_THIS_FUNCTION;
138  }
139  else
140  {
141  goto MDecision_2___005;
142  }
143  // ********************************************************************
144 MDecision_2___005:
145  if (Decision_2___005( ThisCurrentPosition
146  , ThisCurrentVelocity
147  , ThisTargetPosition
148  , ThisTargetVelocity
149  , MaxAcceleration
150  , CurrentTime
151  , SynchronizationTime ))
152  {
153  ProfileStep2PosLinHldPosLin( CurrentTime
154  , SynchronizationTime
155  , ThisCurrentPosition
156  , ThisCurrentVelocity
157  , ThisTargetPosition
158  , ThisTargetVelocity
159  , MaxAcceleration
160  , &(*PolynomialsInternal)
161  , Inverted );
162  goto END_OF_THIS_FUNCTION;
163  }
164  else
165  {
166  goto MDecision_2___006;
167  }
168  // ********************************************************************
169 MDecision_2___006:
170  if (Decision_2___006( CurrentTime
171  , SynchronizationTime
172  , ThisCurrentPosition
173  , ThisCurrentVelocity
174  , ThisTargetPosition
175  , ThisTargetVelocity
176  , MaxAcceleration ))
177  {
178  ProfileStep2NegLinHldPosLin( CurrentTime
179  , SynchronizationTime
180  , ThisCurrentPosition
181  , ThisCurrentVelocity
182  , ThisTargetPosition
183  , ThisTargetVelocity
184  , MaxAcceleration
185  , &(*PolynomialsInternal)
186  , Inverted );
187  goto END_OF_THIS_FUNCTION;
188  }
189  else
190  {
191  VToZeroStep2( &CurrentTime
192  , &ThisCurrentPosition
193  , &ThisCurrentVelocity
194  , MaxAcceleration
195  , &(*PolynomialsInternal)
196  , Inverted );
197 
198  NegateStep2( &ThisCurrentPosition
199  , &ThisCurrentVelocity
200  , &ThisTargetPosition
201  , &ThisTargetVelocity
202  , &Inverted );
203 
204  ProfileStep2PosTrapNegLin( CurrentTime
205  , SynchronizationTime
206  , ThisCurrentPosition
207  , ThisCurrentVelocity
208  , ThisTargetPosition
209  , ThisTargetVelocity
210  , MaxAcceleration
211  , &(*PolynomialsInternal)
212  , Inverted );
213  goto END_OF_THIS_FUNCTION;
214 
215  }
216  // ********************************************************************
217 MDecision_2___007:
218  if (Decision_2___007(ThisTargetVelocity))
219  {
220  goto MDecision_2___008;
221  }
222  else
223  {
224  goto MDecision_2___010;
225  }
226  // ********************************************************************
227 MDecision_2___008:
228  if (Decision_2___008( ThisCurrentPosition
229  , ThisCurrentVelocity
230  , ThisTargetPosition
231  , ThisTargetVelocity
232  , MaxAcceleration
233  , CurrentTime
234  , SynchronizationTime ))
235  {
236  ProfileStep2PosLinHldNegLin( CurrentTime
237  , SynchronizationTime
238  , ThisCurrentPosition
239  , ThisCurrentVelocity
240  , ThisTargetPosition
241  , ThisTargetVelocity
242  , MaxAcceleration
243  , &(*PolynomialsInternal)
244  , Inverted );
245  goto END_OF_THIS_FUNCTION;
246  }
247  else
248  {
249  goto MDecision_2___009;
250  }
251  // ********************************************************************
252 MDecision_2___009:
253  if (Decision_2___009( ThisCurrentPosition
254  , ThisCurrentVelocity
255  , ThisTargetPosition
256  , ThisTargetVelocity
257  , MaxAcceleration
258  , CurrentTime
259  , SynchronizationTime ))
260  {
261  ProfileStep2NegLinHldNegLin( CurrentTime
262  , SynchronizationTime
263  , ThisCurrentPosition
264  , ThisCurrentVelocity
265  , ThisTargetPosition
266  , ThisTargetVelocity
267  , MaxAcceleration
268  , &(*PolynomialsInternal)
269  , Inverted );
270  goto END_OF_THIS_FUNCTION;
271  }
272  else
273  {
274  goto MDecision_2___006;
275  }
276  // ********************************************************************
277 MDecision_2___010:
278  if (Decision_2___010( ThisCurrentPosition
279  , ThisCurrentVelocity
280  , ThisTargetPosition
281  , ThisTargetVelocity
282  , MaxAcceleration ))
283  {
284  VToZeroStep2( &CurrentTime
285  , &ThisCurrentPosition
286  , &ThisCurrentVelocity
287  , MaxAcceleration
288  , &(*PolynomialsInternal)
289  , Inverted );
290 
291  NegateStep2( &ThisCurrentPosition
292  , &ThisCurrentVelocity
293  , &ThisTargetPosition
294  , &ThisTargetVelocity
295  , &Inverted );
296 
297  goto MDecision_2___011;
298  }
299  else
300  {
301  goto MDecision_2___012;
302  }
303  // ********************************************************************
304 MDecision_2___011:
305  if (Decision_2___011( ThisCurrentPosition
306  , ThisCurrentVelocity
307  , ThisTargetPosition
308  , ThisTargetVelocity
309  , MaxAcceleration
310  , CurrentTime
311  , SynchronizationTime ))
312  {
313  ProfileStep2PosLinHldNegLin( CurrentTime
314  , SynchronizationTime
315  , ThisCurrentPosition
316  , ThisCurrentVelocity
317  , ThisTargetPosition
318  , ThisTargetVelocity
319  , MaxAcceleration
320  , &(*PolynomialsInternal)
321  , Inverted );
322  goto END_OF_THIS_FUNCTION;
323  }
324  else
325  {
326  ProfileStep2PosLinHldPosLin( CurrentTime
327  , SynchronizationTime
328  , ThisCurrentPosition
329  , ThisCurrentVelocity
330  , ThisTargetPosition
331  , ThisTargetVelocity
332  , MaxAcceleration
333  , &(*PolynomialsInternal)
334  , Inverted );
335  goto END_OF_THIS_FUNCTION;
336  }
337  // ********************************************************************
338 MDecision_2___012:
339  if (Decision_2___012( ThisCurrentPosition
340  , ThisCurrentVelocity
341  , ThisTargetPosition
342  , ThisTargetVelocity
343  , MaxAcceleration
344  , CurrentTime
345  , SynchronizationTime ))
346  {
347  ProfileStep2PosTrapNegLin( CurrentTime
348  , SynchronizationTime
349  , ThisCurrentPosition
350  , ThisCurrentVelocity
351  , ThisTargetPosition
352  , ThisTargetVelocity
353  , MaxAcceleration
354  , &(*PolynomialsInternal)
355  , Inverted );
356  goto END_OF_THIS_FUNCTION;
357  }
358  else
359  {
360  ProfileStep2NegLinHldNegLinNegLin( CurrentTime
361  , SynchronizationTime
362  , ThisCurrentPosition
363  , ThisCurrentVelocity
364  , ThisTargetPosition
365  , ThisTargetVelocity
366  , MaxAcceleration
367  , &(*PolynomialsInternal)
368  , Inverted );
369  goto END_OF_THIS_FUNCTION;
370  }
371  // ********************************************************************
372 END_OF_THIS_FUNCTION:
373 
374  return;
375 }
376 
TypeIIRMLMath::ProfileStep2PosTrapNegLin
void ProfileStep2PosTrapNegLin(const double &CurrentTime, const double &SynchronizationTime, const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
Parameterization of the Step 2 velocity profile PosTrapNegLin (leaf of the Step 2 decision tree) ...
Definition: TypeIIRMLStep2Profiles.cpp:652
TypeIIRMLMath::ProfileStep2NegLinHldNegLin
void ProfileStep2NegLinHldNegLin(const double &CurrentTime, const double &SynchronizationTime, const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
Parameterization of the Step 2 velocity profile NegLinHldNegLin (leaf of the Step 2 decision tree) ...
Definition: TypeIIRMLStep2Profiles.cpp:504
TypeIIRMLMath::VToVMaxStep2
void VToVMaxStep2(double *ThisCurrentTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
One intermediate Step 2 trajectory segment: v -> vmax (NegLin)
Definition: TypeIIRMLStep2IntermediateProfiles.cpp:74
TypeIIRMLMath::NegateStep2
void NegateStep2(double *ThisCurrentPosition, double *ThisCurrentVelocity, double *ThisTargetPosition, double *ThisTargetVelocity, bool *Inverted)
Negate input values.
Definition: TypeIIRMLStep2IntermediateProfiles.cpp:55
TypeIIRMLMath::Decision_2___009
bool Decision_2___009(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, const double &CurrentTime, const double &SynchronizationTime)
If v->vtrgt->hold, so that t=tsync, is p<=ptrgt?
Definition: TypeIIRMLDecisions.cpp:412
TypeIIRMLStep2Profiles.h
Header file for the Step 2 motion profiles.
TypeIIRMLMath::TypeIIRMLDecisionTree2
void TypeIIRMLDecisionTree2(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, const double &SynchronizationTime, MotionPolynomials *PolynomialsInternal)
This function contains the decision tree of the Step 2 of the Type II On-Line Trajectory Generation a...
Definition: TypeIIRMLDecisionTree2.cpp:56
TypeIIRMLMath::Decision_2___002
bool Decision_2___002(const double &CurrentVelocity, const double &MaxVelocity)
Is (vi <= +vmax)?
Definition: TypeIIRMLDecisions.cpp:308
TypeIIRMLDecisions.h
Header file for decisions of the two decision trees of the Type II On-Line Trajectory Generation algo...
TypeIIRMLMath::MotionPolynomials
Three arrays of TypeIIRMLMath::TypeIIRMLPolynomial.
Definition: TypeIIRMLPolynomial.h:260
TypeIIRMLMath::ProfileStep2NegLinHldPosLin
void ProfileStep2NegLinHldPosLin(const double &CurrentTime, const double &SynchronizationTime, const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
Parameterization of the Step 2 velocity profile NegLinHldPosLin (leaf of the Step 2 decision tree) ...
Definition: TypeIIRMLStep2Profiles.cpp:353
TypeIIRMLMath::ProfileStep2PosLinHldNegLin
void ProfileStep2PosLinHldNegLin(const double &CurrentTime, const double &SynchronizationTime, const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
Parameterization of the Step 2 velocity profile PosLinHldNegLin (leaf of the Step 2 decision tree) ...
Definition: TypeIIRMLStep2Profiles.cpp:54
TypeIIRMLMath::Decision_2___010
bool Decision_2___010(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
If v->0->vtrgt, is p>=ptrgt?
Definition: TypeIIRMLDecisions.cpp:430
TypeIIRMLMath.h
Header file for functions and definitions of constant values and macros.
TypeIIRMLMath::Decision_2___006
bool Decision_2___006(const double &CurrentTime, const double &SynchronizationTime, const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration)
If (v->0->vtrgt, is p<=ptrgt || t > tsync)?
Definition: TypeIIRMLDecisions.cpp:366
TypeIIRMLDecisionTree2.h
Header file for the Step 2 decision tree of the Type II On-Line Trajectory Generation algorithm (time...
TypeIIRMLMath::Decision_2___001
bool Decision_2___001(const double &CurrentVelocity)
Is (vi >= 0)?
Definition: TypeIIRMLDecisions.cpp:299
TypeIIRMLMath::Decision_2___007
bool Decision_2___007(const double &TargetVelocity)
Is (vtrgt >= 0)?
Definition: TypeIIRMLDecisions.cpp:385
TypeIIRMLMath::VToZeroStep2
void VToZeroStep2(double *ThisCurrentTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
One intermediate Step 2 trajectory segment: v -> 0 (NegLin)
Definition: TypeIIRMLStep2IntermediateProfiles.cpp:114
TypeIIRMLMath::Decision_2___012
bool Decision_2___012(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, const double &CurrentTime, const double &SynchronizationTime)
If v->hold->0->vtrgt, so that t=tsync, is p<=ptrgt?
Definition: TypeIIRMLDecisions.cpp:468
TypeIIRMLMath::ProfileStep2NegLinHldNegLinNegLin
void ProfileStep2NegLinHldNegLinNegLin(const double &CurrentTime, const double &SynchronizationTime, const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
Parameterization of the Step 2 velocity profile NegLinHldNegLinNegLin (leaf of the Step 2 decision tr...
Definition: TypeIIRMLStep2Profiles.cpp:812
TypeIIRMLMath::Decision_2___008
bool Decision_2___008(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, const double &CurrentTime, const double &SynchronizationTime)
If v->hold->vtrgt, so that t=tsync, is p<=ptrgt?
Definition: TypeIIRMLDecisions.cpp:394
TypeIIRMLMath::Decision_2___003
bool Decision_2___003(const double &CurrentVelocity, const double &TargetVelocity)
Is (vi <= vtrgt)?
Definition: TypeIIRMLDecisions.cpp:319
TypeIIRMLStep2IntermediateProfiles.h
Header file for intermediate profile segments of Step 2.
TypeIIRMLMath::Decision_2___011
bool Decision_2___011(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, const double &CurrentTime, const double &SynchronizationTime)
If v->vtrgt->hold, so that t=tsync, is p<=ptrgt?
Definition: TypeIIRMLDecisions.cpp:447
TypeIIRMLMath::Decision_2___005
bool Decision_2___005(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, const double &CurrentTime, const double &SynchronizationTime)
If v->hold->vtrgt, so that t=tsync, is p<=ptrgt?
Definition: TypeIIRMLDecisions.cpp:348
TypeIIRMLMath::Decision_2___004
bool Decision_2___004(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, const double &CurrentTime, const double &SynchronizationTime)
If v->vtrgt->hold, so that t=tsync, is p<=ptrgt?
Definition: TypeIIRMLDecisions.cpp:330
TypeIIRMLMath::ProfileStep2PosLinHldPosLin
void ProfileStep2PosLinHldPosLin(const double &CurrentTime, const double &SynchronizationTime, const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted)
Parameterization of the Step 2 velocity profile PosLinHldNegLin (leaf of the Step 2 decision tree) ...
Definition: TypeIIRMLStep2Profiles.cpp:205

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