moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_constraints.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage 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 /* Author: Ioan Sucan, E. Gil Jones */
36 
38 #include <gtest/gtest.h>
39 #include <urdf_parser/urdf_parser.h>
40 #include <fstream>
41 #include <tf2_eigen/tf2_eigen.hpp>
42 #include <math.h>
44 
45 class LoadPlanningModelsPr2 : public testing::Test
46 {
47 protected:
48  void SetUp() override
49  {
51  }
52 
53  void TearDown() override
54  {
55  }
56 
57 protected:
58  moveit::core::RobotModelPtr robot_model_;
59 };
60 
61 TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
62 {
63  moveit::core::RobotState robot_state(robot_model_);
64  robot_state.setToDefaultValues();
65  moveit::core::Transforms tf(robot_model_->getModelFrame());
66 
68  moveit_msgs::msg::JointConstraint jcm;
69  jcm.joint_name = "head_pan_joint";
70  jcm.position = 0.4;
71  jcm.tolerance_above = 0.1;
72  jcm.tolerance_below = 0.05;
73 
74  EXPECT_TRUE(jc.configure(jcm));
75  // weight should have been changed to 1.0
76  EXPECT_NEAR(jc.getConstraintWeight(), 1.0, std::numeric_limits<double>::epsilon());
77 
78  // tests that the default state is outside the bounds
79  // given that the default state is at 0.0
80  EXPECT_TRUE(jc.configure(jcm));
82  EXPECT_FALSE(p1.satisfied);
83  EXPECT_NEAR(p1.distance, jcm.position, 1e-6);
84 
85  // tests that when we set the state within the bounds
86  // the constraint is satisfied
87  double jval = 0.41;
88  robot_state.setJointPositions(jcm.joint_name, &jval);
90  EXPECT_TRUE(p2.satisfied);
91  EXPECT_NEAR(p2.distance, 0.01, 1e-6);
92 
93  // exactly equal to the low bound is fine too
94  jval = 0.35;
95  robot_state.setJointPositions(jcm.joint_name, &jval);
96  EXPECT_TRUE(jc.decide(robot_state).satisfied);
97 
98  // and so is less than epsilon when there's no other source of error
99  // jvals[jcm.joint_name] = 0.35-std::numeric_limits<double>::epsilon();
100  jval = 0.35 - std::numeric_limits<double>::epsilon();
101 
102  robot_state.setJointPositions(jcm.joint_name, &jval);
103  EXPECT_TRUE(jc.decide(robot_state).satisfied);
104 
105  // but this is too much
106  jval = 0.35 - 3 * std::numeric_limits<double>::epsilon();
107  robot_state.setJointPositions(jcm.joint_name, &jval);
108  EXPECT_FALSE(jc.decide(robot_state).satisfied);
109 
110  // negative value makes configuration fail
111  jcm.tolerance_below = -0.05;
112  EXPECT_FALSE(jc.configure(jcm));
113 
114  jcm.tolerance_below = 0.05;
115  EXPECT_TRUE(jc.configure(jcm));
116 
117  // still satisfied at a slightly different state
118  jval = 0.46;
119  robot_state.setJointPositions(jcm.joint_name, &jval);
120  EXPECT_TRUE(jc.decide(robot_state).satisfied);
121 
122  // still satisfied at a slightly different state
123  jval = 0.501;
124  robot_state.setJointPositions(jcm.joint_name, &jval);
125  EXPECT_FALSE(jc.decide(robot_state).satisfied);
126 
127  // still satisfied at a slightly different state
128  jval = 0.39;
129  robot_state.setJointPositions(jcm.joint_name, &jval);
130  EXPECT_TRUE(jc.decide(robot_state).satisfied);
131 
132  // outside the bounds
133  jval = 0.34;
134  robot_state.setJointPositions(jcm.joint_name, &jval);
135  EXPECT_FALSE(jc.decide(robot_state).satisfied);
136 
137  // testing equality
138  kinematic_constraints::JointConstraint jc2(robot_model_);
139  EXPECT_TRUE(jc2.configure(jcm));
140  EXPECT_TRUE(jc2.enabled());
141  EXPECT_TRUE(jc.equal(jc2, 1e-12));
142 
143  // if name not equal, not equal
144  jcm.joint_name = "head_tilt_joint";
145  EXPECT_TRUE(jc2.configure(jcm));
146  EXPECT_FALSE(jc.equal(jc2, 1e-12));
147 
148  // if different, test margin behavior
149  jcm.joint_name = "head_pan_joint";
150  jcm.position = 0.3;
151  EXPECT_TRUE(jc2.configure(jcm));
152  EXPECT_FALSE(jc.equal(jc2, 1e-12));
153  // exactly equal is still false
154  EXPECT_FALSE(jc.equal(jc2, .1));
155  EXPECT_TRUE(jc.equal(jc2, .101));
156 
157  // no name makes this false
158  jcm.joint_name = "";
159  jcm.position = 0.4;
160  EXPECT_FALSE(jc2.configure(jcm));
161  EXPECT_FALSE(jc2.enabled());
162  EXPECT_FALSE(jc.equal(jc2, 1e-12));
163 
164  // no DOF makes this false
165  jcm.joint_name = "base_footprint_joint";
166  EXPECT_FALSE(jc2.configure(jcm));
167 
168  // clear means not enabled
169  jcm.joint_name = "head_pan_joint";
170  EXPECT_TRUE(jc2.configure(jcm));
171  jc2.clear();
172  EXPECT_FALSE(jc2.enabled());
173  EXPECT_FALSE(jc.equal(jc2, 1e-12));
174 }
175 
176 TEST_F(LoadPlanningModelsPr2, JointConstraintsCont)
177 {
178  moveit::core::RobotState robot_state(robot_model_);
179  robot_state.setToDefaultValues();
180  robot_state.update();
181  moveit::core::Transforms tf(robot_model_->getModelFrame());
182 
184  moveit_msgs::msg::JointConstraint jcm;
185 
186  jcm.joint_name = "l_wrist_roll_joint";
187  jcm.position = 0.0;
188  jcm.tolerance_above = 0.04;
189  jcm.tolerance_below = 0.02;
190  jcm.weight = 1.0;
191 
192  EXPECT_TRUE(jc.configure(jcm));
193 
194  std::map<std::string, double> jvals;
195 
196  // state should have zeros, and work
197  EXPECT_TRUE(jc.decide(robot_state).satisfied);
198 
199  // within the above tolerance
200  jvals[jcm.joint_name] = .03;
201  robot_state.setVariablePositions(jvals);
202  robot_state.update();
203  EXPECT_TRUE(jc.decide(robot_state).satisfied);
204 
205  // outside the above tolerance
206  jvals[jcm.joint_name] = .05;
207  robot_state.setVariablePositions(jvals);
208  robot_state.update();
209  EXPECT_FALSE(jc.decide(robot_state).satisfied);
210 
211  // inside the below tolerance
212  jvals[jcm.joint_name] = -.01;
213  robot_state.setVariablePositions(jvals);
214  EXPECT_TRUE(jc.decide(robot_state).satisfied);
215 
216  // outside the below tolerance
217  jvals[jcm.joint_name] = -.03;
218  robot_state.setVariablePositions(jvals);
219  EXPECT_FALSE(jc.decide(robot_state).satisfied);
220 
221  // now testing wrap around from positive to negative
222  jcm.position = 3.14;
223  EXPECT_TRUE(jc.configure(jcm));
224 
225  // testing that wrap works
226  jvals[jcm.joint_name] = 3.17;
227  robot_state.setVariablePositions(jvals);
229  EXPECT_TRUE(p1.satisfied);
230  EXPECT_NEAR(p1.distance, 0.03, 1e-6);
231 
232  // testing that negative wrap works
233  jvals[jcm.joint_name] = -3.14;
234  robot_state.setVariablePositions(jvals);
236  EXPECT_TRUE(p2.satisfied);
237  EXPECT_NEAR(p2.distance, 0.003185, 1e-4);
238 
239  // over bound testing
240  jvals[jcm.joint_name] = 3.19;
241  robot_state.setVariablePositions(jvals);
242  EXPECT_FALSE(jc.decide(robot_state).satisfied);
243 
244  // reverses to other direction
245  // but still tested using above tolerance
246  jvals[jcm.joint_name] = -3.11;
247  robot_state.setVariablePositions(jvals);
248  EXPECT_TRUE(jc.decide(robot_state).satisfied);
249 
250  // outside of the bound given the wrap
251  jvals[jcm.joint_name] = -3.09;
252  robot_state.setVariablePositions(jvals);
253  EXPECT_FALSE(jc.decide(robot_state).satisfied);
254 
255  // lower tolerance testing
256  // within bounds
257  jvals[jcm.joint_name] = 3.13;
258  robot_state.setVariablePositions(jvals);
259  EXPECT_TRUE(jc.decide(robot_state).satisfied);
260 
261  // within outside
262  jvals[jcm.joint_name] = 3.11;
263  robot_state.setVariablePositions(jvals);
264  EXPECT_FALSE(jc.decide(robot_state).satisfied);
265 
266  // testing the other direction
267  jcm.position = -3.14;
268  EXPECT_TRUE(jc.configure(jcm));
269 
270  // should be governed by above tolerance
271  jvals[jcm.joint_name] = -3.11;
272  robot_state.setVariablePositions(jvals);
273  EXPECT_TRUE(jc.decide(robot_state).satisfied);
274 
275  // outside upper bound
276  jvals[jcm.joint_name] = -3.09;
277  robot_state.setVariablePositions(jvals);
278  EXPECT_FALSE(jc.decide(robot_state).satisfied);
279 
280  // governed by lower bound
281  jvals[jcm.joint_name] = 3.13;
282  robot_state.setVariablePositions(jvals);
283  EXPECT_TRUE(jc.decide(robot_state).satisfied);
284 
285  // outside lower bound (but would be inside upper)
286  jvals[jcm.joint_name] = 3.12;
287  robot_state.setVariablePositions(jvals);
288  EXPECT_TRUE(jc.decide(robot_state).satisfied);
289 
290  // testing wrap
291  jcm.position = 6.28;
292  EXPECT_TRUE(jc.configure(jcm));
293 
294  // should wrap to zero
295  jvals[jcm.joint_name] = 0.0;
296  robot_state.setVariablePositions(jvals);
297  EXPECT_TRUE(jc.decide(robot_state).satisfied);
298 
299  // should wrap to close and test to be near
300  moveit_msgs::msg::JointConstraint jcm2 = jcm;
301  jcm2.position = -6.28;
302  kinematic_constraints::JointConstraint jc2(robot_model_);
303  EXPECT_TRUE(jc.configure(jcm2));
304  jc.equal(jc2, .02);
305 }
306 
307 TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF)
308 {
309  moveit::core::RobotState robot_state(robot_model_);
310  robot_state.setToDefaultValues();
311 
313  moveit_msgs::msg::JointConstraint jcm;
314  jcm.joint_name = "world_joint";
315  jcm.position = 3.14;
316  jcm.tolerance_above = 0.1;
317  jcm.tolerance_below = 0.05;
318  jcm.weight = 1.0;
319 
320  // shouldn't work for multi-dof without local name
321  EXPECT_FALSE(jc.configure(jcm));
322 
323  // this should, and function like any other single joint constraint
324  jcm.joint_name = "world_joint/x";
325  EXPECT_TRUE(jc.configure(jcm));
326 
327  std::map<std::string, double> jvals;
328  jvals[jcm.joint_name] = 3.2;
329  robot_state.setVariablePositions(jvals);
331  EXPECT_TRUE(p1.satisfied);
332 
333  jvals[jcm.joint_name] = 3.25;
334  robot_state.setVariablePositions(jvals);
336  EXPECT_FALSE(p2.satisfied);
337 
338  jvals[jcm.joint_name] = -3.14;
339  robot_state.setVariablePositions(jvals);
341  EXPECT_FALSE(p3.satisfied);
342 
343  // theta is continuous
344  jcm.joint_name = "world_joint/theta";
345  EXPECT_TRUE(jc.configure(jcm));
346 
347  jvals[jcm.joint_name] = -3.14;
348  robot_state.setVariablePositions(jvals);
350  EXPECT_TRUE(p4.satisfied);
351 
352  jvals[jcm.joint_name] = 3.25;
353  robot_state.setVariablePositions(jvals);
355  EXPECT_FALSE(p5.satisfied);
356 }
357 
358 TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed)
359 {
360  moveit::core::RobotState robot_state(robot_model_);
361  robot_state.setToDefaultValues();
362  robot_state.update(true);
363  moveit::core::Transforms tf(robot_model_->getModelFrame());
365  moveit_msgs::msg::PositionConstraint pcm;
366 
367  // empty certainly means false
368  EXPECT_FALSE(pc.configure(pcm, tf));
369 
370  pcm.link_name = "l_wrist_roll_link";
371  pcm.target_point_offset.x = 0;
372  pcm.target_point_offset.y = 0;
373  pcm.target_point_offset.z = 0;
374  pcm.constraint_region.primitives.resize(1);
375  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
376 
377  // no dimensions, so no valid regions
378  EXPECT_FALSE(pc.configure(pcm, tf));
379 
380  pcm.constraint_region.primitives[0].dimensions.resize(1);
381  pcm.constraint_region.primitives[0].dimensions[0] = 0.2;
382 
383  // no pose, so no valid region
384  EXPECT_FALSE(pc.configure(pcm, tf));
385 
386  pcm.constraint_region.primitive_poses.resize(1);
387  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
388  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
389  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
390  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
391  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
392  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
393  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
394  pcm.weight = 1.0;
395 
396  // intentionally leaving header frame blank to test behavior
397  EXPECT_FALSE(pc.configure(pcm, tf));
398 
399  pcm.header.frame_id = robot_model_->getModelFrame();
400  EXPECT_TRUE(pc.configure(pcm, tf));
401  EXPECT_FALSE(pc.mobileReferenceFrame());
402 
403  EXPECT_TRUE(pc.decide(robot_state).satisfied);
404 
405  std::map<std::string, double> jvals;
406  jvals["torso_lift_joint"] = 0.4;
407  robot_state.setVariablePositions(jvals);
408  robot_state.update();
409  EXPECT_FALSE(pc.decide(robot_state).satisfied);
410  EXPECT_TRUE(pc.equal(pc, 1e-12));
411 
412  // arbitrary offset that puts it back into the pose range
413  pcm.target_point_offset.x = 0;
414  pcm.target_point_offset.y = 0;
415  pcm.target_point_offset.z = .15;
416 
417  EXPECT_TRUE(pc.configure(pcm, tf));
418  EXPECT_TRUE(pc.hasLinkOffset());
419  EXPECT_TRUE(pc.decide(robot_state).satisfied);
420 
421  pc.clear();
422  EXPECT_FALSE(pc.enabled());
423 
424  // invalid quaternion results in zero quaternion
425  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
426  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
427  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
428  pcm.constraint_region.primitive_poses[0].orientation.w = 0.0;
429 
430  EXPECT_TRUE(pc.configure(pcm, tf));
431  EXPECT_TRUE(pc.decide(robot_state).satisfied);
432 }
433 
434 TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile)
435 {
436  moveit::core::RobotState robot_state(robot_model_);
437  robot_state.setToDefaultValues();
438  moveit::core::Transforms tf(robot_model_->getModelFrame());
439  robot_state.update();
440 
442  moveit_msgs::msg::PositionConstraint pcm;
443 
444  pcm.link_name = "l_wrist_roll_link";
445  pcm.target_point_offset.x = 0;
446  pcm.target_point_offset.y = 0;
447  pcm.target_point_offset.z = 0;
448 
449  pcm.constraint_region.primitives.resize(1);
450  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
451  pcm.constraint_region.primitives[0].dimensions.resize(1);
452  pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.38 * 2.0;
453 
454  pcm.header.frame_id = "r_wrist_roll_link";
455 
456  pcm.constraint_region.primitive_poses.resize(1);
457  pcm.constraint_region.primitive_poses[0].position.x = 0.0;
458  pcm.constraint_region.primitive_poses[0].position.y = 0.6;
459  pcm.constraint_region.primitive_poses[0].position.z = 0.0;
460  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
461  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
462  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
463  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
464  pcm.weight = 1.0;
465 
466  EXPECT_FALSE(tf.isFixedFrame(pcm.link_name));
467  EXPECT_TRUE(pc.configure(pcm, tf));
468  EXPECT_TRUE(pc.mobileReferenceFrame());
469 
470  EXPECT_TRUE(pc.decide(robot_state).satisfied);
471 
472  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
473  pcm.constraint_region.primitives[0].dimensions.resize(3);
474  pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.1;
475  pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 0.1;
476  pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 0.1;
477  EXPECT_TRUE(pc.configure(pcm, tf));
478 
479  std::map<std::string, double> jvals;
480  jvals["l_shoulder_pan_joint"] = 0.4;
481  robot_state.setVariablePositions(jvals);
482  robot_state.update();
483  EXPECT_TRUE(pc.decide(robot_state).satisfied);
484  EXPECT_TRUE(pc.equal(pc, 1e-12));
485 
486  jvals["l_shoulder_pan_joint"] = -0.4;
487  robot_state.setVariablePositions(jvals);
488  robot_state.update();
489  EXPECT_FALSE(pc.decide(robot_state).satisfied);
490 
491  // adding a second constrained region makes this work
492  pcm.constraint_region.primitive_poses.resize(2);
493  pcm.constraint_region.primitive_poses[1].position.x = 0.0;
494  pcm.constraint_region.primitive_poses[1].position.y = 0.1;
495  pcm.constraint_region.primitive_poses[1].position.z = 0.0;
496  pcm.constraint_region.primitive_poses[1].orientation.x = 0.0;
497  pcm.constraint_region.primitive_poses[1].orientation.y = 0.0;
498  pcm.constraint_region.primitive_poses[1].orientation.z = 0.0;
499  pcm.constraint_region.primitive_poses[1].orientation.w = 1.0;
500 
501  pcm.constraint_region.primitives.resize(2);
502  pcm.constraint_region.primitives[1].type = shape_msgs::msg::SolidPrimitive::BOX;
503  pcm.constraint_region.primitives[1].dimensions.resize(3);
504  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.1;
505  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 0.1;
506  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 0.1;
507  EXPECT_TRUE(pc.configure(pcm, tf));
508  EXPECT_TRUE(pc.decide(robot_state, false).satisfied);
509 }
510 
511 TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality)
512 {
513  moveit::core::RobotState robot_state(robot_model_);
514  robot_state.setToDefaultValues();
515  moveit::core::Transforms tf(robot_model_->getModelFrame());
516 
519  moveit_msgs::msg::PositionConstraint pcm;
520 
521  pcm.link_name = "l_wrist_roll_link";
522  pcm.target_point_offset.x = 0;
523  pcm.target_point_offset.y = 0;
524  pcm.target_point_offset.z = 0;
525 
526  pcm.constraint_region.primitives.resize(2);
527  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
528  pcm.constraint_region.primitives[0].dimensions.resize(1);
529  pcm.constraint_region.primitives[0].dimensions[0] = 0.38 * 2.0;
530  pcm.constraint_region.primitives[1].type = shape_msgs::msg::SolidPrimitive::BOX;
531  pcm.constraint_region.primitives[1].dimensions.resize(3);
532  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 2.0;
533  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 2.0;
534  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 2.0;
535 
536  pcm.header.frame_id = "r_wrist_roll_link";
537  pcm.constraint_region.primitive_poses.resize(2);
538  pcm.constraint_region.primitive_poses[0].position.x = 0.0;
539  pcm.constraint_region.primitive_poses[0].position.y = 0.6;
540  pcm.constraint_region.primitive_poses[0].position.z = 0.0;
541  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
542  pcm.constraint_region.primitive_poses[1].position.x = 2.0;
543  pcm.constraint_region.primitive_poses[1].position.y = 0.0;
544  pcm.constraint_region.primitive_poses[1].position.z = 0.0;
545  pcm.constraint_region.primitive_poses[1].orientation.w = 1.0;
546  pcm.weight = 1.0;
547 
548  EXPECT_TRUE(pc.configure(pcm, tf));
549  EXPECT_TRUE(pc2.configure(pcm, tf));
550 
551  EXPECT_TRUE(pc.equal(pc2, .001));
552  EXPECT_TRUE(pc2.equal(pc, .001));
553 
554  // putting regions in different order
555  moveit_msgs::msg::PositionConstraint pcm2 = pcm;
556  pcm2.constraint_region.primitives[0] = pcm.constraint_region.primitives[1];
557  pcm2.constraint_region.primitives[1] = pcm.constraint_region.primitives[0];
558 
559  pcm2.constraint_region.primitive_poses[0] = pcm.constraint_region.primitive_poses[1];
560  pcm2.constraint_region.primitive_poses[1] = pcm.constraint_region.primitive_poses[0];
561 
562  EXPECT_TRUE(pc2.configure(pcm2, tf));
563  EXPECT_TRUE(pc.equal(pc2, .001));
564  EXPECT_TRUE(pc2.equal(pc, .001));
565 
566  // messing with one value breaks it
567  pcm2.constraint_region.primitive_poses[0].position.z = .01;
568  EXPECT_TRUE(pc2.configure(pcm2, tf));
569  EXPECT_FALSE(pc.equal(pc2, .001));
570  EXPECT_FALSE(pc2.equal(pc, .001));
571  EXPECT_TRUE(pc.equal(pc2, .1));
572  EXPECT_TRUE(pc2.equal(pc, .1));
573 
574  // adding an identical third shape to the last one is ok
575  pcm2.constraint_region.primitive_poses[0].position.z = 0.0;
576  pcm2.constraint_region.primitives.resize(3);
577  pcm2.constraint_region.primitive_poses.resize(3);
578  pcm2.constraint_region.primitives[2] = pcm2.constraint_region.primitives[0];
579  pcm2.constraint_region.primitive_poses[2] = pcm2.constraint_region.primitive_poses[0];
580  EXPECT_TRUE(pc2.configure(pcm2, tf));
581  EXPECT_TRUE(pc.equal(pc2, .001));
582  EXPECT_TRUE(pc2.equal(pc, .001));
583 
584  // but if we change it, it's not
585  pcm2.constraint_region.primitives[2].dimensions[0] = 3.0;
586  EXPECT_TRUE(pc2.configure(pcm2, tf));
587  EXPECT_FALSE(pc.equal(pc2, .001));
588  EXPECT_FALSE(pc2.equal(pc, .001));
589 
590  // changing the shape also changes it
591  pcm2.constraint_region.primitives[2].dimensions[0] = pcm2.constraint_region.primitives[0].dimensions[0];
592  pcm2.constraint_region.primitives[2].type = shape_msgs::msg::SolidPrimitive::SPHERE;
593  EXPECT_TRUE(pc2.configure(pcm2, tf));
594  EXPECT_FALSE(pc.equal(pc2, .001));
595 }
596 
597 TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)
598 {
599  moveit::core::RobotState robot_state(robot_model_);
600  robot_state.setToDefaultValues();
601  robot_state.update();
602  moveit::core::Transforms tf(robot_model_->getModelFrame());
603 
605 
606  moveit_msgs::msg::OrientationConstraint ocm;
607 
608  EXPECT_FALSE(oc.configure(ocm, tf));
609 
610  ocm.link_name = "r_wrist_roll_link";
611 
612  // all we currently have to specify is the link name to get a valid constraint
613  EXPECT_TRUE(oc.configure(ocm, tf));
614 
615  ocm.header.frame_id = robot_model_->getModelFrame();
616  ocm.orientation.x = 0.0;
617  ocm.orientation.y = 0.0;
618  ocm.orientation.z = 0.0;
619  ocm.orientation.w = 1.0;
620  ocm.absolute_x_axis_tolerance = 0.1;
621  ocm.absolute_y_axis_tolerance = 0.1;
622  ocm.absolute_z_axis_tolerance = 0.1;
623  ocm.weight = 1.0;
624 
625  EXPECT_TRUE(oc.configure(ocm, tf));
626  EXPECT_FALSE(oc.mobileReferenceFrame());
627 
628  EXPECT_FALSE(oc.decide(robot_state).satisfied);
629 
630  ocm.header.frame_id = ocm.link_name;
631  EXPECT_TRUE(oc.configure(ocm, tf));
632 
633  EXPECT_TRUE(oc.decide(robot_state).satisfied);
634  EXPECT_TRUE(oc.equal(oc, 1e-12));
635  EXPECT_TRUE(oc.mobileReferenceFrame());
636 
637  ASSERT_TRUE(oc.getLinkModel());
638 
639  geometry_msgs::msg::Pose p = tf2::toMsg(robot_state.getGlobalLinkTransform(oc.getLinkModel()->getName()));
640 
641  ocm.orientation = p.orientation;
642  ocm.header.frame_id = robot_model_->getModelFrame();
643  EXPECT_TRUE(oc.configure(ocm, tf));
644  EXPECT_TRUE(oc.decide(robot_state).satisfied);
645 
646  std::map<std::string, double> jvals;
647  jvals["r_wrist_roll_joint"] = .05;
648  robot_state.setVariablePositions(jvals);
649  robot_state.update();
650  EXPECT_TRUE(oc.decide(robot_state).satisfied);
651 
652  jvals["r_wrist_roll_joint"] = .11;
653  robot_state.setVariablePositions(jvals);
654  robot_state.update();
655  EXPECT_FALSE(oc.decide(robot_state).satisfied);
656 
657  // rotation by pi does not wrap to zero
658  jvals["r_wrist_roll_joint"] = M_PI;
659  robot_state.setVariablePositions(jvals);
660  robot_state.update();
661  EXPECT_FALSE(oc.decide(robot_state).satisfied);
662 }
663 
664 TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple)
665 {
666  moveit::core::RobotState robot_state(robot_model_);
667  robot_state.setToDefaultValues();
668  robot_state.update();
669  moveit::core::Transforms tf(robot_model_->getModelFrame());
670 
672  moveit_msgs::msg::VisibilityConstraint vcm;
673 
674  EXPECT_FALSE(vc.configure(vcm, tf));
675 
676  vcm.sensor_pose.header.frame_id = "base_footprint";
677  vcm.sensor_pose.pose.position.z = -1.0;
678  vcm.sensor_pose.pose.orientation.x = 0.0;
679  vcm.sensor_pose.pose.orientation.y = 1.0;
680  vcm.sensor_pose.pose.orientation.z = 0.0;
681  vcm.sensor_pose.pose.orientation.w = 0.0;
682 
683  vcm.target_pose.header.frame_id = "base_footprint";
684  vcm.target_pose.pose.position.z = -2.0;
685  vcm.target_pose.pose.orientation.y = 0.0;
686  vcm.target_pose.pose.orientation.w = 1.0;
687 
688  vcm.target_radius = .2;
689  vcm.cone_sides = 10;
690  vcm.max_view_angle = 0.0;
691  vcm.max_range_angle = 0.0;
692  vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_Z;
693  vcm.weight = 1.0;
694 
695  EXPECT_TRUE(vc.configure(vcm, tf));
696  // sensor and target are perfectly lined up
697  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
698 
699  vcm.max_view_angle = .1;
700 
701  // true, even with view angle
702  EXPECT_TRUE(vc.configure(vcm, tf));
703  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
704 
705  // very slight angle, so still ok
706  vcm.target_pose.pose.orientation.y = 0.03;
707  vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2));
708  EXPECT_TRUE(vc.configure(vcm, tf));
709  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
710 
711  // a little bit more puts it over
712  vcm.target_pose.pose.orientation.y = 0.06;
713  vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2));
714  EXPECT_TRUE(vc.configure(vcm, tf));
715  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
716 }
717 
718 TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2)
719 {
720  moveit::core::RobotState robot_state(robot_model_);
721  robot_state.setToDefaultValues();
722  robot_state.update();
723  moveit::core::Transforms tf(robot_model_->getModelFrame());
724 
726  moveit_msgs::msg::VisibilityConstraint vcm;
727 
728  vcm.sensor_pose.header.frame_id = "narrow_stereo_optical_frame";
729  vcm.sensor_pose.pose.position.z = 0.05;
730  vcm.sensor_pose.pose.orientation.w = 1.0;
731 
732  vcm.target_pose.header.frame_id = "l_gripper_r_finger_tip_link";
733  vcm.target_pose.pose.position.z = 0.03;
734  vcm.target_pose.pose.orientation.w = 1.0;
735 
736  vcm.cone_sides = 10;
737  vcm.max_view_angle = 0.0;
738  vcm.max_range_angle = 0.0;
739  vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_Z;
740  vcm.weight = 1.0;
741 
742  // false because target radius is 0.0
743  EXPECT_FALSE(vc.configure(vcm, tf));
744 
745  // this is all fine
746  vcm.target_radius = .05;
747  EXPECT_TRUE(vc.configure(vcm, tf));
748  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
749 
750  // this moves into collision with the cone, and should register false
751  std::map<std::string, double> state_values;
752  state_values["l_shoulder_lift_joint"] = .5;
753  state_values["r_shoulder_pan_joint"] = .5;
754  state_values["r_elbow_flex_joint"] = -1.4;
755  robot_state.setVariablePositions(state_values);
756  robot_state.update();
757  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
758 
759  // this moves far enough away that it's fine
760  state_values["r_shoulder_pan_joint"] = .4;
761  robot_state.setVariablePositions(state_values);
762  robot_state.update();
763  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
764 
765  // this is in collision with the arm, but now the cone, and should be fine
766  state_values["l_shoulder_lift_joint"] = 0;
767  state_values["r_shoulder_pan_joint"] = .5;
768  state_values["r_elbow_flex_joint"] = -.6;
769  robot_state.setVariablePositions(state_values);
770  robot_state.update();
771  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
772 
773  // this shouldn't matter
774  vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_X;
775  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
776 
777  robot_state.setToDefaultValues();
778  robot_state.update();
779  // just hits finger tip
780  vcm.target_radius = .01;
781  vcm.target_pose.pose.position.z = 0.00;
782  vcm.target_pose.pose.position.x = 0.035;
783  EXPECT_TRUE(vc.configure(vcm, tf));
784  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
785 
786  // larger target means it also hits finger
787  vcm.target_radius = .05;
788  EXPECT_TRUE(vc.configure(vcm, tf));
789  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
790 }
791 
792 TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet)
793 {
794  moveit::core::RobotState robot_state(robot_model_);
795  robot_state.setToDefaultValues();
796  moveit::core::Transforms tf(robot_model_->getModelFrame());
797 
799  EXPECT_TRUE(kcs.empty());
800 
801  moveit_msgs::msg::JointConstraint jcm;
802  jcm.joint_name = "head_pan_joint";
803  jcm.position = 0.4;
804  jcm.tolerance_above = 0.1;
805  jcm.tolerance_below = 0.05;
806  jcm.weight = 1.0;
807 
808  // this is a valid constraint
809  std::vector<moveit_msgs::msg::JointConstraint> jcv;
810  jcv.push_back(jcm);
811  EXPECT_TRUE(kcs.add(jcv));
812 
813  // but it isn't satisfied in the default state
814  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
815 
816  // now it is
817  std::map<std::string, double> jvals;
818  jvals[jcm.joint_name] = 0.41;
819  robot_state.setVariablePositions(jvals);
820  robot_state.update();
821  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
822 
823  // adding another constraint for a different joint
824  EXPECT_FALSE(kcs.empty());
825  kcs.clear();
826  EXPECT_TRUE(kcs.empty());
827  jcv.push_back(jcm);
828  jcv.back().joint_name = "head_tilt_joint";
829  EXPECT_TRUE(kcs.add(jcv));
830 
831  // now this one isn't satisfied
832  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
833 
834  // now it is
835  jvals[jcv.back().joint_name] = 0.41;
836  robot_state.setVariablePositions(jvals);
837  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
838 
839  // changing one joint outside the bounds makes it unsatisfied
840  jvals[jcv.back().joint_name] = 0.51;
841  robot_state.setVariablePositions(jvals);
842  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
843 
844  // one invalid constraint makes the add return false
845  kcs.clear();
846  jcv.back().joint_name = "no_joint";
847  EXPECT_FALSE(kcs.add(jcv));
848 
849  // but we can still evaluate it successfully for the remaining constraint
850  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
851 
852  // violating the remaining good constraint changes this
853  jvals["head_pan_joint"] = 0.51;
854  robot_state.setVariablePositions(jvals);
855  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
856 }
857 
858 TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality)
859 {
860  moveit::core::RobotState robot_state(robot_model_);
861  robot_state.setToDefaultValues();
862  moveit::core::Transforms tf(robot_model_->getModelFrame());
863 
866 
867  moveit_msgs::msg::JointConstraint jcm;
868  jcm.joint_name = "head_pan_joint";
869  jcm.position = 0.4;
870  jcm.tolerance_above = 0.1;
871  jcm.tolerance_below = 0.05;
872  jcm.weight = 1.0;
873 
874  moveit_msgs::msg::PositionConstraint pcm;
875  pcm.link_name = "l_wrist_roll_link";
876  pcm.target_point_offset.x = 0;
877  pcm.target_point_offset.y = 0;
878  pcm.target_point_offset.z = 0;
879  pcm.constraint_region.primitives.resize(1);
880  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
881  pcm.constraint_region.primitives[0].dimensions.resize(1);
882  pcm.constraint_region.primitives[0].dimensions[0] = 0.2;
883 
884  pcm.constraint_region.primitive_poses.resize(1);
885  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
886  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
887  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
888  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
889  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
890  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
891  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
892  pcm.weight = 1.0;
893 
894  pcm.header.frame_id = robot_model_->getModelFrame();
895 
896  // this is a valid constraint
897  std::vector<moveit_msgs::msg::JointConstraint> jcv;
898  jcv.push_back(jcm);
899  EXPECT_TRUE(kcs.add(jcv));
900 
901  std::vector<moveit_msgs::msg::PositionConstraint> pcv;
902  pcv.push_back(pcm);
903  EXPECT_TRUE(kcs.add(pcv, tf));
904 
905  // now adding in reverse order
906  EXPECT_TRUE(kcs2.add(pcv, tf));
907  EXPECT_TRUE(kcs2.add(jcv));
908 
909  // should be true
910  EXPECT_TRUE(kcs.equal(kcs2, .001));
911  EXPECT_TRUE(kcs2.equal(kcs, .001));
912 
913  // adding another copy of one of the constraints doesn't change anything
914  jcv.push_back(jcm);
915  EXPECT_TRUE(kcs2.add(jcv));
916 
917  EXPECT_TRUE(kcs.equal(kcs2, .001));
918  EXPECT_TRUE(kcs2.equal(kcs, .001));
919 
920  jcm.joint_name = "head_pan_joint";
921  jcm.position = 0.35;
922  jcm.tolerance_above = 0.1;
923  jcm.tolerance_below = 0.05;
924  jcm.weight = 1.0;
925 
926  jcv.push_back(jcm);
927  EXPECT_TRUE(kcs2.add(jcv));
928 
929  EXPECT_FALSE(kcs.equal(kcs2, .001));
930  EXPECT_FALSE(kcs2.equal(kcs, .001));
931 
932  // but they are within this margin
933  EXPECT_TRUE(kcs.equal(kcs2, .1));
934  EXPECT_TRUE(kcs2.equal(kcs, .1));
935 }
936 
937 int main(int argc, char** argv)
938 {
939  testing::InitGoogleTest(&argc, argv);
940  return RUN_ALL_TESTS();
941 }
moveit::core::RobotModelPtr robot_model_
void TearDown() override
Class for handling single DOF joint constraints.
bool equal(const KinematicConstraint &other, double margin) const override
Check if two joint constraints are the same.
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
void clear() override
Clear the stored constraint.
bool configure(const moveit_msgs::msg::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::msg::JointConstraint.
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
A class that contains many different constraints, and can check RobotState *versus the full set....
bool equal(const KinematicConstraintSet &other, double margin) const
Whether or not another KinematicConstraintSet is equal to this one.
bool empty() const
Returns whether or not there are any constraints in the set.
bool add(const moveit_msgs::msg::Constraints &c, const moveit::core::Transforms &tf)
Add all known constraints.
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result.
double getConstraintWeight() const
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
Class for constraints on the orientation of a link.
bool equal(const KinematicConstraint &other, double margin) const override
Check if two orientation constraints are the same.
bool mobileReferenceFrame() const
Whether or not a mobile reference frame is being employed.
bool configure(const moveit_msgs::msg::OrientationConstraint &oc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::msg::OrientationConstraint.
const moveit::core::LinkModel * getLinkModel() const
Gets the subject link model.
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Class for constraints on the XYZ position of a link.
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
void clear() override
Clear the stored constraint.
bool configure(const moveit_msgs::msg::PositionConstraint &pc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::msg::PositionConstraint.
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
bool mobileReferenceFrame() const
If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false.
bool equal(const KinematicConstraint &other, double margin) const override
Check if two constraints are the same. For position constraints this means that:
bool hasLinkOffset() const
If the constraint is enabled and the link offset is substantially different than zero.
Class for constraints on the visibility relationship between a sensor and a target.
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
bool configure(const moveit_msgs::msg::VisibilityConstraint &vc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::msg::VisibilityConstraint.
const std::string & getName() const
The name of this link.
Definition: link_model.h:86
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1246
void update(bool force=false)
Update all transforms.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:515
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:59
virtual bool isFixedFrame(const std::string &frame) const
Check whether a frame stays constant as the state of the robot model changes. This is true for any tr...
Definition: transforms.cpp:98
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Struct for containing the results of constraint evaluation.
bool satisfied
Whether or not the constraint or constraints were satisfied.
double distance
The distance evaluation from the constraint or constraints.
TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
int main(int argc, char **argv)