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 
37 #include <moveit/kinematic_constraints/kinematic_constraint.h>
38 #include <gtest/gtest.h>
39 #include <urdf_parser/urdf_parser.h>
40 #include <fstream>
41 #include <eigen_conversions/eigen_msg.h>
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());
71  kmodel.reset(new robot_model::RobotModel(urdf_model, srdf_model));
72  };
73 
74  void TearDown() override
75  {
76  }
77 
78 protected:
79  urdf::ModelInterfaceSharedPtr urdf_model;
80  srdf::ModelSharedPtr srdf_model;
81  robot_model::RobotModelPtr kmodel;
82 };
83 
84 TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
85 {
86  robot_state::RobotState ks(kmodel);
87  ks.setToDefaultValues();
88  robot_state::Transforms tf(kmodel->getModelFrame());
89 
90  kinematic_constraints::JointConstraint jc(kmodel);
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));
104  kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
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);
112  kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
113  EXPECT_TRUE(p2.satisfied);
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
161  kinematic_constraints::JointConstraint jc2(kmodel);
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 {
201  robot_state::RobotState ks(kmodel);
202  ks.setToDefaultValues();
203  ks.update();
204  robot_state::Transforms tf(kmodel->getModelFrame());
205 
206  kinematic_constraints::JointConstraint jc(kmodel);
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);
251  kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
252  EXPECT_TRUE(p1.satisfied);
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);
258  kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
259  EXPECT_TRUE(p2.satisfied);
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;
325  kinematic_constraints::JointConstraint jc2(kmodel);
326  EXPECT_TRUE(jc.configure(jcm2));
327  jc.equal(jc2, .02);
328 }
329 
330 TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF)
331 {
332  robot_state::RobotState ks(kmodel);
333  ks.setToDefaultValues();
334 
335  kinematic_constraints::JointConstraint jc(kmodel);
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);
353  kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
354  EXPECT_TRUE(p1.satisfied);
355 
356  jvals[jcm.joint_name] = 3.25;
357  ks.setVariablePositions(jvals);
358  kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
359  EXPECT_FALSE(p2.satisfied);
360 
361  jvals[jcm.joint_name] = -3.14;
362  ks.setVariablePositions(jvals);
363  kinematic_constraints::ConstraintEvaluationResult p3 = jc.decide(ks);
364  EXPECT_FALSE(p3.satisfied);
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);
372  kinematic_constraints::ConstraintEvaluationResult p4 = jc.decide(ks);
373  EXPECT_TRUE(p4.satisfied);
374 
375  jvals[jcm.joint_name] = 3.25;
376  ks.setVariablePositions(jvals);
377  kinematic_constraints::ConstraintEvaluationResult p5 = jc.decide(ks);
378  EXPECT_FALSE(p5.satisfied);
379 }
380 
381 TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed)
382 {
383  robot_state::RobotState ks(kmodel);
384  ks.setToDefaultValues();
385  ks.update(true);
386  robot_state::Transforms tf(kmodel->getModelFrame());
387  kinematic_constraints::PositionConstraint pc(kmodel);
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 {
459  robot_state::RobotState ks(kmodel);
460  ks.setToDefaultValues();
461  robot_state::Transforms tf(kmodel->getModelFrame());
462  ks.update();
463 
464  kinematic_constraints::PositionConstraint pc(kmodel);
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));
491  EXPECT_TRUE(pc.mobileReferenceFrame());
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 {
536  robot_state::RobotState ks(kmodel);
537  ks.setToDefaultValues();
538  robot_state::Transforms tf(kmodel->getModelFrame());
539 
540  kinematic_constraints::PositionConstraint pc(kmodel);
541  kinematic_constraints::PositionConstraint pc2(kmodel);
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 {
622  robot_state::RobotState ks(kmodel);
623  ks.setToDefaultValues();
624  ks.update();
625  robot_state::Transforms tf(kmodel->getModelFrame());
626 
627  kinematic_constraints::OrientationConstraint oc(kmodel);
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 {
685  robot_state::RobotState ks(kmodel);
686  ks.setToDefaultValues();
687  ks.update();
688  robot_state::Transforms tf(kmodel->getModelFrame());
689 
690  kinematic_constraints::VisibilityConstraint vc(kmodel);
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 {
736  robot_state::RobotState ks(kmodel);
737  ks.setToDefaultValues();
738  ks.update();
739  robot_state::Transforms tf(kmodel->getModelFrame());
740 
741  kinematic_constraints::VisibilityConstraint vc(kmodel);
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 {
810  robot_state::RobotState ks(kmodel);
811  ks.setToDefaultValues();
812  robot_state::Transforms tf(kmodel->getModelFrame());
813 
814  kinematic_constraints::KinematicConstraintSet kcs(kmodel);
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 {
876  robot_state::RobotState ks(kmodel);
877  ks.setToDefaultValues();
878  robot_state::Transforms tf(kmodel->getModelFrame());
879 
880  kinematic_constraints::KinematicConstraintSet kcs(kmodel);
881  kinematic_constraints::KinematicConstraintSet kcs2(kmodel);
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 }
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:527
LoadPlanningModelsPr2::kmodel
robot_model::RobotModelPtr kmodel
Definition: test_constraint_samplers.cpp:127
TEST_F
TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
Definition: test_constraints.cpp:84
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition: kinematic_constraint.h:348
kinematic_constraints::VisibilityConstraint
Class for constraints on the visibility relationship between a sensor and a target.
Definition: kinematic_constraint.h:750
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1368
tf::poseEigenToMsg
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
kinematic_constraints::PositionConstraint::configure
bool configure(const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::PositionConstraint.
Definition: kinematic_constraint.cpp:272
kinematic_constraints::JointConstraint
Class for handling single DOF joint constraints.
Definition: kinematic_constraint.h:203
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set...
Definition: kinematic_constraint.h:861
kinematic_constraints::PositionConstraint::decide
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:433
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
Definition: robot_state.cpp:297
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
kinematic_constraints::KinematicConstraintSet::equal
bool equal(const KinematicConstraintSet &other, double margin) const
Whether or not another KinematicConstraintSet is equal to this one.
Definition: kinematic_constraint.cpp:1164
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
kinematic_constraints::ConstraintEvaluationResult::distance
double distance
The distance evaluation from the constraint or constraints.
Definition: kinematic_constraint.h:72
boost::shared_ptr< Model >
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:288
kinematic_constraints::KinematicConstraintSet::add
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
Add all known constraints.
Definition: kinematic_constraint.cpp:1119
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
Whether or not the constraint or constraints were satisfied.
Definition: kinematic_constraint.h:71
EXPECT_FALSE
#define EXPECT_FALSE(args)
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:563
LoadPlanningModelsPr2::urdf_model
urdf::ModelInterfaceSharedPtr urdf_model
Definition: test_constraint_samplers.cpp:125
xml_string
def xml_string(rootXml, addHeader=True)
kinematic_constraints::PositionConstraint::equal
virtual bool equal(const KinematicConstraint &other, double margin) const
Check if two constraints are the same. For position constraints this means that:
Definition: kinematic_constraint.cpp:376
kinematic_constraints::ConstraintEvaluationResult
Struct for containing the results of constraint evaluation.
Definition: kinematic_constraint.h:56
kinematic_constraints::PositionConstraint
Class for constraints on the XYZ position of a link.
Definition: kinematic_constraint.h:505
moveit::core::RobotState
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
kinematic_constraints::PositionConstraint::mobileReferenceFrame
bool mobileReferenceFrame() const
If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false...
Definition: kinematic_constraint.h:634
main
int main(int argc, char **argv)
Definition: test_constraints.cpp:953
EXPECT_TRUE
#define EXPECT_TRUE(args)
kinematic_constraints::JointConstraint::configure
bool configure(const moveit_msgs::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::JointConstraint.
Definition: kinematic_constraint.cpp:68
kinematic_constraints::JointConstraint::decide
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:208
eigen_msg.h
kinematic_constraint.h
LoadPlanningModelsPr2::TearDown
void TearDown() override
Definition: test_constraints.cpp:74

moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05