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