moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
unittest_velocity_profile_atrap.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2018 Pilz GmbH & Co. KG
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Pilz GmbH & Co. KG nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35#include <gtest/gtest.h>
36
37#include <pilz_industrial_motion_planner/velocity_profile_atrap.h>
38
39// Modultest Level1 of Class VelocityProfileATrap
40#define EPSILON 1.0e-10
41
42TEST(ATrapTest, Test_SetProfile1)
43{
44 pilz_industrial_motion_planner::VelocityProfileATrap vp =
45 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
46
47 // can reach the maximal velocity
48 vp.SetProfile(3, 35);
49
50 EXPECT_NEAR(vp.Duration(), 11.0, EPSILON);
51
52 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
53 EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON);
54 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
55
56 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
57 EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON);
58 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
59
60 EXPECT_NEAR(vp.Pos(1), 4.0, EPSILON);
61 EXPECT_NEAR(vp.Vel(1), 2.0, EPSILON);
62 EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON);
63
64 EXPECT_NEAR(vp.Pos(2), 7.0, EPSILON);
65 EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON);
66 EXPECT_NEAR(vp.Acc(2), 2.0, EPSILON);
67
68 EXPECT_NEAR(vp.Pos(4.5), 17.0, EPSILON);
69 EXPECT_NEAR(vp.Vel(4.5), 4.0, EPSILON);
70 EXPECT_NEAR(vp.Acc(4.5), 0.0, EPSILON);
71
72 EXPECT_NEAR(vp.Pos(7), 27.0, EPSILON);
73 EXPECT_NEAR(vp.Vel(7), 4.0, EPSILON);
74 EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON);
75
76 EXPECT_NEAR(vp.Pos(9), 33.0, EPSILON);
77 EXPECT_NEAR(vp.Vel(9), 2.0, EPSILON);
78 EXPECT_NEAR(vp.Acc(9), -1.0, EPSILON);
79
80 EXPECT_NEAR(vp.Pos(11), 35.0, EPSILON);
81 EXPECT_NEAR(vp.Vel(11), 0.0, EPSILON);
82 EXPECT_NEAR(vp.Acc(11), -1.0, EPSILON);
83
84 EXPECT_NEAR(vp.Pos(12), 35.0, EPSILON);
85 EXPECT_NEAR(vp.Vel(12), 0.0, EPSILON);
86 EXPECT_NEAR(vp.Acc(12), 0.0, EPSILON);
87}
88
89TEST(ATrapTest, Test_SetProfile2)
90{
91 pilz_industrial_motion_planner::VelocityProfileATrap vp =
92 pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1.5);
93
94 // just arrive the maximal velocity
95 vp.SetProfile(5, 26);
96
97 EXPECT_NEAR(vp.Duration(), 7.0, EPSILON);
98
99 EXPECT_NEAR(vp.Pos(-1), 5.0, EPSILON);
100 EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON);
101 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
102
103 EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON);
104 EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON);
105 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
106
107 EXPECT_NEAR(vp.Pos(1.5), 7.25, EPSILON);
108 EXPECT_NEAR(vp.Vel(1.5), 3.0, EPSILON);
109 EXPECT_NEAR(vp.Acc(1.5), 2.0, EPSILON);
110
111 EXPECT_NEAR(vp.Pos(3), 14.0, EPSILON);
112 EXPECT_NEAR(vp.Vel(3), 6.0, EPSILON);
113 EXPECT_NEAR(vp.Acc(3), 2.0, EPSILON);
114
115 EXPECT_NEAR(vp.Pos(5), 23.0, EPSILON);
116 EXPECT_NEAR(vp.Vel(5), 3.0, EPSILON);
117 EXPECT_NEAR(vp.Acc(5), -1.5, EPSILON);
118
119 EXPECT_NEAR(vp.Pos(7), 26.0, EPSILON);
120 EXPECT_NEAR(vp.Vel(7), 0.0, EPSILON);
121 EXPECT_NEAR(vp.Acc(7), -1.5, EPSILON);
122
123 EXPECT_NEAR(vp.Pos(8), 26.0, EPSILON);
124 EXPECT_NEAR(vp.Vel(8), 0.0, EPSILON);
125 EXPECT_NEAR(vp.Acc(8), 0.0, EPSILON);
126}
127
128TEST(ATrapTest, Test_SetProfile3)
129{
130 pilz_industrial_motion_planner::VelocityProfileATrap vp =
131 pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1);
132
133 // cannot reach the maximal velocity
134 vp.SetProfile(5, 17);
135
136 EXPECT_NEAR(vp.Duration(), 6.0, EPSILON);
137
138 EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON);
139 EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON);
140 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
141
142 EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON);
143 EXPECT_NEAR(vp.Vel(1), 2.0, EPSILON);
144 EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON);
145
146 EXPECT_NEAR(vp.Pos(2), 9.0, EPSILON);
147 EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON);
148 EXPECT_NEAR(vp.Acc(2), 2.0, EPSILON);
149
150 EXPECT_NEAR(vp.Pos(4), 15.0, EPSILON);
151 EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON);
152 EXPECT_NEAR(vp.Acc(4), -1, EPSILON);
153
154 EXPECT_NEAR(vp.Pos(6), 17.0, EPSILON);
155 EXPECT_NEAR(vp.Vel(6), 0.0, EPSILON);
156 EXPECT_NEAR(vp.Acc(6), -1.0, EPSILON);
157
158 EXPECT_NEAR(vp.Pos(7), 17.0, EPSILON);
159 EXPECT_NEAR(vp.Vel(7), 0.0, EPSILON);
160 EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON);
161}
162
163TEST(ATrapTest, Test_SetProfile4)
164{
165 pilz_industrial_motion_planner::VelocityProfileATrap vp =
166 pilz_industrial_motion_planner::VelocityProfileATrap(6, 2, 1);
167
168 // empty profile
169 vp.SetProfile(5, 5);
170
171 EXPECT_NEAR(vp.Duration(), 0.0, EPSILON);
172
173 EXPECT_NEAR(vp.Pos(-1), 5.0, EPSILON);
174 EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON);
175 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
176
177 EXPECT_NEAR(vp.Pos(0), 5.0, EPSILON);
178 EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON);
179 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
180}
181
196TEST(ATrapTest, Test_SetProfileToLowDuration)
197{
198 pilz_industrial_motion_planner::VelocityProfileATrap vp1 =
199 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
200 pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1;
201
202 vp1.SetProfileDuration(3, 35, std::numeric_limits<double>::epsilon());
203 double fastest_duration = vp1.Duration();
204
205 vp2.SetProfileDuration(3, 35, fastest_duration);
206
207 EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2;
208}
209
223TEST(ATrapTest, Test_setProfileAllDurationsToLowDuration)
224{
225 pilz_industrial_motion_planner::VelocityProfileATrap vp1 =
226 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
227 pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1;
228
229 vp1.SetProfile(3, 35);
230 double fastest_duration = vp1.Duration();
231
232 // Trigger Duration()>(3*fastest_duration/4)
233 vp2.setProfileAllDurations(3, 35, fastest_duration / 4, fastest_duration / 4, fastest_duration / 4);
234
235 EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2;
236}
237
249TEST(ATrapTest, Test_SetProfileZeroStartVelocity)
250{
251 pilz_industrial_motion_planner::VelocityProfileATrap vp1 =
252 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
253 pilz_industrial_motion_planner::VelocityProfileATrap vp2 = vp1;
254
255 vp1.SetProfile(1, 2);
256
257 vp2.setProfileStartVelocity(1, 2, 0); // <-- Set zero velocity
258 EXPECT_TRUE(vp1 == vp2) << "Not equal! Profile 1: \n" << vp1 << "\n Profile 2: " << vp2;
259}
260
261TEST(ATrapTest, Test_SetProfileDuration)
262{
263 pilz_industrial_motion_planner::VelocityProfileATrap vp =
264 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
265
266 // set the duration as twice as the fastest profile
267 vp.SetProfileDuration(3, 35, 22.0);
268
269 EXPECT_NEAR(vp.Duration(), 22.0, EPSILON);
270
271 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
272 EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON);
273 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
274
275 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
276 EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON);
277 EXPECT_NEAR(vp.Acc(0), 0, EPSILON);
278
279 EXPECT_NEAR(vp.Pos(2), 4.0, EPSILON);
280 EXPECT_NEAR(vp.Vel(2), 1.0, EPSILON);
281 EXPECT_NEAR(vp.Acc(2), 0.5, EPSILON);
282
283 EXPECT_NEAR(vp.Pos(4), 7.0, EPSILON);
284 EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON);
285 EXPECT_NEAR(vp.Acc(4), 0.5, EPSILON);
286
287 EXPECT_NEAR(vp.Pos(9), 17.0, EPSILON);
288 EXPECT_NEAR(vp.Vel(9), 2.0, EPSILON);
289 EXPECT_NEAR(vp.Acc(9), 0.0, EPSILON);
290
291 EXPECT_NEAR(vp.Pos(14), 27.0, EPSILON);
292 EXPECT_NEAR(vp.Vel(14), 2.0, EPSILON);
293 EXPECT_NEAR(vp.Acc(14), 0.0, EPSILON);
294
295 EXPECT_NEAR(vp.Pos(18), 33.0, EPSILON);
296 EXPECT_NEAR(vp.Vel(18), 1.0, EPSILON);
297 EXPECT_NEAR(vp.Acc(18), -0.25, EPSILON);
298
299 EXPECT_NEAR(vp.Pos(22), 35.0, EPSILON);
300 EXPECT_NEAR(vp.Vel(22), 0.0, EPSILON);
301 EXPECT_NEAR(vp.Acc(22), -0.25, EPSILON);
302
303 EXPECT_NEAR(vp.Pos(23), 35.0, EPSILON);
304 EXPECT_NEAR(vp.Vel(23), 0.0, EPSILON);
305 EXPECT_NEAR(vp.Acc(23), 0.0, EPSILON);
306}
307
308TEST(ATrapTest, Test_setProfileAllDurations1)
309{
310 pilz_industrial_motion_planner::VelocityProfileATrap vp =
311 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
312
313 // set durations
314 EXPECT_TRUE(vp.setProfileAllDurations(3, 35, 3.0, 4.0, 5.0));
315
316 EXPECT_NEAR(vp.Duration(), 12.0, EPSILON);
317
318 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
319 EXPECT_NEAR(vp.Vel(-1), 0.0, EPSILON);
320 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
321
322 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
323 EXPECT_NEAR(vp.Vel(0), 0.0, EPSILON);
324 EXPECT_NEAR(vp.Acc(0), 0, EPSILON);
325
326 EXPECT_NEAR(vp.Pos(2), 3.0 + 8.0 / 3.0, EPSILON);
327 EXPECT_NEAR(vp.Vel(2), 8.0 / 3.0, EPSILON);
328 EXPECT_NEAR(vp.Acc(2), 4.0 / 3.0, EPSILON);
329
330 EXPECT_NEAR(vp.Pos(3), 9.0, EPSILON);
331 EXPECT_NEAR(vp.Vel(3), 4.0, EPSILON);
332 EXPECT_NEAR(vp.Acc(3), 4.0 / 3.0, EPSILON);
333
334 EXPECT_NEAR(vp.Pos(5), 17.0, EPSILON);
335 EXPECT_NEAR(vp.Vel(5), 4.0, EPSILON);
336 EXPECT_NEAR(vp.Acc(5), 0.0, EPSILON);
337
338 EXPECT_NEAR(vp.Pos(7), 25.0, EPSILON);
339 EXPECT_NEAR(vp.Vel(7), 4.0, EPSILON);
340 EXPECT_NEAR(vp.Acc(7), 0.0, EPSILON);
341
342 EXPECT_NEAR(vp.Pos(9), 31.4, EPSILON);
343 EXPECT_NEAR(vp.Vel(9), 2.4, EPSILON);
344 EXPECT_NEAR(vp.Acc(9), -0.8, EPSILON);
345
346 EXPECT_NEAR(vp.Pos(12), 35.0, EPSILON);
347 EXPECT_NEAR(vp.Vel(12), 0.0, EPSILON);
348 EXPECT_NEAR(vp.Acc(12), -0.8, EPSILON);
349
350 EXPECT_NEAR(vp.Pos(13), 35.0, EPSILON);
351 EXPECT_NEAR(vp.Vel(13), 0.0, EPSILON);
352 EXPECT_NEAR(vp.Acc(13), 0.0, EPSILON);
353}
354
355TEST(ATrapTest, Test_setProfileAllDurations2)
356{
357 pilz_industrial_motion_planner::VelocityProfileATrap vp =
358 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
359
360 // invalid maximal velocity
361 EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 3.0, 3.0, 5.0));
362 // invalid acceleration
363 EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 1.0, 4.0, 7.0));
364 // invalid deceleration
365 EXPECT_FALSE(vp.setProfileAllDurations(3, 35, 7.0, 4.0, 1.0));
366}
367
368TEST(ATrapTest, Test_setProfileStartVelocity1)
369{
370 pilz_industrial_motion_planner::VelocityProfileATrap vp =
371 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
372
373 // invalid cases
374 EXPECT_FALSE(vp.setProfileStartVelocity(3.0, 5.0, -1.0));
375
376 // only deceleration
377 vp.setProfileStartVelocity(3.0, 5.0, 2.0);
378
379 EXPECT_NEAR(vp.Duration(), 2.0, EPSILON);
380 EXPECT_NEAR(vp.firstPhaseDuration(), 2.0, EPSILON);
381 EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON);
382 EXPECT_NEAR(vp.thirdPhaseDuration(), 0.0, EPSILON);
383
384 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
385 EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON);
386 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
387
388 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
389 EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON);
390 EXPECT_NEAR(vp.Acc(0), 0, EPSILON);
391
392 EXPECT_NEAR(vp.Pos(1), 4.5, EPSILON);
393 EXPECT_NEAR(vp.Vel(1), 1.0, EPSILON);
394 EXPECT_NEAR(vp.Acc(1), -1.0, EPSILON);
395
396 EXPECT_NEAR(vp.Pos(2), 5.0, EPSILON);
397 EXPECT_NEAR(vp.Vel(2), 0.0, EPSILON);
398 EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON);
399
400 EXPECT_NEAR(vp.Pos(3), 5.0, EPSILON);
401 EXPECT_NEAR(vp.Vel(3), 0.0, EPSILON);
402 EXPECT_NEAR(vp.Acc(3), 0.0, EPSILON);
403}
404
405TEST(ATrapTest, Test_setProfileStartVelocity2)
406{
407 pilz_industrial_motion_planner::VelocityProfileATrap vp =
408 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
409
410 // deceleration, acceleration and deceleration
411 vp.setProfileStartVelocity(3.0, 4.0, 2.0);
412 EXPECT_NEAR(vp.Duration(), 2.0 + 3 * sqrt(1.0 / 3.0), EPSILON);
413 EXPECT_NEAR(vp.firstPhaseDuration(), 2.0, EPSILON);
414 EXPECT_NEAR(vp.secondPhaseDuration(), sqrt(1.0 / 3.0), EPSILON);
415 EXPECT_NEAR(vp.thirdPhaseDuration(), 2 * sqrt(1.0 / 3.0), EPSILON);
416
417 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
418 EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON);
419 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
420
421 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
422 EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON);
423 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
424
425 EXPECT_NEAR(vp.Pos(1), 4.5, EPSILON);
426 EXPECT_NEAR(vp.Vel(1), 1.0, EPSILON);
427 EXPECT_NEAR(vp.Acc(1), -1.0, EPSILON);
428
429 EXPECT_NEAR(vp.Pos(2), 5.0, EPSILON);
430 EXPECT_NEAR(vp.Vel(2), 0.0, EPSILON);
431 EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON);
432
433 EXPECT_NEAR(vp.Pos(2.1), 4.99, EPSILON);
434 EXPECT_NEAR(vp.Vel(2.1), -0.2, EPSILON);
435 EXPECT_NEAR(vp.Acc(2.1), -2.0, EPSILON);
436
437 EXPECT_NEAR(vp.Pos(2 + sqrt(1.0 / 3.0)), 5.0 - 1.0 / 3.0, EPSILON);
438 EXPECT_NEAR(vp.Vel(2 + sqrt(1.0 / 3.0)), -2 * sqrt(1.0 / 3.0), EPSILON);
439 EXPECT_NEAR(vp.Acc(2 + sqrt(1.0 / 3.0)), -2.0, EPSILON);
440
441 EXPECT_NEAR(vp.Pos(2 + 3 * sqrt(1.0 / 3.0) - 0.2), 4.02, EPSILON);
442 EXPECT_NEAR(vp.Vel(2 + 3 * sqrt(1.0 / 3.0) - 0.2), -0.2, EPSILON);
443 EXPECT_NEAR(vp.Acc(2 + 3 * sqrt(1.0 / 3.0) - 0.2), 1.0, EPSILON);
444
445 EXPECT_NEAR(vp.Pos(2 + 3 * sqrt(1.0 / 3.0)), 4.0, EPSILON);
446 EXPECT_NEAR(vp.Vel(2 + 3 * sqrt(1.0 / 3.0)), 0, EPSILON);
447 EXPECT_NEAR(vp.Acc(2 + 3 * sqrt(1.0 / 3.0)), 1.0, EPSILON);
448
449 EXPECT_NEAR(vp.Pos(5), 4.0, EPSILON);
450 EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON);
451 EXPECT_NEAR(vp.Acc(5), 0.0, EPSILON);
452}
453
454TEST(ATrapTest, Test_setProfileStartVelocity3)
455{
456 pilz_industrial_motion_planner::VelocityProfileATrap vp =
457 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
458
459 // acceleration, deceleration
460 vp.setProfileStartVelocity(3, 14, 2);
461 EXPECT_NEAR(vp.Duration(), 5.0, EPSILON);
462 EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON);
463 EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON);
464 EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON);
465
466 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
467 EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON);
468 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
469
470 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
471 EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON);
472 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
473
474 EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON);
475 EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON);
476 EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON);
477
478 EXPECT_NEAR(vp.Pos(2), 9.5, EPSILON);
479 EXPECT_NEAR(vp.Vel(2), 3.0, EPSILON);
480 EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON);
481
482 EXPECT_NEAR(vp.Pos(3), 12.0, EPSILON);
483 EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON);
484 EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON);
485
486 EXPECT_NEAR(vp.Pos(5), 14.0, EPSILON);
487 EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON);
488 EXPECT_NEAR(vp.Acc(5), -1.0, EPSILON);
489
490 EXPECT_NEAR(vp.Pos(6), 14, EPSILON);
491 EXPECT_NEAR(vp.Vel(6), 0, EPSILON);
492 EXPECT_NEAR(vp.Acc(6), 0, EPSILON);
493}
494
495TEST(ATrapTest, Test_setProfileStartVelocity4)
496{
497 pilz_industrial_motion_planner::VelocityProfileATrap vp =
498 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
499
500 // acceleration, constant, deceleration
501 vp.setProfileStartVelocity(3, 14, 2);
502 EXPECT_NEAR(vp.Duration(), 5.0, EPSILON);
503 EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON);
504 EXPECT_NEAR(vp.secondPhaseDuration(), 0.0, EPSILON);
505 EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON);
506
507 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
508 EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON);
509 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
510
511 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
512 EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON);
513 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
514
515 EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON);
516 EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON);
517 EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON);
518
519 EXPECT_NEAR(vp.Pos(2), 9.5, EPSILON);
520 EXPECT_NEAR(vp.Vel(2), 3.0, EPSILON);
521 EXPECT_NEAR(vp.Acc(2), -1.0, EPSILON);
522
523 EXPECT_NEAR(vp.Pos(3), 12.0, EPSILON);
524 EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON);
525 EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON);
526
527 EXPECT_NEAR(vp.Pos(5), 14.0, EPSILON);
528 EXPECT_NEAR(vp.Vel(5), 0.0, EPSILON);
529 EXPECT_NEAR(vp.Acc(5), -1.0, EPSILON);
530
531 EXPECT_NEAR(vp.Pos(6), 14, EPSILON);
532 EXPECT_NEAR(vp.Vel(6), 0, EPSILON);
533 EXPECT_NEAR(vp.Acc(6), 0, EPSILON);
534}
535
536TEST(ATrapTest, Test_setProfileStartVelocity5)
537{
538 pilz_industrial_motion_planner::VelocityProfileATrap vp =
539 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
540
541 // acceleration, constant, deceleration
542 vp.setProfileStartVelocity(3, 18, 2);
543 EXPECT_NEAR(vp.Duration(), 6.0, EPSILON);
544 EXPECT_NEAR(vp.firstPhaseDuration(), 1.0, EPSILON);
545 EXPECT_NEAR(vp.secondPhaseDuration(), 1.0, EPSILON);
546 EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON);
547
548 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
549 EXPECT_NEAR(vp.Vel(-1), 2.0, EPSILON);
550 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
551
552 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
553 EXPECT_NEAR(vp.Vel(0), 2.0, EPSILON);
554 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
555
556 EXPECT_NEAR(vp.Pos(1), 6.0, EPSILON);
557 EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON);
558 EXPECT_NEAR(vp.Acc(1), 2.0, EPSILON);
559
560 EXPECT_NEAR(vp.Pos(2), 10, EPSILON);
561 EXPECT_NEAR(vp.Vel(2), 4.0, EPSILON);
562 EXPECT_NEAR(vp.Acc(2), 0.0, EPSILON);
563
564 EXPECT_NEAR(vp.Pos(3), 13.5, EPSILON);
565 EXPECT_NEAR(vp.Vel(3), 3.0, EPSILON);
566 EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON);
567
568 EXPECT_NEAR(vp.Pos(4), 16.0, EPSILON);
569 EXPECT_NEAR(vp.Vel(4), 2.0, EPSILON);
570 EXPECT_NEAR(vp.Acc(4), -1.0, EPSILON);
571
572 EXPECT_NEAR(vp.Pos(6), 18.0, EPSILON);
573 EXPECT_NEAR(vp.Vel(6), 0.0, EPSILON);
574 EXPECT_NEAR(vp.Acc(6), -1.0, EPSILON);
575
576 EXPECT_NEAR(vp.Pos(7), 18, EPSILON);
577 EXPECT_NEAR(vp.Vel(7), 0, EPSILON);
578 EXPECT_NEAR(vp.Acc(7), 0, EPSILON);
579}
580
581TEST(ATrapTest, Test_setProfileStartVelocity6)
582{
583 pilz_industrial_motion_planner::VelocityProfileATrap vp =
584 pilz_industrial_motion_planner::VelocityProfileATrap(4, 2, 1);
585
586 // acceleration, constant, deceleration
587 vp.setProfileStartVelocity(3, 15, 4);
588 EXPECT_NEAR(vp.Duration(), 5.0, EPSILON);
589 EXPECT_NEAR(vp.firstPhaseDuration(), 0.0, EPSILON);
590 EXPECT_NEAR(vp.secondPhaseDuration(), 1.0, EPSILON);
591 EXPECT_NEAR(vp.thirdPhaseDuration(), 4.0, EPSILON);
592
593 EXPECT_NEAR(vp.Pos(-1), 3.0, EPSILON);
594 EXPECT_NEAR(vp.Vel(-1), 4.0, EPSILON);
595 EXPECT_NEAR(vp.Acc(-1), 0.0, EPSILON);
596
597 EXPECT_NEAR(vp.Pos(0), 3.0, EPSILON);
598 EXPECT_NEAR(vp.Vel(0), 4.0, EPSILON);
599 EXPECT_NEAR(vp.Acc(0), 0.0, EPSILON);
600
601 EXPECT_NEAR(vp.Pos(1), 7.0, EPSILON);
602 EXPECT_NEAR(vp.Vel(1), 4.0, EPSILON);
603 EXPECT_NEAR(vp.Acc(1), 0.0, EPSILON);
604
605 EXPECT_NEAR(vp.Pos(3), 13, EPSILON);
606 EXPECT_NEAR(vp.Vel(3), 2.0, EPSILON);
607 EXPECT_NEAR(vp.Acc(3), -1.0, EPSILON);
608}
609
616TEST(ATrapTest, Test_Clone)
617{
618 pilz_industrial_motion_planner::VelocityProfileATrap vp =
619 pilz_industrial_motion_planner::VelocityProfileATrap(4, 1, 1);
620 vp.setProfileAllDurations(0, 10, 10, 10, 10);
621 pilz_industrial_motion_planner::VelocityProfileATrap* vp_clone =
622 static_cast<pilz_industrial_motion_planner::VelocityProfileATrap*>(vp.Clone());
623 EXPECT_EQ(vp, *vp_clone);
624 delete vp_clone;
625}
626
627int main(int argc, char** argv)
628{
629 testing::InitGoogleTest(&argc, argv);
630 return RUN_ALL_TESTS();
631}
pilz_industrial_motion_planner::VelocityProfileATrap
A PTP Trajectory Generator of Asymmetric Trapezoidal Velocity Profile. Differences to VelocityProfile...
Definition velocity_profile_atrap.h:52
pilz_industrial_motion_planner::VelocityProfileATrap::Vel
double Vel(double time) const override
Get velocity at given time.
Definition velocity_profile_atrap.cpp:336
pilz_industrial_motion_planner::VelocityProfileATrap::Pos
double Pos(double time) const override
Get position at given time.
Definition velocity_profile_atrap.cpp:312
pilz_industrial_motion_planner::VelocityProfileATrap::thirdPhaseDuration
double thirdPhaseDuration() const
get the time of third phase
Definition velocity_profile_atrap.h:136
pilz_industrial_motion_planner::VelocityProfileATrap::SetProfile
void SetProfile(double pos1, double pos2) override
compute the fastest profile Algorithm:
Definition velocity_profile_atrap.cpp:61
pilz_industrial_motion_planner::VelocityProfileATrap::setProfileStartVelocity
bool setProfileStartVelocity(double pos1, double pos2, double vel1)
Profile with start velocity Note: This function is not general and is currently only used for live co...
Definition velocity_profile_atrap.cpp:207
pilz_industrial_motion_planner::VelocityProfileATrap::Duration
double Duration() const override
Duration.
Definition velocity_profile_atrap.cpp:307
pilz_industrial_motion_planner::VelocityProfileATrap::secondPhaseDuration
double secondPhaseDuration() const
get the time of second phase
Definition velocity_profile_atrap.h:128
pilz_industrial_motion_planner::VelocityProfileATrap::SetProfileDuration
void SetProfileDuration(double pos1, double pos2, double duration) override
Profile scaled by the total duration.
Definition velocity_profile_atrap.cpp:129
pilz_industrial_motion_planner::VelocityProfileATrap::Clone
KDL::VelocityProfile * Clone() const override
returns copy of current VelocityProfile object
Definition velocity_profile_atrap.cpp:384
pilz_industrial_motion_planner::VelocityProfileATrap::setProfileAllDurations
bool setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, double duration3)
Profile with given acceleration/constant/deceleration durations. Each duration must obey the maximal ...
Definition velocity_profile_atrap.cpp:152
pilz_industrial_motion_planner::VelocityProfileATrap::Acc
double Acc(double time) const override
Get given acceleration/deceleration at given time.
Definition velocity_profile_atrap.cpp:360
pilz_industrial_motion_planner::VelocityProfileATrap::firstPhaseDuration
double firstPhaseDuration() const
get the time of first phase
Definition velocity_profile_atrap.h:120
EPSILON
#define EPSILON
Definition unittest_velocity_profile_atrap.cpp:40
main
int main(int argc, char **argv)
Definition unittest_velocity_profile_atrap.cpp:627
TEST
TEST(ATrapTest, Test_SetProfile1)
Definition unittest_velocity_profile_atrap.cpp:42
velocity_profile_atrap.h
Generated by doxygen 1.9.8