velocityprofiletest.cpp
Go to the documentation of this file.
2 #include <frames_io.hpp>
4 
5 using namespace KDL;
6 
8 {
9 }
10 
12 {
13 }
14 
16 {
17  // 2 second ramp up (cover 2 distance),
18  // 2 second flat velocity (cover 4 distance)
19  // 2 second ramp down (cover 2 distance),
20  VelocityProfile_Trap v(2, 1);
21  double time;
22  v.SetProfile(2, 10);
23  CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
24 
25  // start
26  time = 0;
27  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
28  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
29  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
30 
31  // end of ramp up
32  time = 2;
33  CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
34  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
35  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
36 
37  // middle of flat velocity
38  time = 3;
39  CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
40  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
41  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
42 
43  // end of flat velocity
44  time = 4;
45  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
46  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
47  CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
48 
49  // middle of ramp down
50  time = 5;
51  CPPUNIT_ASSERT_DOUBLES_EQUAL(9.5, v.Pos(time),epsilon);
52  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
53  CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
54 
55  // end
56  time = 6;
57  CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
58  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
59  CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Acc(time),epsilon);
60 
61  // fenceposts - before and after
62  time = -1;
63  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
64  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
65  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
66  time = 11;
67  CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
68  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
69  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
70 }
71 
73 {
74  // 2 second ramp up (cover -2 distance),
75  // 2 second flat velocity (cover -4 distance)
76  // 2 second ramp down (cover -2 distance),
77  VelocityProfile_Trap v(2, 1);
78  v.SetProfile(2, -6);
79  CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
80 }
81 
83 {
84  // 2 second ramp up (cover 4 distance),
85  // 0 second flat velocity (cover 0 distance)
86  // 2 second ramp down (cover 4 distance),
87  VelocityProfile_Trap v(4, 2);
88  v.SetProfile(2, 10);
89  CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
90 
91  // new profile
92  v.SetProfile(2, -6);
93  CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
94 
95  // another new profile : ramp + 2 sec + ramp
96  v.SetProfile(13, 13 + 4 + 8 + 4);
97  CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Duration(),epsilon);
98 }
99 
101 {
102  // same as first max velocity test, but twice as
103  // long (max velocity gives 6 seconds)
104  VelocityProfile_Trap v(2, 1);
105  double time;
106  v.SetProfileDuration(2, 10, 12.0);
107  CPPUNIT_ASSERT_DOUBLES_EQUAL(12.0, v.Duration(),epsilon);
108 
109  // start
110  time = 0;
111  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
112  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
113  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.25, v.Acc(time),epsilon);
114 
115  // end of ramp up
116  time = 4;
117  CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
118  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
119  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
120 
121  // middle of flat velocity
122  time = 6;
123  CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
124  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
125  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
126 
127  // end of flat velocity
128  time = 8;
129  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
130  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time),epsilon);
131  CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
132 
133  // middle of ramp down
134  time = 10;
135  CPPUNIT_ASSERT_DOUBLES_EQUAL(9.5, v.Pos(time),epsilon);
136  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.5, v.Vel(time),epsilon);
137  CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
138 
139  // end
140  time = 12;
141  CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, v.Pos(time),epsilon);
142  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
143  CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.25, v.Acc(time),epsilon);
144 }
145 
147 {
148  // 2 second ramp up (cover 2 distance),
149  // 2 second flat velocity (cover 4 distance)
150  VelocityProfile_TrapHalf v(2, 1, true);
151  double time;
152  v.SetProfile(2, 2+6);
153  CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
154 
155  // start
156  time = 0;
157  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
158  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
159  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
160 
161  // end of ramp up
162  time = 2;
163  CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Pos(time),epsilon);
164  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
165  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
166 
167  // middle of flat velocity
168  time = 3;
169  CPPUNIT_ASSERT_DOUBLES_EQUAL(6.0, v.Pos(time),epsilon);
170  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
171  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
172 
173  // end
174  time = 4;
175  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
176  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
177  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
178 
179  // fenceposts - before and after
180  time = -1;
181  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
182  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
183  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
184  time = 5;
185  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
186  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
187  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
188 }
189 
191 {
192  // 2 second flat velocity (cover 4 distance)
193  // 2 second ramp up (cover 2 distance),
194  VelocityProfile_TrapHalf v(2, 1, false);
195  double time;
196  v.SetProfile(9, 9-6);
197  CPPUNIT_ASSERT_DOUBLES_EQUAL(4.0, v.Duration(),epsilon);
198 
199  // start - flat velocity
200  time = 0;
201  CPPUNIT_ASSERT_DOUBLES_EQUAL(9.0, v.Pos(time),epsilon);
202  CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
203  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
204 
205  // end of flat velocity
206  time = 2;
207  CPPUNIT_ASSERT_DOUBLES_EQUAL(5.0, v.Pos(time),epsilon);
208  CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
209  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
210 
211  // middle of ramp down
212  time = 3;
213  CPPUNIT_ASSERT_DOUBLES_EQUAL(3.5, v.Pos(time),epsilon);
214  CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Vel(time),epsilon);
215  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Acc(time),epsilon);
216 
217  // end
218  time = 4;
219  CPPUNIT_ASSERT_DOUBLES_EQUAL(3.0, v.Pos(time),epsilon);
220  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
221  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
222 
223  // fenceposts - before and after
224  time = -1;
225  CPPUNIT_ASSERT_DOUBLES_EQUAL(9.0, v.Pos(time),epsilon);
226  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
227  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
228  time = 5;
229  CPPUNIT_ASSERT_DOUBLES_EQUAL(3.0, v.Pos(time),epsilon);
230  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
231  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
232 }
233 
235 {
236  // same as TestTrapHalf__SetProfile_Start() but twice as slow
237  // Lingers at start position with zero velocity for a period of time,
238  // as does not scale the velocity; only scales the acceleration!?
239  VelocityProfile_TrapHalf v(2, 1, true);
240  double time;
241  v.SetProfileDuration(2, 2+6, 8);
242  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Duration(),epsilon);
243 
244  // start - no motion
245  time = 0;
246  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
247  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
248  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
249 
250  // no motion
251  time = 1.9;
252  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
253  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
254  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
255 
256  // begin ramp at scaled acceleration
257  time = 2;
258  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time), 0.001);
259  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
260  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.333, v.Acc(time), 0.001);
261 
262  // middle of ramp up
263  time = 5;
264  CPPUNIT_ASSERT_DOUBLES_EQUAL(3.5, v.Pos(time), 0.001);
265  CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, v.Vel(time), 0.001);
266  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.3333, v.Acc(time), 0.001);
267 
268  // end - continue with given velocity
269  time = 8;
270  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
271  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Vel(time),epsilon);
272  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
273 
274  // fenceposts - before and after
275  time = -1;
276  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
277  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
278  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
279  time = 9;
280  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
281  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
282  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
283 
284 }
285 
287 {
288  // same as TestTrapHalf__SetProfile_Start() but twice as slow
289  // Lingers at start position with zero velocity for a period of time,
290  // as does not scale the velocity; only scales the acceleration!?
291  VelocityProfile_TrapHalf v(2, 1, true);
292  double time;
293  v.SetProfileDuration(2+6, 2, 8);
294  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Duration(),epsilon);
295 
296  // start - no motion
297  time = 0;
298  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
299  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
300  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
301 
302  // no motion
303  time = 1.9;
304  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
305  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
306  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
307 
308  // begin ramp at scaled acceleration
309  time = 2;
310  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time), 0.001);// WRONG, backwards!
311  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
312  CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.333, v.Acc(time), 0.001);
313 
314  // middle of ramp up
315  time = 5;
316  CPPUNIT_ASSERT_DOUBLES_EQUAL(6.5, v.Pos(time), 0.001);
317  CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0, v.Vel(time), 0.001);
318  CPPUNIT_ASSERT_DOUBLES_EQUAL(-0.3333, v.Acc(time), 0.001);
319 
320  // end - continue with given velocity
321  time = 8;
322  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
323  CPPUNIT_ASSERT_DOUBLES_EQUAL(-2.0, v.Vel(time),epsilon);
324  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
325 
326  // fenceposts - before and after
327  time = -1;
328  CPPUNIT_ASSERT_DOUBLES_EQUAL(8.0, v.Pos(time),epsilon);
329  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
330  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
331  time = 9;
332  CPPUNIT_ASSERT_DOUBLES_EQUAL(2.0, v.Pos(time),epsilon);
333  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Vel(time),epsilon);
334  CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, v.Acc(time),epsilon);
335 
336 }
337 
339 {
340  double time;
341  double pos1 = 10.0;
342  double pos2 = -1.0;
343 
345  v.SetProfile(pos1, pos2);
346 
347  time = 0;
348  CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
349 
350  time = 1.0;
351  CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
352 
353  time = -1.0;
354  CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
355 }
356 
358 {
359  double time;
360  double pos1 = 10.0;
361  double pos2 = -1.0;
362  double duration = 5.0;
364  v.SetProfileDuration(pos1, pos2, duration);
365 
366  time = -1.0;
367  CPPUNIT_ASSERT_DOUBLES_EQUAL(pos1, v.Pos(time), epsilon);
368 
369  time = duration/2;
370  CPPUNIT_ASSERT_DOUBLES_EQUAL((pos1 + pos2)/2, v.Pos(time), epsilon);
371  CPPUNIT_ASSERT_DOUBLES_EQUAL((pos2-pos1)/duration, v.Vel(time), epsilon);
372 
373  time = duration;
374  CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
375 
376  time = duration + 1.0;
377  CPPUNIT_ASSERT_DOUBLES_EQUAL(pos2, v.Pos(time), epsilon);
378 }
virtual double Vel(double time) const
virtual double Pos(double time) const
virtual double Pos(double time) const
virtual double Acc(double time) const
void SetProfile(double pos1, double pos2)
virtual double Vel(double time) const
virtual double Pos(double time) const
virtual void SetProfileDuration(double pos1, double pos2, double newduration)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
virtual double Acc(double time) const
virtual void SetProfileDuration(double pos1, double pos2, double duration)
virtual void SetProfile(double pos1, double pos2)
CPPUNIT_TEST_SUITE_REGISTRATION(VelocityProfileTest)
virtual void SetProfile(double pos1, double pos2)
virtual void SetProfileDuration(double pos1, double pos2, double newduration)
virtual double Vel(double time) const
virtual double Duration() const


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14