Robotics Library  0.7.0
TrapezoidalVelocity.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 //
14 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
18 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24 // POSSIBILITY OF SUCH DAMAGE.
25 //
26 
27 #ifndef RL_MATH_TRAPEZOIDALVELOCITY_H
28 #define RL_MATH_TRAPEZOIDALVELOCITY_H
29 
30 #include <cassert>
31 #include <cmath>
32 #include <limits>
33 
34 #include "Real.h"
35 
36 namespace rl
37 {
38  namespace math
39  {
40  template<typename T>
41  class TrapezoidalVelocity
42  {
43  public:
44  TrapezoidalVelocity() :
45  am(0),
46  dm(0),
47  x0(0),
48  xe(0),
49  v0(0),
50  ve(0),
51  vm(0),
52  ah(0),
53  dh(0),
54  ta(::std::numeric_limits<Real>::max()),
55  tc(::std::numeric_limits<Real>::max()),
56  td(::std::numeric_limits<Real>::max())
57  {
58  }
59 
60  virtual ~TrapezoidalVelocity()
61  {
62  }
63 
64  T a(const Real& t) const
65  {
66  if (t < ta)
67  {
68  return ah;
69  }
70  else if (t < ta + tc)
71  {
72  return 0;
73  }
74  else if (t < ta + tc + td)
75  {
76  return -dh;
77  }
78  else
79  {
80  return 0;
81  }
82  }
83 
84  void interpolate()
85  {
86  T x = xe - x0;
87 
88  T ta1 = ( vm - v0) / am;
89  T ta2 = ( vm - v0) / am;
90  T ta3 = (-vm - v0) / am;
91  T ta4 = (-vm - v0) / am;
92  T ta5 = ( vm - v0) / -am;
93  T ta6 = ( vm - v0) / -am;
94  T ta7 = (-vm - v0) / -am;
95  T ta8 = (-vm - v0) / -am;
96 
97  T tc1 = ( dm * v0 * v0 - dm * vm * vm - am * vm * vm + am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
98  T tc2 = ( dm * v0 * v0 - dm * vm * vm + am * vm * vm - am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
99  T tc3 = -( dm * v0 * v0 - dm * vm * vm - am * vm * vm + am * ve * ve + 2 * x * am * dm) / vm / dm / am / 2;
100  T tc4 = -( dm * v0 * v0 - dm * vm * vm + am * vm * vm - am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
101  T tc5 = (-dm * v0 * v0 + dm * vm * vm - am * vm * vm + am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
102  T tc6 = (-dm * v0 * v0 + dm * vm * vm + am * vm * vm - am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
103  T tc7 = -(-dm * v0 * v0 + dm * vm * vm - am * vm * vm + am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
104  T tc8 = -(-dm * v0 * v0 + dm * vm * vm + am * vm * vm - am * ve * ve + 2 * x * am * dm) / vm / am / dm / 2;
105 
106  T td1 = (ve - vm) / -dm;
107  T td2 = (ve - vm) / dm;
108  T td3 = (ve + vm) / -dm;
109  T td4 = (ve + vm) / dm;
110  T td5 = (ve - vm) / -dm;
111  T td6 = (ve - vm) / dm;
112  T td7 = (ve + vm) / -dm;
113  T td8 = (ve + vm) / dm;
114 
115  T t1 = ta1 + tc1 + td1;
116  T t2 = ta2 + tc2 + td2;
117  T t3 = ta3 + tc3 + td3;
118  T t4 = ta4 + tc4 + td4;
119  T t5 = ta5 + tc5 + td5;
120  T t6 = ta6 + tc6 + td6;
121  T t7 = ta7 + tc7 + td7;
122  T t8 = ta8 + tc8 + td8;
123 
124  T t = ::std::numeric_limits<T>::max();
125 
126  T vh = vm;
127 
128  // am, vm, -dm
129  if (ta1 >= 0 && tc1 >= 0 && td1 >= 0 && ::std::abs(t1) < ::std::abs(t))
130  {
131  t = t1;
132  ta = ta1;
133  tc = tc1;
134  td = td1;
135  ah = am;
136  vh = vm;
137  dh = dm;
138  }
139  // am, vm, dm
140  if (ta2 >= 0 && tc2 >= 0 && td2 >= 0 && ::std::abs(t2) < ::std::abs(t))
141  {
142  t = t2;
143  ta = ta2;
144  tc = tc2;
145  td = td2;
146  ah = am;
147  vh = vm;
148  dh = -dm;
149  }
150  // am, -vm, -dm
151  if (ta3 >= 0 && tc3 >= 0 && td3 >= 0 && ::std::abs(t3) < ::std::abs(t))
152  {
153  t = t3;
154  ta = ta3;
155  tc = tc3;
156  td = td3;
157  ah = am;
158  vh = -vm;
159  dh = dm;
160  }
161  // am, -vm, dm
162  if (ta4 >= 0 && tc4 >= 0 && td4 >= 0 && ::std::abs(t4) < ::std::abs(t))
163  {
164  t = t4;
165  ta = ta4;
166  tc = tc4;
167  td = td4;
168  ah = am;
169  vh = -vm;
170  dh = -dm;
171  }
172  // -am, vm, -dm
173  if (ta5 >= 0 && tc5 >= 0 && td5 >= 0 && ::std::abs(t5) < ::std::abs(t))
174  {
175  t = t5;
176  ta = ta5;
177  tc = tc5;
178  td = td5;
179  ah = -am;
180  vh = vm;
181  dh = dm;
182  }
183  // -am, vm, dm
184  if (ta6 >= 0 && tc6 >= 0 && td6 >= 0 && ::std::abs(t6) < ::std::abs(t))
185  {
186  t = t6;
187  ta = ta6;
188  tc = tc6;
189  td = td6;
190  ah = -am;
191  vh = vm;
192  dh = -dm;
193  }
194  // -am, -vm, -dm
195  if (ta7 >= 0 && tc7 >= 0 && td7 >= 0 && ::std::abs(t7) < ::std::abs(t))
196  {
197  t = t7;
198  ta = ta7;
199  tc = tc7;
200  td = td7;
201  ah = -am;
202  vh = -vm;
203  dh = dm;
204  }
205  // -am, -vm, dm
206  if (ta8 >= 0 && tc8 >= 0 && td8 >= 0 && ::std::abs(t8) < ::std::abs(t))
207  {
208  t = t8;
209  ta = ta8;
210  tc = tc8;
211  td = td8;
212  ah = -am;
213  vh = -vm;
214  dh = -dm;
215  }
216 
217  T v1 = 1 / ( dm + am) * ::std::sqrt( ( dm + am) * ( dm * v0 * v0 + am * ve * ve + 2 * x * am * dm));
218  T v2 = 1 / (-dm + am) * ::std::sqrt(-(-dm + am) * ( dm * v0 * v0 - am * ve * ve + 2 * x * am * dm));
219  T v3 = 1 / ( dm + am) * ::std::sqrt( ( dm + am) * ( dm * v0 * v0 + am * ve * ve + 2 * x * am * dm));
220  T v4 = 1 / (-dm + am) * ::std::sqrt(-(-dm + am) * ( dm * v0 * v0 - am * ve * ve + 2 * x * am * dm));
221  T v5 = 1 / (-dm + am) * ::std::sqrt( (-dm + am) * (-dm * v0 * v0 + am * ve * ve + 2 * x * am * dm));
222  T v6 = 1 / ( dm + am) * ::std::sqrt(-( dm + am) * (-dm * v0 * v0 - am * ve * ve + 2 * x * am * dm));
223  T v7 = 1 / (-dm + am) * ::std::sqrt( (-dm + am) * (-dm * v0 * v0 + am * ve * ve + 2 * x * am * dm));
224  T v8 = 1 / ( dm + am) * ::std::sqrt(-( dm + am) * (-dm * v0 * v0 - am * ve * ve + 2 * x * am * dm));
225 
226  T ta1a = ( v1 - v0) / am;
227  T ta1b = (-v1 - v0) / am;
228  T ta2a = ( v2 - v0) / am;
229  T ta2b = (-v2 - v0) / am;
230  T ta3a = (-v3 - v0) / am;
231  T ta3b = ( v3 - v0) / am;
232  T ta4a = (-v4 - v0) / am;
233  T ta4b = ( v4 - v0) / am;
234  T ta5a = ( v5 - v0) / -am;
235  T ta5b = (-v5 - v0) / -am;
236  T ta6a = ( v6 - v0) / -am;
237  T ta6b = (-v6 - v0) / -am;
238  T ta7a = (-v7 - v0) / -am;
239  T ta7b = ( v7 - v0) / -am;
240  T ta8a = (-v8 - v0) / -am;
241  T ta8b = ( v8 - v0) / -am;
242 
243  T td1a = (ve - v1) / -dm;
244  T td1b = (ve + v1) / -dm;
245  T td2a = (ve - v2) / dm;
246  T td2b = (ve + v2) / dm;
247  T td3a = (ve + v3) / -dm;
248  T td3b = (ve - v3) / -dm;
249  T td4a = (ve + v4) / dm;
250  T td4b = (ve - v4) / dm;
251  T td5a = (ve - v5) / -dm;
252  T td5b = (ve + v5) / -dm;
253  T td6a = (ve - v6) / dm;
254  T td6b = (ve + v6) / dm;
255  T td7a = (ve + v7) / -dm;
256  T td7b = (ve - v7) / -dm;
257  T td8a = (ve + v8) / dm;
258  T td8b = (ve - v8) / dm;
259 
260  T t1a = ta1a + td1a;
261  T t1b = ta1b + td1b;
262  T t2a = ta2a + td2a;
263  T t2b = ta2b + td2b;
264  T t3a = ta3a + td3a;
265  T t3b = ta3b + td3b;
266  T t4a = ta4a + td4a;
267  T t4b = ta4b + td4b;
268  T t5a = ta5a + td5a;
269  T t5b = ta5b + td5b;
270  T t6a = ta6a + td6a;
271  T t6b = ta6b + td6b;
272  T t7a = ta7a + td7a;
273  T t7b = ta7b + td7b;
274  T t8a = ta8a + td8a;
275  T t8b = ta8b + td8b;
276 
277  // am, vm, -dm
278  if (ta1a >= 0 && td1a >= 0 && ::std::abs(t1a) < ::std::abs(t) && ::std::abs(v1) < ::std::abs(vh))
279  {
280  t = t1a;
281  ta = ta1a;
282  tc = 0;
283  td = td1a;
284  ah = am;
285  vh = v1;
286  dh = dm;
287  }
288  if (ta1b >= 0 && td1b >= 0 && ::std::abs(t1b) < ::std::abs(t) && ::std::abs(v1) < ::std::abs(vh))
289  {
290  t = t1b;
291  ta = ta1b;
292  tc = 0;
293  td = td1b;
294  ah = am;
295  vh = -v1;
296  dh = dm;
297  }
298  // am, vm, dm
299  if (ta2a >= 0 && td2a >= 0 && ::std::abs(t2a) < ::std::abs(t) && ::std::abs(v2) < ::std::abs(vh))
300  {
301  t = t2a;
302  ta = ta2a;
303  tc = 0;
304  td = td2a;
305  ah = am;
306  vh = v2;
307  dh = -dm;
308  }
309  if (ta2b >= 0 && td2 >= 0 && ::std::abs(t2b) < ::std::abs(t) && ::std::abs(v2) < ::std::abs(vh))
310  {
311  t = t2b;
312  ta = ta2b;
313  tc = 0;
314  td = td2b;
315  ah = am;
316  vh = -v2;
317  dh = -dm;
318  }
319  // am, -vm, -dm
320  if (ta3a >= 0 && td3a >= 0 && ::std::abs(t3a) < ::std::abs(t) && ::std::abs(v3) < ::std::abs(vh))
321  {
322  t = t3a;
323  ta = ta3a;
324  tc = 0;
325  td = td3a;
326  ah = am;
327  vh = -v3;
328  dh = dm;
329  }
330  if (ta3b >= 0 && td3b >= 0 && ::std::abs(t3b) < ::std::abs(t) && ::std::abs(v3) < ::std::abs(vh))
331  {
332  t = t3b;
333  ta = ta3b;
334  tc = 0;
335  td = td3b;
336  ah = am;
337  vh = v3;
338  dh = dm;
339  }
340  // am, -vm, dm
341  if (ta4a >= 0 && td4a >= 0 && ::std::abs(t4a) < ::std::abs(t) && ::std::abs(v4) < ::std::abs(vh))
342  {
343  t = t4a;
344  ta = ta4a;
345  tc = 0;
346  td = td4a;
347  ah = am;
348  vh = -v4;
349  dh = -dm;
350  }
351  if (ta4b >= 0 && td4b >= 0 && ::std::abs(t4b) < ::std::abs(t) && ::std::abs(v4) < ::std::abs(vh))
352  {
353  t = t4b;
354  ta = ta4b;
355  tc = 0;
356  td = td4b;
357  ah = am;
358  vh = v4;
359  dh = -dm;
360  }
361  // -am, vm, -dm
362  if (ta5a >= 0 && td5a >= 0 && ::std::abs(t5a) < ::std::abs(t) && ::std::abs(v5) < ::std::abs(vh))
363  {
364  t = t5a;
365  ta = ta5a;
366  tc = 0;
367  td = td5a;
368  ah = -am;
369  vh = v5;
370  dh = dm;
371  }
372  if (ta5b >= 0 && td5b >= 0 && ::std::abs(t5b) < ::std::abs(t) && ::std::abs(v5) < ::std::abs(vh))
373  {
374  t = t5b;
375  ta = ta5b;
376  tc = 0;
377  td = td5b;
378  ah = -am;
379  vh = -v5;
380  dh = dm;
381  }
382  // -am, vm, dm
383  if (ta6a >= 0 && td6a >= 0 && ::std::abs(t6a) < ::std::abs(t) && ::std::abs(v6) < ::std::abs(vh))
384  {
385  t = t6a;
386  ta = ta6a;
387  tc = 0;
388  td = td6a;
389  ah = -am;
390  vh = v6;
391  dh = -dm;
392  }
393  if (ta6b >= 0 && td6b >= 0 && ::std::abs(t6b) < ::std::abs(t) && ::std::abs(v6) < ::std::abs(vh))
394  {
395  t = t6b;
396  ta = ta6b;
397  tc = 0;
398  td = td6b;
399  ah = -am;
400  vh = -v6;
401  dh = -dm;
402  }
403  // -am, -vm, -dm
404  if (ta7a >= 0 && td7a >= 0 && ::std::abs(t7a) < ::std::abs(t) && ::std::abs(v7) < ::std::abs(vh))
405  {
406  t = t7a;
407  ta = ta7a;
408  tc = 0;
409  td = td7a;
410  ah = -am;
411  vh = -v7;
412  dh = dm;
413  }
414  if (ta7b >= 0 && td7b >= 0 && ::std::abs(t7b) < ::std::abs(t) && ::std::abs(v7) < ::std::abs(vh))
415  {
416  t = t7b;
417  ta = ta7b;
418  tc = 0;
419  td = td7b;
420  ah = -am;
421  vh = v7;
422  dh = dm;
423  }
424  // -am, -vm, dm
425  if (ta8a >= 0 && td8a >= 0 && ::std::abs(t8a) < ::std::abs(t) && ::std::abs(v8) < ::std::abs(vh))
426  {
427  t = t8a;
428  ta = ta8a;
429  tc = 0;
430  td = td8a;
431  ah = -am;
432  vh = -v8;
433  dh = -dm;
434  }
435  if (ta8b >= 0 && td8b >= 0 && ::std::abs(t8b) < ::std::abs(t) && ::std::abs(v8) < ::std::abs(vh))
436  {
437  t = t8b;
438  ta = ta8b;
439  tc = 0;
440  td = td8b;
441  ah = -am;
442  vh = v8;
443  dh = -dm;
444  }
445  }
446 
447  void interpolate(const Real& t)
448  {
449  T x = xe - x0;
450 
451  T v1a = (dm * v0 + am * ve + t * am * dm + ::std::sqrt( 2 * dm * v0 * am * ve + 2 * dm * dm * v0 * t * am + 2 * am * am * ve * t * dm + t * t * am * am * dm * dm - dm * am * ve * ve - 2 * x * am * dm * dm - am * dm * v0 * v0 - 2 * x * am * am * dm)) / ( dm + am);
452  T v1b = (dm * v0 + am * ve + t * am * dm - ::std::sqrt( 2 * dm * v0 * am * ve + 2 * dm * dm * v0 * t * am + 2 * am * am * ve * t * dm + t * t * am * am * dm * dm - dm * am * ve * ve - 2 * x * am * dm * dm - am * dm * v0 * v0 - 2 * x * am * am * dm)) / ( dm + am);
453  T v2a;
454  T v2b;
455  T v3a = -(dm * v0 + am * ve + t * am * dm - ::std::sqrt( 2 * dm * v0 * am * ve + 2 * dm * dm * v0 * t * am + 2 * am * am * ve * t * dm + t * t * am * am * dm * dm - am * dm * v0 * v0 - 2 * x * am * am * dm - dm * am * ve * ve - 2 * x * am * dm * dm)) / ( am + dm);
456  T v3b = -(dm * v0 + am * ve + t * am * dm + ::std::sqrt( 2 * dm * v0 * am * ve + 2 * dm * dm * v0 * t * am + 2 * am * am * ve * t * dm + t * t * am * am * dm * dm - am * dm * v0 * v0 - 2 * x * am * am * dm - dm * am * ve * ve - 2 * x * am * dm * dm)) / ( am + dm);
457  T v4a;
458  T v4b;
459  T v5a;
460  T v5b;
461  T v6a = (dm * v0 + am * ve - t * am * dm + ::std::sqrt( 2 * dm * v0 * am * ve - 2 * dm * dm * v0 * t * am - 2 * am * am * ve * t * dm + t * t * am * am * dm * dm - dm * am * ve * ve + 2 * x * am * dm * dm - am * dm * v0 * v0 + 2 * x * am * am * dm)) / ( dm + am);
462  T v6b = (dm * v0 + am * ve - t * am * dm - ::std::sqrt( 2 * dm * v0 * am * ve - 2 * dm * dm * v0 * t * am - 2 * am * am * ve * t * dm + t * t * am * am * dm * dm - dm * am * ve * ve + 2 * x * am * dm * dm - am * dm * v0 * v0 + 2 * x * am * am * dm)) / ( dm + am);
463  T v7a;
464  T v7b;
465  T v8a = -(dm * v0 + am * ve - t * am * dm - ::std::sqrt( 2 * dm * v0 * am * ve - 2 * dm * dm * v0 * t * am - 2 * am * am * ve * t * dm + t * t * am * am * dm * dm - am * dm * v0 * v0 + 2 * x * am * am * dm - dm * am * ve * ve + 2 * x * am * dm * dm)) / ( am + dm);
466  T v8b = -(dm * v0 + am * ve - t * am * dm + ::std::sqrt( 2 * dm * v0 * am * ve - 2 * dm * dm * v0 * t * am - 2 * am * am * ve * t * dm + t * t * am * am * dm * dm - am * dm * v0 * v0 + 2 * x * am * am * dm - dm * am * ve * ve + 2 * x * am * dm * dm)) / ( am + dm);
467 
468  if (::std::abs(am - dm) <= ::std::numeric_limits<T>::epsilon())
469  {
470  v2a = ( v0 * v0 - ve * ve + 2 * x * am) / ( t * am + v0 - ve) / 2;
471  v2b = ( v0 * v0 - ve * ve + 2 * x * am) / ( t * am + v0 - ve) / 2;
472  v4a = -( v0 * v0 - ve * ve + 2 * x * am) / ( t * am + v0 - ve) / 2;
473  v4b = -( v0 * v0 - ve * ve + 2 * x * am) / ( t * am + v0 - ve) / 2;
474  v5a = -(-v0 * v0 + ve * ve + 2 * x * am) / (-t * am + v0 - ve) / 2;
475  v5b = -(-v0 * v0 + ve * ve + 2 * x * am) / (-t * am + v0 - ve) / 2;
476  v7a = (-v0 * v0 + ve * ve + 2 * x * am) / (-t * am + v0 - ve) / 2;
477  v7b = (-v0 * v0 + ve * ve + 2 * x * am) / (-t * am + v0 - ve) / 2;
478  }
479  else
480  {
481  v2a = -(dm * v0 - am * ve + t * am * dm - ::std::sqrt(-2 * dm * v0 * am * ve + 2 * dm * dm * v0 * t * am - 2 * am * am * ve * t * dm + t * t * am * am * dm * dm + dm * am * ve * ve - 2 * x * am * dm * dm + am * dm * v0 * v0 + 2 * x * am * am * dm)) / (-dm + am);
482  v2b = -(dm * v0 - am * ve + t * am * dm + ::std::sqrt(-2 * dm * v0 * am * ve + 2 * dm * dm * v0 * t * am - 2 * am * am * ve * t * dm + t * t * am * am * dm * dm + dm * am * ve * ve - 2 * x * am * dm * dm + am * dm * v0 * v0 + 2 * x * am * am * dm)) / (-dm + am);
483  v4a = (dm * v0 - am * ve + t * am * dm + ::std::sqrt(-2 * dm * v0 * am * ve + 2 * dm * dm * v0 * t * am - 2 * am * am * ve * t * dm + t * t * am * am * dm * dm + am * dm * v0 * v0 + 2 * x * am * am * dm + dm * am * ve * ve - 2 * x * am * dm * dm)) / ( am - dm);
484  v4b = (dm * v0 - am * ve + t * am * dm - ::std::sqrt(-2 * dm * v0 * am * ve + 2 * dm * dm * v0 * t * am - 2 * am * am * ve * t * dm + t * t * am * am * dm * dm + am * dm * v0 * v0 + 2 * x * am * am * dm + dm * am * ve * ve - 2 * x * am * dm * dm)) / ( am - dm);
485  v5a = -(dm * v0 - am * ve - t * am * dm - ::std::sqrt(-2 * dm * v0 * am * ve - 2 * dm * dm * v0 * t * am + 2 * am * am * ve * t * dm + t * t * am * am * dm * dm + dm * am * ve * ve + 2 * x * am * dm * dm + am * dm * v0 * v0 - 2 * x * am * am * dm)) / (-dm + am);
486  v5b = -(dm * v0 - am * ve - t * am * dm + ::std::sqrt(-2 * dm * v0 * am * ve - 2 * dm * dm * v0 * t * am + 2 * am * am * ve * t * dm + t * t * am * am * dm * dm + dm * am * ve * ve + 2 * x * am * dm * dm + am * dm * v0 * v0 - 2 * x * am * am * dm)) / (-dm + am);
487  v7a = (dm * v0 - am * ve - t * am * dm + ::std::sqrt(-2 * dm * v0 * am * ve - 2 * dm * dm * v0 * t * am + 2 * am * am * ve * t * dm + t * t * am * am * dm * dm + am * dm * v0 * v0 - 2 * x * am * am * dm + dm * am * ve * ve + 2 * x * am * dm * dm)) / ( am - dm);
488  v7b = (dm * v0 - am * ve - t * am * dm - ::std::sqrt(-2 * dm * v0 * am * ve - 2 * dm * dm * v0 * t * am + 2 * am * am * ve * t * dm + t * t * am * am * dm * dm + am * dm * v0 * v0 - 2 * x * am * am * dm + dm * am * ve * ve + 2 * x * am * dm * dm)) / ( am - dm);
489  }
490 
491  T ta1a = ( v1a - v0) / am;
492  T ta1b = ( v1b - v0) / am;
493  T ta2a = ( v2a - v0) / am;
494  T ta2b = ( v2b - v0) / am;
495  T ta3a = (-v3a - v0) / am;
496  T ta3b = (-v3b - v0) / am;
497  T ta4a = (-v4a - v0) / am;
498  T ta4b = (-v4b - v0) / am;
499  T ta5a = ( v5a - v0) / -am;
500  T ta5b = ( v5b - v0) / -am;
501  T ta6a = ( v6a - v0) / -am;
502  T ta6b = ( v6b - v0) / -am;
503  T ta7a = (-v7a - v0) / -am;
504  T ta7b = (-v7b - v0) / -am;
505  T ta8a = (-v8a - v0) / -am;
506  T ta8b = (-v8b - v0) / -am;
507 
508  T td1a = (ve - v1a) / -dm;
509  T td1b = (ve - v1b) / -dm;
510  T td2a = (ve - v2a) / dm;
511  T td2b = (ve - v2b) / dm;
512  T td3a = (ve + v3a) / -dm;
513  T td3b = (ve + v3b) / -dm;
514  T td4a = (ve + v4a) / dm;
515  T td4b = (ve + v4b) / dm;
516  T td5a = (ve - v5a) / -dm;
517  T td5b = (ve - v5b) / -dm;
518  T td6a = (ve - v6a) / dm;
519  T td6b = (ve - v6b) / dm;
520  T td7a = (ve + v7a) / -dm;
521  T td7b = (ve + v7b) / -dm;
522  T td8a = (ve + v8a) / dm;
523  T td8b = (ve + v8b) / dm;
524 
525  T tc1a = t - ta1a - td1a;
526  T tc1b = t - ta1b - td1b;
527  T tc2a = t - ta2a - td2a;
528  T tc2b = t - ta2b - td2b;
529  T tc3a = t - ta3a - td3a;
530  T tc3b = t - ta3b - td3b;
531  T tc4a = t - ta4a - td4a;
532  T tc4b = t - ta4b - td4b;
533  T tc5a = t - ta5a - td5a;
534  T tc5b = t - ta5b - td5b;
535  T tc6a = t - ta6a - td6a;
536  T tc6b = t - ta6b - td6b;
537  T tc7a = t - ta7a - td7a;
538  T tc7b = t - ta7b - td7b;
539  T tc8a = t - ta8a - td8a;
540  T tc8b = t - ta8b - td8b;
541 
542  T vh = vm;
543 
544  // am, vm, -dm
545  if (ta1a >= 0 && tc1a >= 0 && td1a >= 0 && ::std::abs(v1a) < ::std::abs(vh))
546  {
547  ta = ta1a;
548  tc = tc1a;
549  td = td1a;
550  ah = am;
551  vh = v1a;
552  dh = dm;
553  }
554  if (ta1b >= 0 && tc1b >= 0 && td1b >= 0 && ::std::abs(v1b) < ::std::abs(vh))
555  {
556  ta = ta1b;
557  tc = tc1b;
558  td = td1b;
559  ah = am;
560  vh = v1b;
561  dh = dm;
562  }
563  // am, vm, dm
564  if (ta2a >= 0 && tc2a >= 0 && td2a >= 0 && ::std::abs(v2a) < ::std::abs(vh))
565  {
566  ta = ta2a;
567  tc = tc2a;
568  td = td2a;
569  ah = am;
570  vh = v2a;
571  dh = -dm;
572  }
573  if (ta2b >= 0 && tc2b >= 0 && td2b >= 0 && ::std::abs(v2b) < ::std::abs(vh))
574  {
575  ta = ta2b;
576  tc = tc2b;
577  td = td2b;
578  ah = am;
579  vh = v2b;
580  dh = -dm;
581  }
582  // am, -vm, -dm
583  if (ta3a >= 0 && tc3a >= 0 && td3a >= 0 && ::std::abs(v3a) < ::std::abs(vh))
584  {
585  ta = ta3a;
586  tc = tc3a;
587  td = td3a;
588  ah = am;
589  vh = -v3a;
590  dh = dm;
591  }
592  if (ta3b >= 0 && tc3b >= 0 && td3b >= 0 && ::std::abs(v3b) < ::std::abs(vh))
593  {
594  ta = ta3b;
595  tc = tc3b;
596  td = td3b;
597  ah = am;
598  vh = -v3b;
599  dh = dm;
600  }
601  // am, -vm, dm
602  if (ta4a >= 0 && tc4a >= 0 && td4a >= 0 && ::std::abs(v4a) < ::std::abs(vh))
603  {
604  ta = ta4a;
605  tc = tc4a;
606  td = td4a;
607  ah = am;
608  vh = -v4a;
609  dh = -dm;
610  }
611  if (ta4b >= 0 && tc4b >= 0 && td4b >= 0 && ::std::abs(v4b) < ::std::abs(vh))
612  {
613  ta = ta4b;
614  tc = tc4b;
615  td = td4b;
616  ah = am;
617  vh = -v4b;
618  dh = -dm;
619  }
620  // -am, vm, -dm
621  if (ta5a >= 0 && tc5a >= 0 && td5a >= 0 && ::std::abs(v5a) < ::std::abs(vh))
622  {
623  ta = ta5a;
624  tc = tc5a;
625  td = td5a;
626  ah = -am;
627  vh = v5a;
628  dh = dm;
629  }
630  if (ta5b >= 0 && tc5b >= 0 && td5b >= 0 && ::std::abs(v5b) < ::std::abs(vh))
631  {
632  ta = ta5b;
633  tc = tc5b;
634  td = td5b;
635  ah = -am;
636  vh = v5b;
637  dh = dm;
638  }
639  // -am, vm, dm
640  if (ta6a >= 0 && tc6a >= 0 && td6a >= 0 && ::std::abs(v6a) < ::std::abs(vh))
641  {
642  ta = ta6a;
643  tc = tc6a;
644  td = td6a;
645  ah = -am;
646  vh = v6a;
647  dh = -dm;
648  }
649  if (ta6b >= 0 && tc6b >= 0 && td6b >= 0 && ::std::abs(v6b) < ::std::abs(vh))
650  {
651  ta = ta6b;
652  tc = tc6b;
653  td = td6b;
654  ah = -am;
655  vh = v6b;
656  dh = -dm;
657  }
658  // -am, -vm, -dm
659  if (ta7a >= 0 && tc7a >= 0 && td7a >= 0 && ::std::abs(v7a) < ::std::abs(vh))
660  {
661  ta = ta7a;
662  tc = tc7a;
663  td = td7a;
664  ah = -am;
665  vh = -v7a;
666  dh = dm;
667  }
668  if (ta7b >= 0 && tc7b >= 0 && td7b >= 0 && ::std::abs(v7b) < ::std::abs(vh))
669  {
670  ta = ta7b;
671  tc = tc7b;
672  td = td7b;
673  ah = -am;
674  vh = -v7b;
675  dh = dm;
676  }
677  // -am, -vm, dm
678  if (ta8a >= 0 && tc8a >= 0 && td8a >= 0 && ::std::abs(v8a) < ::std::abs(vh))
679  {
680  ta = ta8a;
681  tc = tc8a;
682  td = td8a;
683  ah = -am;
684  vh = -v8a;
685  dh = -dm;
686  }
687  if (ta8b >= 0 && tc8b >= 0 && td8b >= 0 && ::std::abs(v8b) < ::std::abs(vh))
688  {
689  ta = ta8b;
690  tc = tc8b;
691  td = td8b;
692  ah = -am;
693  vh = -v8b;
694  dh = -dm;
695  }
696  }
697 
698  T t() const
699  {
700  return ta + tc + td;
701  }
702 
703  T v(const Real& t) const
704  {
705  if (t < ta)
706  {
707  return v0 + ah * t;
708  }
709  else if (t < ta + tc)
710  {
711  return v0 + ah * ta;
712  }
713  else if (t < ta + tc + td)
714  {
715  return v0 + ah * ta - dh * t + dh * (ta + tc);
716  }
717  else
718  {
719  return ve;
720  }
721  }
722 
723  T x(const Real& t) const
724  {
725  if (t < ta)
726  {
727  return x0 + v0 * t + 0.5 * ah * ::std::pow(t, 2);
728  }
729  else if (t < ta + tc)
730  {
731  return x0 + v0 * t - 0.5 * ah * ::std::pow(ta, 2) + ah * ta * t;
732  }
733  else if (t < ta + tc + td)
734  {
735  return x0 + v0 * t - 0.5 * ah * ::std::pow(ta, 2) + ah * ta * t - 0.5 * dh * ::std::pow(ta + tc, 2) - 0.5 * dh * ::std::pow(t, 2) + dh * (ta + tc) * t;
736  }
737  else
738  {
739  return xe + ve * (t - ta - tc - td);
740  }
741  }
742 
743  T am;
744 
745  T dm;
746 
747  T x0;
748 
749  T xe;
750 
751  T v0;
752 
753  T ve;
754 
755  T vm;
756 
757  protected:
758 
759  private:
760  T ah;
761 
762  T dh;
763 
764  Real ta;
765 
766  Real tc;
767 
768  Real td;
769  };
770  }
771 }
772 
773 #endif // RL_MATH_TRAPEZOIDALVELOCITY_H
rl::math::TrapezoidalVelocity
Definition: TrapezoidalVelocity.h:42
pow
Quaternion< Scalar > pow(const Scalar &t) const
Definition: QuaternionBaseAddons.h:128
rl::math::TrapezoidalVelocity::ah
T ah
Definition: TrapezoidalVelocity.h:760
rl::math::TrapezoidalVelocity::a
T a(const Real &t) const
Definition: TrapezoidalVelocity.h:64
rl::math::TrapezoidalVelocity::t
T t() const
Definition: TrapezoidalVelocity.h:698
rl::math::TrapezoidalVelocity::~TrapezoidalVelocity
virtual ~TrapezoidalVelocity()
Definition: TrapezoidalVelocity.h:60
rl::math::TrapezoidalVelocity::dm
T dm
Definition: TrapezoidalVelocity.h:745
rl::math::TrapezoidalVelocity::interpolate
void interpolate()
Definition: TrapezoidalVelocity.h:84
rl::math::TrapezoidalVelocity::tc
Real tc
Definition: TrapezoidalVelocity.h:766
rl::math::TrapezoidalVelocity::dh
T dh
Definition: TrapezoidalVelocity.h:762
rl::math::TrapezoidalVelocity::v
T v(const Real &t) const
Definition: TrapezoidalVelocity.h:703
rl::math::TrapezoidalVelocity::x
T x(const Real &t) const
Definition: TrapezoidalVelocity.h:723
rl::math::TrapezoidalVelocity::TrapezoidalVelocity
TrapezoidalVelocity()
Definition: TrapezoidalVelocity.h:44
rl::math::TrapezoidalVelocity::x0
T x0
Definition: TrapezoidalVelocity.h:747
Real.h
std
Definition: thread.h:482
rl::math::TrapezoidalVelocity::td
Real td
Definition: TrapezoidalVelocity.h:768
rl::math::TrapezoidalVelocity::xe
T xe
Definition: TrapezoidalVelocity.h:749
rl::math::TrapezoidalVelocity::ve
T ve
Definition: TrapezoidalVelocity.h:753
rl::math::TrapezoidalVelocity::v0
T v0
Definition: TrapezoidalVelocity.h:751
rl::math::Real
double Real
Definition: Real.h:42
rl::math::TrapezoidalVelocity::ta
Real ta
Definition: TrapezoidalVelocity.h:764
rl::math::TrapezoidalVelocity::vm
T vm
Definition: TrapezoidalVelocity.h:755
rl::math::TrapezoidalVelocity::interpolate
void interpolate(const Real &t)
Definition: TrapezoidalVelocity.h:447
rl::math::TrapezoidalVelocity::am
T am
Definition: TrapezoidalVelocity.h:743
rl
Robotics Library.
Definition: AnalogInput.cpp:30
Generated by doxygen 1.8.20