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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Dec 9 2018 03:51:46