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

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