moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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
41using namespace pilz_industrial_motion_planner;
42
43class JointLimitsValidatorTest : public ::testing::Test
44{
45};
46
50TEST_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
71TEST_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
97TEST_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
123TEST_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
155TEST_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
187TEST_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
207TEST_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
231TEST_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
260TEST_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
288TEST_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
308TEST_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
332TEST_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
362TEST_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
390TEST_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
410TEST_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
434TEST_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
464TEST_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
486int 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.8