TypeIIRMLStep2Profiles.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 <TypeIIRMLStep2Profiles.h>
46 #include <TypeIIRMLPolynomial.h>
47 #include <TypeIIRMLMath.h>
48 #include <math.h>
49 
50 
51 //************************************************************************************
52 // ProfileStep2PosLinHldNegLin()
53 
54 void TypeIIRMLMath::ProfileStep2PosLinHldNegLin( const double &CurrentTime
55  , const double &SynchronizationTime
56  , const double &CurrentPosition
57  , const double &CurrentVelocity
58  , const double &TargetPosition
59  , const double &TargetVelocity
60  , const double &MaxAcceleration
61  , MotionPolynomials *PolynomialsLocal
62  , const bool &Inverted )
63 {
64  double HoldVelocity = 0.0
65  , TimeForCurrentStep = 0.0
66  , ThisCurrentPosition = CurrentPosition
67  , ThisCurrentVelocity = CurrentVelocity
68  , TimeDifference = SynchronizationTime - CurrentTime
69  , ThisCurrentTime = CurrentTime;
70 
71  HoldVelocity = 0.5 * (TimeDifference * MaxAcceleration
72  + ThisCurrentVelocity + TargetVelocity
73  - RMLSqrt(pow2(MaxAcceleration)
74  * pow2(TimeDifference) - pow2(ThisCurrentVelocity
75  - TargetVelocity) + 2.0 * MaxAcceleration * (2.0
76  * (ThisCurrentPosition - TargetPosition)
77  + TimeDifference * (ThisCurrentVelocity
78  + TargetVelocity))) );
79 
80  if (HoldVelocity < ThisCurrentVelocity)
81  {
82  HoldVelocity = ThisCurrentVelocity;
83  }
84 
85  // ********************************************************************
86  // Compute polynomial parameters for the time interval t01 (PosLin)
87 
88  TimeForCurrentStep = (HoldVelocity - ThisCurrentVelocity) / MaxAcceleration;
89 
90  if(Inverted)
91  {
92  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
93  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (-ThisCurrentVelocity), ThisCurrentTime);
94  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
95  }
96  else
97  {
98  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
99  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , ThisCurrentVelocity, ThisCurrentTime);
100  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
101  }
102 
103  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
104  PolynomialsLocal->ValidPolynomials++;
105 
106  ThisCurrentPosition += 0.5 * (ThisCurrentVelocity + HoldVelocity)
107  * TimeForCurrentStep;
108  ThisCurrentVelocity = HoldVelocity;
109  ThisCurrentTime += TimeForCurrentStep;
110 
111  // ********************************************************************
112  // Compute polynomial parameters for the time interval t12 (Hld)
113 
114  if (HoldVelocity > TargetVelocity)
115  {
116  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime
117  - (HoldVelocity - TargetVelocity)
118  / MaxAcceleration;
119  }
120  else
121  {
122  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime;
123  }
124 
125  if(Inverted)
126  {
127  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
128  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
129  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
130  }
131  else
132  {
133  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
134  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
135  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
136  }
137 
138  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
139  PolynomialsLocal->ValidPolynomials++;
140 
141  ThisCurrentPosition += ThisCurrentVelocity * TimeForCurrentStep;
142  ThisCurrentTime += TimeForCurrentStep;
143 
144  // ********************************************************************
145  // Compute polynomial parameters for the time interval t23 (NegLin)
146 
147  if (ThisCurrentVelocity > TargetVelocity)
148  {
149  TimeForCurrentStep = (ThisCurrentVelocity - TargetVelocity)
150  / MaxAcceleration;
151  }
152  else
153  {
154  TimeForCurrentStep = 0.0;
155  }
156 
157  if(Inverted)
158  {
159  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
160  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
161  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
162  }
163  else
164  {
165  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
166  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
167  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
168  }
169 
170  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
171  PolynomialsLocal->ValidPolynomials++;
172 
173  ThisCurrentPosition += 0.5 * (TargetVelocity + ThisCurrentVelocity)
174  * TimeForCurrentStep;
175  ThisCurrentVelocity -= TimeForCurrentStep * MaxAcceleration;
176  ThisCurrentTime += TimeForCurrentStep;
177 
178  // ********************************************************************
179  // Compute polynomial parameters for the time, after ptrgt and vtrgt
180  // have been reached.
181 
182  if(Inverted)
183  {
184  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
185  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
186  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
187  }
188  else
189  {
190  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
191  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
192  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
193  }
194 
195  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + RML_INFINITY;
196  PolynomialsLocal->ValidPolynomials++;
197 
198  return;
199 }
200 
201 
202 //************************************************************************************
203 // ProfileStep2PosLinHldPosLin()
204 
205 void TypeIIRMLMath::ProfileStep2PosLinHldPosLin( const double &CurrentTime
206  , const double &SynchronizationTime
207  , const double &CurrentPosition
208  , const double &CurrentVelocity
209  , const double &TargetPosition
210  , const double &TargetVelocity
211  , const double &MaxAcceleration
212  , MotionPolynomials *PolynomialsLocal
213  , const bool &Inverted )
214 {
215  double HoldVelocity = 0.0
216  , TimeForCurrentStep = 0.0
217  , ThisCurrentPosition = CurrentPosition
218  , ThisCurrentVelocity = CurrentVelocity
219  , TimeDifference = SynchronizationTime - CurrentTime
220  , ThisCurrentTime = CurrentTime;
221 
222  HoldVelocity = (0.5 * (2.0 * MaxAcceleration * (TargetPosition
223  - CurrentPosition) + pow2(CurrentVelocity)
224  - pow2(TargetVelocity))) / (MaxAcceleration
225  * TimeDifference + CurrentVelocity
226  - TargetVelocity);
227 
228  if (HoldVelocity < ThisCurrentVelocity)
229  {
230  HoldVelocity = ThisCurrentVelocity;
231  }
232 
233  // ********************************************************************
234  // Compute polynomial parameters for the time interval t01 (PosLin)
235 
236  TimeForCurrentStep = (HoldVelocity - ThisCurrentVelocity) / MaxAcceleration;
237 
238  if(Inverted)
239  {
240  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
241  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (-ThisCurrentVelocity), ThisCurrentTime);
242  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
243  }
244  else
245  {
246  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
247  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , ThisCurrentVelocity, ThisCurrentTime);
248  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
249  }
250 
251  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
252  PolynomialsLocal->ValidPolynomials++;
253 
254  ThisCurrentPosition += 0.5 * (ThisCurrentVelocity + HoldVelocity)
255  * TimeForCurrentStep;
256  ThisCurrentVelocity = HoldVelocity;
257  ThisCurrentTime += TimeForCurrentStep;
258 
259  // ********************************************************************
260  // Compute polynomial parameters for the time interval t12 (Hld)
261 
262  if (TargetVelocity > HoldVelocity)
263  {
264  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime
265  - (TargetVelocity - HoldVelocity)
266  / MaxAcceleration;
267  }
268  else
269  {
270  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime;
271  }
272 
273  if(Inverted)
274  {
275  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
276  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
277  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
278  }
279  else
280  {
281  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
282  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
283  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
284  }
285 
286  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
287  PolynomialsLocal->ValidPolynomials++;
288 
289  ThisCurrentPosition += ThisCurrentVelocity * TimeForCurrentStep;
290  ThisCurrentTime += TimeForCurrentStep;
291 
292  // ********************************************************************
293  // Compute polynomial parameters for the time interval t23 (PosLin)
294 
295  if (TargetVelocity > ThisCurrentVelocity)
296  {
297  TimeForCurrentStep = (TargetVelocity - ThisCurrentVelocity)
298  / MaxAcceleration;
299  }
300  else
301  {
302  TimeForCurrentStep = 0.0;
303  }
304 
305  if(Inverted)
306  {
307  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
308  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (-ThisCurrentVelocity), ThisCurrentTime);
309  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
310  }
311  else
312  {
313  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
314  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , ThisCurrentVelocity, ThisCurrentTime);
315  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
316  }
317 
318  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
319  PolynomialsLocal->ValidPolynomials++;
320 
321  ThisCurrentPosition += 0.5 * (TargetVelocity + ThisCurrentVelocity)
322  * TimeForCurrentStep;
323  ThisCurrentVelocity += TimeForCurrentStep * MaxAcceleration;
324  ThisCurrentTime += TimeForCurrentStep;
325 
326  // ********************************************************************
327  // Compute polynomial parameters for the time, after ptrgt and vtrgt
328  // have been reached.
329 
330  if(Inverted)
331  {
332  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
333  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
334  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
335  }
336  else
337  {
338  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
339  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
340  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
341  }
342 
343  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + RML_INFINITY;
344  PolynomialsLocal->ValidPolynomials++;
345 
346  return;
347 }
348 
349 
350 //************************************************************************************
351 // ProfileStep2NegLinHldPosLin()
352 
353 void TypeIIRMLMath::ProfileStep2NegLinHldPosLin( const double &CurrentTime
354  , const double &SynchronizationTime
355  , const double &CurrentPosition
356  , const double &CurrentVelocity
357  , const double &TargetPosition
358  , const double &TargetVelocity
359  , const double &MaxAcceleration
360  , MotionPolynomials *PolynomialsLocal
361  , const bool &Inverted )
362 {
363  double HoldVelocity = 0.0
364  , TimeForCurrentStep = 0.0
365  , ThisCurrentPosition = CurrentPosition
366  , ThisCurrentVelocity = CurrentVelocity
367  , TimeDifference = SynchronizationTime - CurrentTime
368  , ThisCurrentTime = CurrentTime;
369 
370  HoldVelocity = 0.5 * (CurrentVelocity + TargetVelocity
371  - TimeDifference * MaxAcceleration
372  + RMLSqrt(pow2(MaxAcceleration)
373  * pow2(TimeDifference) - pow2(CurrentVelocity
374  - TargetVelocity) - 2.0 * MaxAcceleration * (2.0
375  * (CurrentPosition - TargetPosition)
376  + TimeDifference * (CurrentVelocity
377  + TargetVelocity))));
378 
379  if (HoldVelocity > ThisCurrentVelocity)
380  {
381  HoldVelocity = ThisCurrentVelocity;
382  }
383 
384  // ********************************************************************
385  // Compute polynomial parameters for the time interval t01 (NegLin)
386 
387  TimeForCurrentStep = (ThisCurrentVelocity - HoldVelocity) / MaxAcceleration;
388 
389  if(Inverted)
390  {
391  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
392  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
393  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
394  }
395  else
396  {
397  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
398  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
399  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
400  }
401 
402  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
403  PolynomialsLocal->ValidPolynomials++;
404 
405  ThisCurrentPosition += 0.5 * (ThisCurrentVelocity + HoldVelocity)
406  * TimeForCurrentStep;
407  ThisCurrentVelocity = HoldVelocity;
408  ThisCurrentTime += TimeForCurrentStep;
409 
410  // ********************************************************************
411  // Compute polynomial parameters for the time interval t12 (Hld)
412 
413  if (TargetVelocity > HoldVelocity)
414  {
415  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime
416  - (TargetVelocity - HoldVelocity)
417  / MaxAcceleration;
418  }
419  else
420  {
421  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime;
422  }
423 
424  if(Inverted)
425  {
426  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
427  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
428  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
429  }
430  else
431  {
432  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
433  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
434  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
435  }
436 
437  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
438  PolynomialsLocal->ValidPolynomials++;
439 
440  ThisCurrentPosition += ThisCurrentVelocity * TimeForCurrentStep;
441  ThisCurrentTime += TimeForCurrentStep;
442 
443  // ********************************************************************
444  // Compute polynomial parameters for the time interval t23 (PosLin)
445 
446  if (TargetVelocity > ThisCurrentVelocity)
447  {
448  TimeForCurrentStep = (TargetVelocity - ThisCurrentVelocity)
449  / MaxAcceleration;
450  }
451  else
452  {
453  TimeForCurrentStep = 0.0;
454  }
455 
456  if(Inverted)
457  {
458  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
459  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (-ThisCurrentVelocity), ThisCurrentTime);
460  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
461  }
462  else
463  {
464  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
465  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , ThisCurrentVelocity, ThisCurrentTime);
466  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
467  }
468 
469  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
470  PolynomialsLocal->ValidPolynomials++;
471 
472  ThisCurrentPosition += 0.5 * (TargetVelocity + ThisCurrentVelocity)
473  * TimeForCurrentStep;
474  ThisCurrentVelocity += TimeForCurrentStep * MaxAcceleration;
475  ThisCurrentTime += TimeForCurrentStep;
476 
477  // ********************************************************************
478  // Compute polynomial parameters for the time, after ptrgt and vtrgt
479  // have been reached.
480 
481  if(Inverted)
482  {
483  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
484  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
485  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
486  }
487  else
488  {
489  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
490  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
491  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
492  }
493 
494  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + RML_INFINITY;
495  PolynomialsLocal->ValidPolynomials++;
496 
497  return;
498 }
499 
500 
501 //************************************************************************************
502 // ProfileStep2NegLinHldNegLin()
503 
504 void TypeIIRMLMath::ProfileStep2NegLinHldNegLin( const double &CurrentTime
505  , const double &SynchronizationTime
506  , const double &CurrentPosition
507  , const double &CurrentVelocity
508  , const double &TargetPosition
509  , const double &TargetVelocity
510  , const double &MaxAcceleration
511  , MotionPolynomials *PolynomialsLocal
512  , const bool &Inverted )
513 {
514  double HoldVelocity = 0.0
515  , TimeForCurrentStep = 0.0
516  , ThisCurrentPosition = CurrentPosition
517  , ThisCurrentVelocity = CurrentVelocity
518  , TimeDifference = SynchronizationTime - CurrentTime
519  , ThisCurrentTime = CurrentTime;
520 
521  HoldVelocity = (0.5 * (2.0 * MaxAcceleration * (TargetPosition
522  - CurrentPosition) - pow2(CurrentVelocity)
523  + pow2(TargetVelocity))) / (MaxAcceleration
524  * TimeDifference - CurrentVelocity
525  + TargetVelocity);
526 
527  if (HoldVelocity > ThisCurrentVelocity)
528  {
529  HoldVelocity = ThisCurrentVelocity;
530  }
531 
532  // ********************************************************************
533  // Compute polynomial parameters for the time interval t01 (NegLin)
534 
535  TimeForCurrentStep = (ThisCurrentVelocity - HoldVelocity) / MaxAcceleration;
536 
537  if(Inverted)
538  {
539  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
540  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
541  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
542  }
543  else
544  {
545  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
546  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
547  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
548  }
549 
550  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
551  PolynomialsLocal->ValidPolynomials++;
552 
553  ThisCurrentPosition += 0.5 * (ThisCurrentVelocity + HoldVelocity)
554  * TimeForCurrentStep;
555  ThisCurrentVelocity = HoldVelocity;
556  ThisCurrentTime += TimeForCurrentStep;
557 
558  // ********************************************************************
559  // Compute polynomial parameters for the time interval t12 (Hld)
560 
561  if (HoldVelocity > TargetVelocity)
562  {
563  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime
564  - (HoldVelocity - TargetVelocity)
565  / MaxAcceleration;
566  }
567  else
568  {
569  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime;
570  }
571 
572  if(Inverted)
573  {
574  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
575  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
576  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
577  }
578  else
579  {
580  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
581  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
582  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
583  }
584 
585  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
586  PolynomialsLocal->ValidPolynomials++;
587 
588  ThisCurrentPosition += ThisCurrentVelocity * TimeForCurrentStep;
589  ThisCurrentTime += TimeForCurrentStep;
590 
591  // ********************************************************************
592  // Compute polynomial parameters for the time interval t23 (NegLin)
593 
594  if (ThisCurrentVelocity > TargetVelocity)
595  {
596  TimeForCurrentStep = (ThisCurrentVelocity - TargetVelocity)
597  / MaxAcceleration;
598  }
599  else
600  {
601  TimeForCurrentStep = 0.0;
602  }
603 
604  if(Inverted)
605  {
606  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
607  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
608  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
609  }
610  else
611  {
612  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
613  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
614  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
615  }
616 
617  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
618  PolynomialsLocal->ValidPolynomials++;
619 
620  ThisCurrentPosition += 0.5 * (TargetVelocity + ThisCurrentVelocity)
621  * TimeForCurrentStep;
622  ThisCurrentVelocity -= TimeForCurrentStep * MaxAcceleration;
623  ThisCurrentTime += TimeForCurrentStep;
624 
625  // ********************************************************************
626  // Compute polynomial parameters for the time, after ptrgt and vtrgt
627  // have been reached.
628 
629  if(Inverted)
630  {
631  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
632  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
633  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
634  }
635  else
636  {
637  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
638  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
639  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
640  }
641 
642  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + RML_INFINITY;
643  PolynomialsLocal->ValidPolynomials++;
644 
645  return;
646 }
647 
648 
649 //************************************************************************************
650 // ProfileStep2PosTrapNegLin()
651 
652 void TypeIIRMLMath::ProfileStep2PosTrapNegLin( const double &CurrentTime
653  , const double &SynchronizationTime
654  , const double &CurrentPosition
655  , const double &CurrentVelocity
656  , const double &TargetPosition
657  , const double &TargetVelocity
658  , const double &MaxAcceleration
659  , MotionPolynomials *PolynomialsLocal
660  , const bool &Inverted )
661 {
662  double HoldVelocity = 0.0
663  , TimeForCurrentStep = 0.0
664  , ThisCurrentPosition = CurrentPosition
665  , ThisCurrentVelocity = CurrentVelocity
666  , TimeDifference = SynchronizationTime - CurrentTime
667  , ThisCurrentTime = CurrentTime;
668 
669  HoldVelocity = 0.5 * (MaxAcceleration * TimeDifference
670  + CurrentVelocity + TargetVelocity
671  - RMLSqrt(pow2(MaxAcceleration)
672  * pow2(TimeDifference) - pow2(CurrentVelocity
673  - TargetVelocity)+ 2.0 * MaxAcceleration * (2.0
674  * (CurrentPosition - TargetPosition)
675  + TimeDifference * (CurrentVelocity
676  + TargetVelocity))));
677 
678  if (HoldVelocity < ThisCurrentVelocity)
679  {
680  HoldVelocity = ThisCurrentVelocity;
681  }
682 
683  // ********************************************************************
684  // Compute polynomial parameters for the time interval t01 (PosLin)
685 
686  TimeForCurrentStep = (HoldVelocity - ThisCurrentVelocity) / MaxAcceleration;
687 
688  if(Inverted)
689  {
690  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
691  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (-ThisCurrentVelocity), ThisCurrentTime);
692  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
693  }
694  else
695  {
696  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
697  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , ThisCurrentVelocity, ThisCurrentTime);
698  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
699  }
700 
701  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
702  PolynomialsLocal->ValidPolynomials++;
703 
704  ThisCurrentPosition += 0.5 * (ThisCurrentVelocity + HoldVelocity)
705  * TimeForCurrentStep;
706  ThisCurrentVelocity = HoldVelocity;
707  ThisCurrentTime += TimeForCurrentStep;
708 
709  // ********************************************************************
710  // Compute polynomial parameters for the time interval t12 (Hld)
711 
712  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime
713  - (HoldVelocity - TargetVelocity)
714  / MaxAcceleration;
715 
716  if(Inverted)
717  {
718  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
719  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
720  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
721  }
722  else
723  {
724  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
725  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
726  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
727  }
728 
729  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
730  PolynomialsLocal->ValidPolynomials++;
731 
732  ThisCurrentPosition += ThisCurrentVelocity * TimeForCurrentStep;
733  ThisCurrentTime += TimeForCurrentStep;
734 
735  // ********************************************************************
736  // Compute polynomial parameters for the time interval t23 (NegLin)
737 
738  TimeForCurrentStep = ThisCurrentVelocity / MaxAcceleration;
739 
740  if(Inverted)
741  {
742  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
743  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
744  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
745  }
746  else
747  {
748  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
749  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
750  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
751  }
752 
753  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
754  PolynomialsLocal->ValidPolynomials++;
755 
756  ThisCurrentPosition += 0.5 * ThisCurrentVelocity * TimeForCurrentStep;
757  ThisCurrentVelocity -= TimeForCurrentStep * MaxAcceleration;
758  ThisCurrentTime += TimeForCurrentStep;
759 
760  // ********************************************************************
761  // Compute polynomial parameters for the time interval t34 (NegLin)
762 
763  TimeForCurrentStep = -TargetVelocity / MaxAcceleration;
764 
765  if(Inverted)
766  {
767  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
768  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
769  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
770  }
771  else
772  {
773  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
774  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
775  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
776  }
777 
778  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
779  PolynomialsLocal->ValidPolynomials++;
780 
781  ThisCurrentPosition += 0.5 * TargetVelocity * TimeForCurrentStep;
782  ThisCurrentVelocity -= TimeForCurrentStep * MaxAcceleration;
783  ThisCurrentTime += TimeForCurrentStep;
784 
785  // ********************************************************************
786  // Compute polynomial parameters for the time, after ptrgt and vtrgt
787  // have been reached.
788 
789  if(Inverted)
790  {
791  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
792  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
793  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
794  }
795  else
796  {
797  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
798  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
799  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
800  }
801 
802  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + RML_INFINITY;
803  PolynomialsLocal->ValidPolynomials++;
804 
805  return;
806 }
807 
808 
809 //************************************************************************************
810 // ProfileStep2NegLinHldNegLinNegLin()
811 
812 void TypeIIRMLMath::ProfileStep2NegLinHldNegLinNegLin( const double &CurrentTime
813  , const double &SynchronizationTime
814  , const double &CurrentPosition
815  , const double &CurrentVelocity
816  , const double &TargetPosition
817  , const double &TargetVelocity
818  , const double &MaxAcceleration
819  , MotionPolynomials *PolynomialsLocal
820  , const bool &Inverted )
821 {
822  double HoldVelocity = 0.0
823  , TimeForCurrentStep = 0.0
824  , ThisCurrentPosition = CurrentPosition
825  , ThisCurrentVelocity = CurrentVelocity
826  , TimeDifference = SynchronizationTime - CurrentTime
827  , ThisCurrentTime = CurrentTime;
828 
829  HoldVelocity = (0.5 * (2.0 * MaxAcceleration * (TargetPosition
830  - CurrentPosition) - pow2(CurrentVelocity)
831  + pow2(TargetVelocity))) / (MaxAcceleration
832  * TimeDifference - CurrentVelocity
833  + TargetVelocity);
834 
835  if (HoldVelocity > ThisCurrentVelocity)
836  {
837  HoldVelocity = ThisCurrentVelocity;
838  }
839 
840  if (HoldVelocity < 0.0)
841  {
842  HoldVelocity = 0.0;
843  }
844 
845  // ********************************************************************
846  // Compute polynomial parameters for the time interval t01 (NegLin)
847 
848  TimeForCurrentStep = (ThisCurrentVelocity - HoldVelocity) / MaxAcceleration;
849 
850  if(Inverted)
851  {
852  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
853  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
854  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
855  }
856  else
857  {
858  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
859  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
860  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
861  }
862 
863  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
864  PolynomialsLocal->ValidPolynomials++;
865 
866  ThisCurrentPosition += 0.5 * (ThisCurrentVelocity + HoldVelocity)
867  * TimeForCurrentStep;
868  ThisCurrentVelocity = HoldVelocity;
869  ThisCurrentTime += TimeForCurrentStep;
870 
871  // ********************************************************************
872  // Compute polynomial parameters for the time interval t12 (Hld)
873 
874  TimeForCurrentStep = SynchronizationTime - ThisCurrentTime
875  - (HoldVelocity - TargetVelocity)
876  / MaxAcceleration;
877 
878  if(Inverted)
879  {
880  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
881  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
882  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
883  }
884  else
885  {
886  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
887  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
888  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
889  }
890 
891  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
892  PolynomialsLocal->ValidPolynomials++;
893 
894  ThisCurrentPosition += ThisCurrentVelocity * TimeForCurrentStep;
895  ThisCurrentTime += TimeForCurrentStep;
896 
897  // ********************************************************************
898  // Compute polynomial parameters for the time interval t23 (NegLin)
899 
900  TimeForCurrentStep = ThisCurrentVelocity / MaxAcceleration;
901 
902  if(Inverted)
903  {
904  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
905  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
906  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
907  }
908  else
909  {
910  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
911  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
912  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
913  }
914 
915  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
916  PolynomialsLocal->ValidPolynomials++;
917 
918  ThisCurrentPosition += 0.5 * ThisCurrentVelocity * TimeForCurrentStep;
919  ThisCurrentVelocity -= TimeForCurrentStep * MaxAcceleration;
920  ThisCurrentTime += TimeForCurrentStep;
921 
922  // ********************************************************************
923  // Compute polynomial parameters for the time interval t34 (NegLin)
924 
925  TimeForCurrentStep = -TargetVelocity / MaxAcceleration;
926 
927  if(Inverted)
928  {
929  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
930  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-ThisCurrentVelocity), ThisCurrentTime);
931  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, ThisCurrentTime);
932  }
933  else
934  {
935  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
936  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , ThisCurrentVelocity, ThisCurrentTime);
937  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), ThisCurrentTime);
938  }
939 
940  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + TimeForCurrentStep;
941  PolynomialsLocal->ValidPolynomials++;
942 
943  ThisCurrentPosition += 0.5 * TargetVelocity * TimeForCurrentStep;
944  ThisCurrentVelocity -= TimeForCurrentStep * MaxAcceleration;
945  ThisCurrentTime += TimeForCurrentStep;
946 
947  // ********************************************************************
948  // Compute polynomial parameters for the time, after ptrgt and vtrgt
949  // have been reached.
950 
951  if(Inverted)
952  {
953  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-ThisCurrentVelocity), (-ThisCurrentPosition), ThisCurrentTime);
954  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-ThisCurrentVelocity), ThisCurrentTime);
955  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
956  }
957  else
958  {
959  PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, ThisCurrentVelocity, ThisCurrentPosition, ThisCurrentTime);
960  PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , ThisCurrentVelocity, ThisCurrentTime);
961  PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , 0.0, ThisCurrentTime);
962  }
963 
964  PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = ThisCurrentTime + RML_INFINITY;
965  PolynomialsLocal->ValidPolynomials++;
966 
967  return;
968 }
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::MotionPolynomials::PolynomialTimes
double PolynomialTimes[MAXIMAL_NO_OF_POLYNOMIALS]
An array of ending times in seconds.
Definition: TypeIIRMLPolynomial.h:273
TypeIIRMLMath::MotionPolynomials::AccelerationPolynomial
TypeIIRMLPolynomial AccelerationPolynomial[MAXIMAL_NO_OF_POLYNOMIALS]
An array of acceleration polynomials.
Definition: TypeIIRMLPolynomial.h:318
TypeIIRMLStep2Profiles.h
Header file for the Step 2 motion profiles.
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.h
Header file for functions and definitions of constant values and macros.
RML_INFINITY
#define RML_INFINITY
A value for infinity .
Definition: TypeIIRMLMath.h:201
TypeIIRMLMath::RMLSqrt
double RMLSqrt(const double &Value)
Calculates the real square root of a given value. If the value is negative a value of almost zero wil...
Definition: TypeIIRMLMath.h:291
TypeIIRMLPolynomial.h
Header file for the class TypeIIRMLMath::TypeIIRMLPolynomial and the struct TypeIIRMLMath::MotionPoly...
TypeIIRMLMath::MotionPolynomials::VelocityPolynomial
TypeIIRMLPolynomial VelocityPolynomial[MAXIMAL_NO_OF_POLYNOMIALS]
An array of velocity polynomials.
Definition: TypeIIRMLPolynomial.h:303
pow2
#define pow2(A)
A to the power of 2.
Definition: TypeIIRMLMath.h:273
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::TypeIIRMLPolynomial::SetCoefficients
void SetCoefficients(const double &Coeff2, const double &Coeff1, const double &Coeff0, const double &Diff)
Sets the coefficients of the polynomial object.
Definition: TypeIIRMLPolynomial.cpp:74
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
TypeIIRMLMath::MotionPolynomials::ValidPolynomials
unsigned char ValidPolynomials
The number of polynomials in use (0 ... MAXIMAL_NO_OF_POLYNOMIALS)
Definition: TypeIIRMLPolynomial.h:327
TypeIIRMLMath::MotionPolynomials::PositionPolynomial
TypeIIRMLPolynomial PositionPolynomial[MAXIMAL_NO_OF_POLYNOMIALS]
An array of position polynomials.
Definition: TypeIIRMLPolynomial.h:288

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