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.h>
43 #include <boost/math/constants/constants.hpp>
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  robot_model::RobotModelPtr robot_model_;
59 };
60 
61 TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
62 {
64  robot_state.setToDefaultValues();
65  robot_state::Transforms tf(robot_model_->getModelFrame());
66 
68  moveit_msgs::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));
81  kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(robot_state);
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);
89  kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(robot_state);
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
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 {
179  robot_state.setToDefaultValues();
180  robot_state.update();
181  robot_state::Transforms tf(robot_model_->getModelFrame());
182 
184  moveit_msgs::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  // ouside 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);
228  kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(robot_state);
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);
235  kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(robot_state);
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::JointConstraint jcm2 = jcm;
301  jcm2.position = -6.28;
303  EXPECT_TRUE(jc.configure(jcm2));
304  jc.equal(jc2, .02);
305 }
306 
307 TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF)
308 {
310  robot_state.setToDefaultValues();
311 
313  moveit_msgs::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);
332 
333  jvals[jcm.joint_name] = 3.25;
334  robot_state.setVariablePositions(jvals);
337 
338  jvals[jcm.joint_name] = -3.14;
339  robot_state.setVariablePositions(jvals);
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);
351 
352  jvals[jcm.joint_name] = 3.25;
353  robot_state.setVariablePositions(jvals);
356 }
357 
358 TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed)
359 {
361  robot_state.setToDefaultValues();
362  robot_state.update(true);
363  robot_state::Transforms tf(robot_model_->getModelFrame());
365  moveit_msgs::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::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 {
437  robot_state.setToDefaultValues();
438  robot_state::Transforms tf(robot_model_->getModelFrame());
439  robot_state.update();
440 
442  moveit_msgs::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::SolidPrimitive::SPHERE;
451  pcm.constraint_region.primitives[0].dimensions.resize(1);
452  pcm.constraint_region.primitives[0].dimensions[shape_msgs::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));
469 
470  EXPECT_TRUE(pc.decide(robot_state).satisfied);
471 
472  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
473  pcm.constraint_region.primitives[0].dimensions.resize(3);
474  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1;
475  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
476  pcm.constraint_region.primitives[0].dimensions[shape_msgs::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::SolidPrimitive::BOX;
503  pcm.constraint_region.primitives[1].dimensions.resize(3);
504  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1;
505  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
506  pcm.constraint_region.primitives[1].dimensions[shape_msgs::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 {
514  robot_state.setToDefaultValues();
515  robot_state::Transforms tf(robot_model_->getModelFrame());
516 
519  moveit_msgs::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::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::SolidPrimitive::BOX;
531  pcm.constraint_region.primitives[1].dimensions.resize(3);
532  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 2.0;
533  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 2.0;
534  pcm.constraint_region.primitives[1].dimensions[shape_msgs::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::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::SolidPrimitive::SPHERE;
593  EXPECT_TRUE(pc2.configure(pcm2, tf));
594  EXPECT_FALSE(pc.equal(pc2, .001));
595 }
596 
597 TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)
598 {
600  robot_state.setToDefaultValues();
601  robot_state.update();
602  robot_state::Transforms tf(robot_model_->getModelFrame());
603 
605 
606  moveit_msgs::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::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"] = boost::math::constants::pi<double>();
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 {
667  robot_state.setToDefaultValues();
668  robot_state.update();
669  robot_state::Transforms tf(robot_model_->getModelFrame());
670 
672  moveit_msgs::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.y = 1.0;
679 
680  vcm.target_pose.header.frame_id = "base_footprint";
681  vcm.target_pose.pose.position.z = -2.0;
682  vcm.target_pose.pose.orientation.y = 0.0;
683  vcm.target_pose.pose.orientation.w = 1.0;
684 
685  vcm.target_radius = .2;
686  vcm.cone_sides = 10;
687  vcm.max_view_angle = 0.0;
688  vcm.max_range_angle = 0.0;
689  vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_Z;
690  vcm.weight = 1.0;
691 
692  EXPECT_TRUE(vc.configure(vcm, tf));
693  // sensor and target are perfectly lined up
694  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
695 
696  vcm.max_view_angle = .1;
697 
698  // true, even with view angle
699  EXPECT_TRUE(vc.configure(vcm, tf));
700  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
701 
702  // very slight angle, so still ok
703  vcm.target_pose.pose.orientation.y = 0.03;
704  vcm.target_pose.pose.orientation.w = .9995;
705  EXPECT_TRUE(vc.configure(vcm, tf));
706  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
707 
708  // a little bit more puts it over
709  vcm.target_pose.pose.orientation.y = 0.06;
710  vcm.target_pose.pose.orientation.w = .9981;
711  EXPECT_TRUE(vc.configure(vcm, tf));
712  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
713 }
714 
715 TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2)
716 {
718  robot_state.setToDefaultValues();
719  robot_state.update();
720  robot_state::Transforms tf(robot_model_->getModelFrame());
721 
723  moveit_msgs::VisibilityConstraint vcm;
724 
725  vcm.sensor_pose.header.frame_id = "narrow_stereo_optical_frame";
726  vcm.sensor_pose.pose.position.z = 0.05;
727  vcm.sensor_pose.pose.orientation.w = 1.0;
728 
729  vcm.target_pose.header.frame_id = "l_gripper_r_finger_tip_link";
730  vcm.target_pose.pose.position.z = 0.03;
731  vcm.target_pose.pose.orientation.w = 1.0;
732 
733  vcm.cone_sides = 10;
734  vcm.max_view_angle = 0.0;
735  vcm.max_range_angle = 0.0;
736  vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_Z;
737  vcm.weight = 1.0;
738 
739  // false because target radius is 0.0
740  EXPECT_FALSE(vc.configure(vcm, tf));
741 
742  // this is all fine
743  vcm.target_radius = .05;
744  EXPECT_TRUE(vc.configure(vcm, tf));
745  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
746 
747  // this moves into collision with the cone, and should register false
748  std::map<std::string, double> state_values;
749  state_values["l_shoulder_lift_joint"] = .5;
750  state_values["r_shoulder_pan_joint"] = .5;
751  state_values["r_elbow_flex_joint"] = -1.4;
752  robot_state.setVariablePositions(state_values);
753  robot_state.update();
754  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
755 
756  // this moves far enough away that it's fine
757  state_values["r_shoulder_pan_joint"] = .4;
758  robot_state.setVariablePositions(state_values);
759  robot_state.update();
760  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
761 
762  // this is in collision with the arm, but now the cone, and should be fine
763  state_values["l_shoulder_lift_joint"] = 0;
764  state_values["r_shoulder_pan_joint"] = .5;
765  state_values["r_elbow_flex_joint"] = -.6;
766  robot_state.setVariablePositions(state_values);
767  robot_state.update();
768  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
769 
770  // this shouldn't matter
771  vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_X;
772  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
773 
774  robot_state.setToDefaultValues();
775  robot_state.update();
776  // just hits finger tip
777  vcm.target_radius = .01;
778  vcm.target_pose.pose.position.z = 0.00;
779  vcm.target_pose.pose.position.x = 0.035;
780  EXPECT_TRUE(vc.configure(vcm, tf));
781  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
782 
783  // larger target means it also hits finger
784  vcm.target_radius = .05;
785  EXPECT_TRUE(vc.configure(vcm, tf));
786  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
787 }
788 
789 TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet)
790 {
792  robot_state.setToDefaultValues();
793  robot_state::Transforms tf(robot_model_->getModelFrame());
794 
796  EXPECT_TRUE(kcs.empty());
797 
798  moveit_msgs::JointConstraint jcm;
799  jcm.joint_name = "head_pan_joint";
800  jcm.position = 0.4;
801  jcm.tolerance_above = 0.1;
802  jcm.tolerance_below = 0.05;
803  jcm.weight = 1.0;
804 
805  // this is a valid constraint
806  std::vector<moveit_msgs::JointConstraint> jcv;
807  jcv.push_back(jcm);
808  EXPECT_TRUE(kcs.add(jcv));
809 
810  // but it isn't satisfied in the default state
811  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
812 
813  // now it is
814  std::map<std::string, double> jvals;
815  jvals[jcm.joint_name] = 0.41;
816  robot_state.setVariablePositions(jvals);
817  robot_state.update();
818  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
819 
820  // adding another constraint for a different joint
821  EXPECT_FALSE(kcs.empty());
822  kcs.clear();
823  EXPECT_TRUE(kcs.empty());
824  jcv.push_back(jcm);
825  jcv.back().joint_name = "head_tilt_joint";
826  EXPECT_TRUE(kcs.add(jcv));
827 
828  // now this one isn't satisfied
829  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
830 
831  // now it is
832  jvals[jcv.back().joint_name] = 0.41;
833  robot_state.setVariablePositions(jvals);
834  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
835 
836  // changing one joint outside the bounds makes it unsatisfied
837  jvals[jcv.back().joint_name] = 0.51;
838  robot_state.setVariablePositions(jvals);
839  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
840 
841  // one invalid constraint makes the add return false
842  kcs.clear();
843  jcv.back().joint_name = "no_joint";
844  EXPECT_FALSE(kcs.add(jcv));
845 
846  // but we can still evaluate it succesfully for the remaining constraint
847  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
848 
849  // violating the remaining good constraint changes this
850  jvals["head_pan_joint"] = 0.51;
851  robot_state.setVariablePositions(jvals);
852  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
853 }
854 
855 TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality)
856 {
858  robot_state.setToDefaultValues();
859  robot_state::Transforms tf(robot_model_->getModelFrame());
860 
863 
864  moveit_msgs::JointConstraint jcm;
865  jcm.joint_name = "head_pan_joint";
866  jcm.position = 0.4;
867  jcm.tolerance_above = 0.1;
868  jcm.tolerance_below = 0.05;
869  jcm.weight = 1.0;
870 
871  moveit_msgs::PositionConstraint pcm;
872  pcm.link_name = "l_wrist_roll_link";
873  pcm.target_point_offset.x = 0;
874  pcm.target_point_offset.y = 0;
875  pcm.target_point_offset.z = 0;
876  pcm.constraint_region.primitives.resize(1);
877  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
878  pcm.constraint_region.primitives[0].dimensions.resize(1);
879  pcm.constraint_region.primitives[0].dimensions[0] = 0.2;
880 
881  pcm.constraint_region.primitive_poses.resize(1);
882  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
883  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
884  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
885  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
886  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
887  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
888  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
889  pcm.weight = 1.0;
890 
891  pcm.header.frame_id = robot_model_->getModelFrame();
892 
893  // this is a valid constraint
894  std::vector<moveit_msgs::JointConstraint> jcv;
895  jcv.push_back(jcm);
896  EXPECT_TRUE(kcs.add(jcv));
897 
898  std::vector<moveit_msgs::PositionConstraint> pcv;
899  pcv.push_back(pcm);
900  EXPECT_TRUE(kcs.add(pcv, tf));
901 
902  // now adding in reverse order
903  EXPECT_TRUE(kcs2.add(pcv, tf));
904  EXPECT_TRUE(kcs2.add(jcv));
905 
906  // should be true
907  EXPECT_TRUE(kcs.equal(kcs2, .001));
908  EXPECT_TRUE(kcs2.equal(kcs, .001));
909 
910  // adding another copy of one of the constraints doesn't change anything
911  jcv.push_back(jcm);
912  EXPECT_TRUE(kcs2.add(jcv));
913 
914  EXPECT_TRUE(kcs.equal(kcs2, .001));
915  EXPECT_TRUE(kcs2.equal(kcs, .001));
916 
917  jcm.joint_name = "head_pan_joint";
918  jcm.position = 0.35;
919  jcm.tolerance_above = 0.1;
920  jcm.tolerance_below = 0.05;
921  jcm.weight = 1.0;
922 
923  jcv.push_back(jcm);
924  EXPECT_TRUE(kcs2.add(jcv));
925 
926  EXPECT_FALSE(kcs.equal(kcs2, .001));
927  EXPECT_FALSE(kcs2.equal(kcs, .001));
928 
929  // but they are within this margin
930  EXPECT_TRUE(kcs.equal(kcs2, .1));
931  EXPECT_TRUE(kcs2.equal(kcs, .1));
932 }
933 
934 int main(int argc, char** argv)
935 {
936  testing::InitGoogleTest(&argc, argv);
937  return RUN_ALL_TESTS();
938 }
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:554
Core components of MoveIt!
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Class for constraints on the orientation of a link.
Class for constraints on the visibility relationship between a sensor and a target.
bool configure(const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::PositionConstraint.
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Class for handling single DOF joint constraints.
A class that contains many different constraints, and can check RobotState *versus the full set...
bool equal(const KinematicConstraint &other, double margin) const override
Check if two constraints are the same. For position constraints this means that:
#define EXPECT_NEAR(a, b, prec)
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:60
double distance
The distance evaluation from the constraint or constraints.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
Add all known constraints.
bool satisfied
Whether or not the constraint or constraints were satisfied.
#define EXPECT_FALSE(args)
void update(bool force=false)
Update all transforms.
bool equal(const KinematicConstraintSet &other, double margin) const
Whether or not another KinematicConstraintSet is equal to this one.
bool mobileReferenceFrame() const
If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false...
Struct for containing the results of constraint evaluation.
Class for constraints on the XYZ position of a link.
B toMsg(const A &a)
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1474
robot_model::RobotModelPtr robot_model_
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:128
int main(int argc, char **argv)
#define EXPECT_TRUE(args)
bool configure(const moveit_msgs::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::JointConstraint.
void TearDown() override


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Mar 17 2022 02:51:04