moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
42 TEST(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 
89 TEST(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 
128 TEST(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 
163 TEST(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 
196 TEST(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 
223 TEST(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 
249 TEST(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 
261 TEST(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 
308 TEST(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 
355 TEST(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 
368 TEST(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 
405 TEST(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 
454 TEST(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 
495 TEST(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 
536 TEST(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 
581 TEST(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 
616 TEST(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 
627 int 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.1