moveit2
The MoveIt Motion Planning Framework for ROS 2.
unittest_joint_limits_validator.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/joint_limits_extension.h>
38 
39 #include <pilz_industrial_motion_planner/joint_limits_validator.h>
40 
41 using namespace pilz_industrial_motion_planner;
42 
43 class JointLimitsValidatorTest : public ::testing::Test
44 {
45 };
46 
50 TEST_F(JointLimitsValidatorTest, CheckPositionEquality)
51 {
52  JointLimitsContainer container;
53 
54  pilz_industrial_motion_planner::JointLimit lim1;
55  lim1.has_position_limits = true;
56  lim1.min_position = -1;
57  lim1.max_position = 1;
58 
59  container.addLimit("joint1", lim1);
60  container.addLimit("joint2", lim1);
61 
62  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
63  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
64  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
65  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
66 }
67 
71 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMinPosition)
72 {
73  JointLimitsContainer container;
74 
75  pilz_industrial_motion_planner::JointLimit lim1;
76  lim1.has_position_limits = true;
77  lim1.min_position = -1;
78  lim1.max_position = 1;
79 
80  pilz_industrial_motion_planner::JointLimit lim2;
81  lim2.has_position_limits = true;
82  lim2.min_position = -2;
83  lim2.max_position = 1;
84 
85  container.addLimit("joint1", lim1);
86  container.addLimit("joint2", lim2);
87 
88  EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container));
89  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
90  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
91  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
92 }
93 
97 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxPosition1)
98 {
99  JointLimitsContainer container;
100 
101  pilz_industrial_motion_planner::JointLimit lim1;
102  lim1.has_position_limits = true;
103  lim1.min_position = -1;
104  lim1.max_position = 2;
105 
106  pilz_industrial_motion_planner::JointLimit lim2;
107  lim2.has_position_limits = true;
108  lim2.min_position = -1;
109  lim2.max_position = 1;
110 
111  container.addLimit("joint1", lim1);
112  container.addLimit("joint2", lim2);
113 
114  EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container));
115  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
116  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
117  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
118 }
119 
123 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxPosition2)
124 {
125  JointLimitsContainer container;
126 
127  pilz_industrial_motion_planner::JointLimit lim1;
128  lim1.has_position_limits = true;
129  lim1.min_position = -1;
130  lim1.max_position = 2;
131 
132  pilz_industrial_motion_planner::JointLimit lim2;
133  lim2.has_position_limits = true;
134  lim2.min_position = -1;
135  lim2.max_position = 1;
136 
137  pilz_industrial_motion_planner::JointLimit lim3;
138  lim3.has_position_limits = true;
139  lim3.min_position = -1;
140  lim3.max_position = 1;
141 
142  container.addLimit("joint1", lim1);
143  container.addLimit("joint2", lim2);
144  container.addLimit("joint3", lim3);
145 
146  EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container));
147  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
148  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
149  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
150 }
151 
155 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasPositionLimits)
156 {
157  JointLimitsContainer container;
158 
159  pilz_industrial_motion_planner::JointLimit lim1;
160  lim1.has_position_limits = true;
161  lim1.min_position = -1;
162  lim1.max_position = 1;
163 
164  pilz_industrial_motion_planner::JointLimit lim2;
165  lim2.has_position_limits = false;
166  lim2.min_position = -1;
167  lim2.max_position = 1;
168 
169  container.addLimit("joint1", lim1);
170  container.addLimit("joint2", lim2);
171 
172  EXPECT_EQ(false, JointLimitsValidator::validateAllPositionLimitsEqual(container));
173  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
174  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
175  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
176 }
177 
178 //----------------------------------------------------------------------------------
179 //----------------------------------------------------------------------------------
180 // VELOCITY
181 //----------------------------------------------------------------------------------
182 //----------------------------------------------------------------------------------
183 
187 TEST_F(JointLimitsValidatorTest, CheckVelocityEquality)
188 {
189  JointLimitsContainer container;
190 
191  pilz_industrial_motion_planner::JointLimit lim1;
192  lim1.has_velocity_limits = true;
193  lim1.max_velocity = 1;
194 
195  container.addLimit("joint1", lim1);
196  container.addLimit("joint2", lim1);
197 
198  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
199  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
200  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
201  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
202 }
203 
207 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxVelocity1)
208 {
209  JointLimitsContainer container;
210 
211  pilz_industrial_motion_planner::JointLimit lim1;
212  lim1.has_velocity_limits = true;
213  lim1.max_velocity = 1;
214 
215  pilz_industrial_motion_planner::JointLimit lim2;
216  lim2.has_velocity_limits = true;
217  lim2.max_velocity = 2;
218 
219  container.addLimit("joint1", lim1);
220  container.addLimit("joint2", lim2);
221 
222  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
223  EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
224  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
225  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
226 }
227 
231 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxVelocity2)
232 {
233  JointLimitsContainer container;
234 
235  pilz_industrial_motion_planner::JointLimit lim1;
236  lim1.has_velocity_limits = true;
237  lim1.max_velocity = 1;
238 
239  pilz_industrial_motion_planner::JointLimit lim2;
240  lim2.has_velocity_limits = true;
241  lim2.max_velocity = 2;
242 
243  pilz_industrial_motion_planner::JointLimit lim3;
244  lim3.has_velocity_limits = true;
245  lim3.max_velocity = 2;
246 
247  container.addLimit("joint1", lim1);
248  container.addLimit("joint2", lim2);
249  container.addLimit("joint3", lim3);
250 
251  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
252  EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
253  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
254  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
255 }
256 
260 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasVelocityLimits)
261 {
262  JointLimitsContainer container;
263 
264  pilz_industrial_motion_planner::JointLimit lim1;
265  lim1.has_velocity_limits = true;
266 
267  pilz_industrial_motion_planner::JointLimit lim2;
268  lim2.has_velocity_limits = false;
269 
270  container.addLimit("joint1", lim1);
271  container.addLimit("joint2", lim2);
272 
273  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
274  EXPECT_EQ(false, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
275  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
276  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
277 }
278 
279 //----------------------------------------------------------------------------------
280 //----------------------------------------------------------------------------------
281 // ACCELERATION
282 //----------------------------------------------------------------------------------
283 //----------------------------------------------------------------------------------
284 
288 TEST_F(JointLimitsValidatorTest, CheckAccelerationEquality)
289 {
290  JointLimitsContainer container;
291 
292  pilz_industrial_motion_planner::JointLimit lim1;
293  lim1.has_acceleration_limits = true;
294  lim1.max_acceleration = 1;
295 
296  container.addLimit("joint1", lim1);
297  container.addLimit("joint2", lim1);
298 
299  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
300  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
301  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
302  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
303 }
304 
308 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxAcceleration1)
309 {
310  JointLimitsContainer container;
311 
312  pilz_industrial_motion_planner::JointLimit lim1;
313  lim1.has_acceleration_limits = true;
314  lim1.max_acceleration = 1;
315 
316  pilz_industrial_motion_planner::JointLimit lim2;
317  lim2.has_acceleration_limits = true;
318  lim2.max_acceleration = 2;
319 
320  container.addLimit("joint1", lim1);
321  container.addLimit("joint2", lim2);
322 
323  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
324  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
325  EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
326  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
327 }
328 
332 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityMaxAcceleration2)
333 {
334  JointLimitsContainer container;
335 
336  pilz_industrial_motion_planner::JointLimit lim1;
337  lim1.has_acceleration_limits = true;
338  lim1.max_acceleration = 1;
339 
340  pilz_industrial_motion_planner::JointLimit lim2;
341  lim2.has_acceleration_limits = true;
342  lim2.max_acceleration = 2;
343 
344  pilz_industrial_motion_planner::JointLimit lim3;
345  lim3.has_acceleration_limits = true;
346  lim3.max_acceleration = 2;
347 
348  container.addLimit("joint1", lim1);
349  container.addLimit("joint2", lim2);
350  container.addLimit("joint3", lim3);
351 
352  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
353  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
354  EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
355  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
356 }
357 
362 TEST_F(JointLimitsValidatorTest, CheckPositionInEqualityHasAccelerationLimits)
363 {
364  JointLimitsContainer container;
365 
366  pilz_industrial_motion_planner::JointLimit lim1;
367  lim1.has_acceleration_limits = true;
368 
369  pilz_industrial_motion_planner::JointLimit lim2;
370  lim2.has_acceleration_limits = false;
371 
372  container.addLimit("joint1", lim1);
373  container.addLimit("joint2", lim2);
374 
375  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
376  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
377  EXPECT_EQ(false, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
378  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
379 }
380 
381 //----------------------------------------------------------------------------------
382 //----------------------------------------------------------------------------------
383 // DECELERATION
384 //----------------------------------------------------------------------------------
385 //----------------------------------------------------------------------------------
386 
390 TEST_F(JointLimitsValidatorTest, CheckDecelerationEquality)
391 {
392  JointLimitsContainer container;
393 
394  pilz_industrial_motion_planner::JointLimit lim1;
395  lim1.has_deceleration_limits = true;
396  lim1.max_deceleration = 1;
397 
398  container.addLimit("joint1", lim1);
399  container.addLimit("joint2", lim1);
400 
401  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
402  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
403  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
404  EXPECT_EQ(true, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
405 }
406 
410 TEST_F(JointLimitsValidatorTest, CheckInEqualityMaxDeceleration1)
411 {
412  JointLimitsContainer container;
413 
414  pilz_industrial_motion_planner::JointLimit lim1;
415  lim1.has_deceleration_limits = true;
416  lim1.max_deceleration = -1;
417 
418  pilz_industrial_motion_planner::JointLimit lim2;
419  lim2.has_deceleration_limits = true;
420  lim2.max_deceleration = -2;
421 
422  container.addLimit("joint1", lim1);
423  container.addLimit("joint2", lim2);
424 
425  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
426  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
427  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
428  EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
429 }
430 
434 TEST_F(JointLimitsValidatorTest, CheckInEqualityMaxDeceleration2)
435 {
436  JointLimitsContainer container;
437 
438  pilz_industrial_motion_planner::JointLimit lim1;
439  lim1.has_deceleration_limits = true;
440  lim1.max_deceleration = -1;
441 
442  pilz_industrial_motion_planner::JointLimit lim2;
443  lim2.has_deceleration_limits = true;
444  lim2.max_deceleration = -2;
445 
446  pilz_industrial_motion_planner::JointLimit lim3;
447  lim3.has_deceleration_limits = true;
448  lim3.max_deceleration = -2;
449 
450  container.addLimit("joint1", lim1);
451  container.addLimit("joint2", lim2);
452  container.addLimit("joint3", lim3);
453 
454  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
455  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
456  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
457  EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
458 }
459 
464 TEST_F(JointLimitsValidatorTest, CheckInEqualityHasDecelerationLimits)
465 {
466  JointLimitsContainer container;
467 
468  pilz_industrial_motion_planner::JointLimit lim1;
469  lim1.has_deceleration_limits = true;
470  lim1.max_deceleration = -1;
471 
472  pilz_industrial_motion_planner::JointLimit lim2;
473  lim2.has_deceleration_limits = false;
474 
475  container.addLimit("joint1", lim1);
476  container.addLimit("joint2", lim2);
477 
478  ASSERT_EQ(2u, container.getCount());
479 
480  EXPECT_EQ(true, JointLimitsValidator::validateAllPositionLimitsEqual(container));
481  EXPECT_EQ(true, JointLimitsValidator::validateAllVelocityLimitsEqual(container));
482  EXPECT_EQ(true, JointLimitsValidator::validateAllAccelerationLimitsEqual(container));
483  EXPECT_EQ(false, JointLimitsValidator::validateAllDecelerationLimitsEqual(container));
484 }
485 
486 int main(int argc, char** argv)
487 {
488  testing::InitGoogleTest(&argc, argv);
489  return RUN_ALL_TESTS();
490 }
pilz_industrial_motion_planner::JointLimitsContainer
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
Definition: joint_limits_container.h:52
pilz_industrial_motion_planner::JointLimitsContainer::addLimit
bool addLimit(const std::string &joint_name, JointLimit joint_limit)
Add a limit.
Definition: joint_limits_container.cpp:52
pilz_industrial_motion_planner::JointLimitsContainer::getCount
size_t getCount() const
Get Number of limits in the container.
Definition: joint_limits_container.cpp:74
pilz_industrial_motion_planner::JointLimitsValidator::validateAllPositionLimitsEqual
static bool validateAllPositionLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
Validates that the position limits of all limits are equal.
Definition: joint_limits_validator.cpp:43
pilz_industrial_motion_planner::JointLimitsValidator::validateAllDecelerationLimitsEqual
static bool validateAllDecelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
Validates that the deceleration of all limits is equal.
Definition: joint_limits_validator.cpp:61
pilz_industrial_motion_planner::JointLimitsValidator::validateAllVelocityLimitsEqual
static bool validateAllVelocityLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
Validates that the velocity of all limits is equal.
Definition: joint_limits_validator.cpp:49
pilz_industrial_motion_planner::JointLimitsValidator::validateAllAccelerationLimitsEqual
static bool validateAllAccelerationLimitsEqual(const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
Validates that the acceleration of all limits is equal.
Definition: joint_limits_validator.cpp:55
joint_limits_extension.h
joint_limits_validator.h
pilz_industrial_motion_planner
Definition: capability_names.h:40
pilz_industrial_motion_planner::TEST_F
TEST_F(GetSolverTipFrameIntegrationTest, TestHasSolverManipulator)
Check if hasSolver() can be called successfully for the manipulator group.
Definition: integrationtest_get_solver_tip_frame.cpp:76
joint_limits::JointLimits::max_velocity
double max_velocity
Definition: joint_limits.hpp:49
joint_limits::JointLimits::has_velocity_limits
bool has_velocity_limits
Definition: joint_limits.hpp:55
joint_limits::JointLimits::has_acceleration_limits
bool has_acceleration_limits
Definition: joint_limits.hpp:56
joint_limits::JointLimits::max_position
double max_position
Definition: joint_limits.hpp:48
joint_limits::JointLimits::max_acceleration
double max_acceleration
Definition: joint_limits.hpp:50
joint_limits::JointLimits::min_position
double min_position
Definition: joint_limits.hpp:47
joint_limits::JointLimits::has_position_limits
bool has_position_limits
Definition: joint_limits.hpp:54
pilz_industrial_motion_planner::joint_limits_interface::JointLimits
Extends joint_limits_interface::JointLimits with a deceleration parameter.
Definition: joint_limits_extension.h:53
pilz_industrial_motion_planner::joint_limits_interface::JointLimits::has_deceleration_limits
bool has_deceleration_limits
Definition: joint_limits_extension.h:61
pilz_industrial_motion_planner::joint_limits_interface::JointLimits::max_deceleration
double max_deceleration
maximum deceleration MUST(!) be negative
Definition: joint_limits_extension.h:59
main
int main(int argc, char **argv)
Definition: unittest_joint_limits_validator.cpp:486
Generated by doxygen 1.9.1