moveit2
The MoveIt Motion Planning Framework for ROS 2.
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.hpp>
42 #include <math.h>
43 #include <moveit/utils/robot_model_test_utils.h>
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  moveit::core::RobotModelPtr robot_model_;
59 };
60 
61 TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
62 {
63  moveit::core::RobotState robot_state(robot_model_);
64  robot_state.setToDefaultValues();
65  moveit::core::Transforms tf(robot_model_->getModelFrame());
66 
67  kinematic_constraints::JointConstraint jc(robot_model_);
68  moveit_msgs::msg::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  moveit::core::RobotState robot_state(robot_model_);
179  robot_state.setToDefaultValues();
180  robot_state.update();
181  moveit::core::Transforms tf(robot_model_->getModelFrame());
182 
183  kinematic_constraints::JointConstraint jc(robot_model_);
184  moveit_msgs::msg::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  // outside 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::msg::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  moveit::core::RobotState robot_state(robot_model_);
310  robot_state.setToDefaultValues();
311 
312  kinematic_constraints::JointConstraint jc(robot_model_);
313  moveit_msgs::msg::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  moveit::core::RobotState robot_state(robot_model_);
361  robot_state.setToDefaultValues();
362  robot_state.update(true);
363  moveit::core::Transforms tf(robot_model_->getModelFrame());
364  kinematic_constraints::PositionConstraint pc(robot_model_);
365  moveit_msgs::msg::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::msg::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  moveit::core::RobotState robot_state(robot_model_);
437  robot_state.setToDefaultValues();
438  moveit::core::Transforms tf(robot_model_->getModelFrame());
439  robot_state.update();
440 
441  kinematic_constraints::PositionConstraint pc(robot_model_);
442  moveit_msgs::msg::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::msg::SolidPrimitive::SPHERE;
451  pcm.constraint_region.primitives[0].dimensions.resize(1);
452  pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::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::msg::SolidPrimitive::BOX;
473  pcm.constraint_region.primitives[0].dimensions.resize(3);
474  pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.1;
475  pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 0.1;
476  pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::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::msg::SolidPrimitive::BOX;
503  pcm.constraint_region.primitives[1].dimensions.resize(3);
504  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.1;
505  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 0.1;
506  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::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  moveit::core::RobotState robot_state(robot_model_);
514  robot_state.setToDefaultValues();
515  moveit::core::Transforms tf(robot_model_->getModelFrame());
516 
517  kinematic_constraints::PositionConstraint pc(robot_model_);
518  kinematic_constraints::PositionConstraint pc2(robot_model_);
519  moveit_msgs::msg::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::msg::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::msg::SolidPrimitive::BOX;
531  pcm.constraint_region.primitives[1].dimensions.resize(3);
532  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 2.0;
533  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 2.0;
534  pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::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::msg::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::msg::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  moveit::core::RobotState robot_state(robot_model_);
600  robot_state.setToDefaultValues();
601  robot_state.update();
602  moveit::core::Transforms tf(robot_model_->getModelFrame());
603 
604  kinematic_constraints::OrientationConstraint oc(robot_model_);
605 
606  moveit_msgs::msg::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::msg::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"] = M_PI;
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  moveit::core::RobotState robot_state(robot_model_);
667  robot_state.setToDefaultValues();
668  robot_state.update();
669  moveit::core::Transforms tf(robot_model_->getModelFrame());
670 
671  kinematic_constraints::VisibilityConstraint vc(robot_model_);
672  moveit_msgs::msg::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.x = 0.0;
679  vcm.sensor_pose.pose.orientation.y = 1.0;
680  vcm.sensor_pose.pose.orientation.z = 0.0;
681  vcm.sensor_pose.pose.orientation.w = 0.0;
682 
683  vcm.target_pose.header.frame_id = "base_footprint";
684  vcm.target_pose.pose.position.z = -2.0;
685  vcm.target_pose.pose.orientation.y = 0.0;
686  vcm.target_pose.pose.orientation.w = 1.0;
687 
688  vcm.target_radius = .2;
689  vcm.cone_sides = 10;
690  vcm.max_view_angle = 0.0;
691  vcm.max_range_angle = 0.0;
692  vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_Z;
693  vcm.weight = 1.0;
694 
695  EXPECT_TRUE(vc.configure(vcm, tf));
696  // sensor and target are perfectly lined up
697  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
698 
699  vcm.max_view_angle = .1;
700 
701  // true, even with view angle
702  EXPECT_TRUE(vc.configure(vcm, tf));
703  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
704 
705  // very slight angle, so still ok
706  vcm.target_pose.pose.orientation.y = 0.03;
707  vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2));
708  EXPECT_TRUE(vc.configure(vcm, tf));
709  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
710 
711  // a little bit more puts it over
712  vcm.target_pose.pose.orientation.y = 0.06;
713  vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2));
714  EXPECT_TRUE(vc.configure(vcm, tf));
715  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
716 }
717 
718 TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2)
719 {
720  moveit::core::RobotState robot_state(robot_model_);
721  robot_state.setToDefaultValues();
722  robot_state.update();
723  moveit::core::Transforms tf(robot_model_->getModelFrame());
724 
725  kinematic_constraints::VisibilityConstraint vc(robot_model_);
726  moveit_msgs::msg::VisibilityConstraint vcm;
727 
728  vcm.sensor_pose.header.frame_id = "narrow_stereo_optical_frame";
729  vcm.sensor_pose.pose.position.z = 0.05;
730  vcm.sensor_pose.pose.orientation.w = 1.0;
731 
732  vcm.target_pose.header.frame_id = "l_gripper_r_finger_tip_link";
733  vcm.target_pose.pose.position.z = 0.03;
734  vcm.target_pose.pose.orientation.w = 1.0;
735 
736  vcm.cone_sides = 10;
737  vcm.max_view_angle = 0.0;
738  vcm.max_range_angle = 0.0;
739  vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_Z;
740  vcm.weight = 1.0;
741 
742  // false because target radius is 0.0
743  EXPECT_FALSE(vc.configure(vcm, tf));
744 
745  // this is all fine
746  vcm.target_radius = .05;
747  EXPECT_TRUE(vc.configure(vcm, tf));
748  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
749 
750  // this moves into collision with the cone, and should register false
751  std::map<std::string, double> state_values;
752  state_values["l_shoulder_lift_joint"] = .5;
753  state_values["r_shoulder_pan_joint"] = .5;
754  state_values["r_elbow_flex_joint"] = -1.4;
755  robot_state.setVariablePositions(state_values);
756  robot_state.update();
757  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
758 
759  // this moves far enough away that it's fine
760  state_values["r_shoulder_pan_joint"] = .4;
761  robot_state.setVariablePositions(state_values);
762  robot_state.update();
763  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
764 
765  // this is in collision with the arm, but now the cone, and should be fine
766  state_values["l_shoulder_lift_joint"] = 0;
767  state_values["r_shoulder_pan_joint"] = .5;
768  state_values["r_elbow_flex_joint"] = -.6;
769  robot_state.setVariablePositions(state_values);
770  robot_state.update();
771  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
772 
773  // this shouldn't matter
774  vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_X;
775  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
776 
777  robot_state.setToDefaultValues();
778  robot_state.update();
779  // just hits finger tip
780  vcm.target_radius = .01;
781  vcm.target_pose.pose.position.z = 0.00;
782  vcm.target_pose.pose.position.x = 0.035;
783  EXPECT_TRUE(vc.configure(vcm, tf));
784  EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
785 
786  // larger target means it also hits finger
787  vcm.target_radius = .05;
788  EXPECT_TRUE(vc.configure(vcm, tf));
789  EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
790 }
791 
792 TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet)
793 {
794  moveit::core::RobotState robot_state(robot_model_);
795  robot_state.setToDefaultValues();
796  moveit::core::Transforms tf(robot_model_->getModelFrame());
797 
798  kinematic_constraints::KinematicConstraintSet kcs(robot_model_);
799  EXPECT_TRUE(kcs.empty());
800 
801  moveit_msgs::msg::JointConstraint jcm;
802  jcm.joint_name = "head_pan_joint";
803  jcm.position = 0.4;
804  jcm.tolerance_above = 0.1;
805  jcm.tolerance_below = 0.05;
806  jcm.weight = 1.0;
807 
808  // this is a valid constraint
809  std::vector<moveit_msgs::msg::JointConstraint> jcv;
810  jcv.push_back(jcm);
811  EXPECT_TRUE(kcs.add(jcv));
812 
813  // but it isn't satisfied in the default state
814  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
815 
816  // now it is
817  std::map<std::string, double> jvals;
818  jvals[jcm.joint_name] = 0.41;
819  robot_state.setVariablePositions(jvals);
820  robot_state.update();
821  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
822 
823  // adding another constraint for a different joint
824  EXPECT_FALSE(kcs.empty());
825  kcs.clear();
826  EXPECT_TRUE(kcs.empty());
827  jcv.push_back(jcm);
828  jcv.back().joint_name = "head_tilt_joint";
829  EXPECT_TRUE(kcs.add(jcv));
830 
831  // now this one isn't satisfied
832  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
833 
834  // now it is
835  jvals[jcv.back().joint_name] = 0.41;
836  robot_state.setVariablePositions(jvals);
837  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
838 
839  // changing one joint outside the bounds makes it unsatisfied
840  jvals[jcv.back().joint_name] = 0.51;
841  robot_state.setVariablePositions(jvals);
842  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
843 
844  // one invalid constraint makes the add return false
845  kcs.clear();
846  jcv.back().joint_name = "no_joint";
847  EXPECT_FALSE(kcs.add(jcv));
848 
849  // but we can still evaluate it successfully for the remaining constraint
850  EXPECT_TRUE(kcs.decide(robot_state).satisfied);
851 
852  // violating the remaining good constraint changes this
853  jvals["head_pan_joint"] = 0.51;
854  robot_state.setVariablePositions(jvals);
855  EXPECT_FALSE(kcs.decide(robot_state).satisfied);
856 }
857 
858 TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality)
859 {
860  moveit::core::RobotState robot_state(robot_model_);
861  robot_state.setToDefaultValues();
862  moveit::core::Transforms tf(robot_model_->getModelFrame());
863 
864  kinematic_constraints::KinematicConstraintSet kcs(robot_model_);
865  kinematic_constraints::KinematicConstraintSet kcs2(robot_model_);
866 
867  moveit_msgs::msg::JointConstraint jcm;
868  jcm.joint_name = "head_pan_joint";
869  jcm.position = 0.4;
870  jcm.tolerance_above = 0.1;
871  jcm.tolerance_below = 0.05;
872  jcm.weight = 1.0;
873 
874  moveit_msgs::msg::PositionConstraint pcm;
875  pcm.link_name = "l_wrist_roll_link";
876  pcm.target_point_offset.x = 0;
877  pcm.target_point_offset.y = 0;
878  pcm.target_point_offset.z = 0;
879  pcm.constraint_region.primitives.resize(1);
880  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
881  pcm.constraint_region.primitives[0].dimensions.resize(1);
882  pcm.constraint_region.primitives[0].dimensions[0] = 0.2;
883 
884  pcm.constraint_region.primitive_poses.resize(1);
885  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
886  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
887  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
888  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
889  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
890  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
891  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
892  pcm.weight = 1.0;
893 
894  pcm.header.frame_id = robot_model_->getModelFrame();
895 
896  // this is a valid constraint
897  std::vector<moveit_msgs::msg::JointConstraint> jcv;
898  jcv.push_back(jcm);
899  EXPECT_TRUE(kcs.add(jcv));
900 
901  std::vector<moveit_msgs::msg::PositionConstraint> pcv;
902  pcv.push_back(pcm);
903  EXPECT_TRUE(kcs.add(pcv, tf));
904 
905  // now adding in reverse order
906  EXPECT_TRUE(kcs2.add(pcv, tf));
907  EXPECT_TRUE(kcs2.add(jcv));
908 
909  // should be true
910  EXPECT_TRUE(kcs.equal(kcs2, .001));
911  EXPECT_TRUE(kcs2.equal(kcs, .001));
912 
913  // adding another copy of one of the constraints doesn't change anything
914  jcv.push_back(jcm);
915  EXPECT_TRUE(kcs2.add(jcv));
916 
917  EXPECT_TRUE(kcs.equal(kcs2, .001));
918  EXPECT_TRUE(kcs2.equal(kcs, .001));
919 
920  jcm.joint_name = "head_pan_joint";
921  jcm.position = 0.35;
922  jcm.tolerance_above = 0.1;
923  jcm.tolerance_below = 0.05;
924  jcm.weight = 1.0;
925 
926  jcv.push_back(jcm);
927  EXPECT_TRUE(kcs2.add(jcv));
928 
929  EXPECT_FALSE(kcs.equal(kcs2, .001));
930  EXPECT_FALSE(kcs2.equal(kcs, .001));
931 
932  // but they are within this margin
933  EXPECT_TRUE(kcs.equal(kcs2, .1));
934  EXPECT_TRUE(kcs2.equal(kcs, .1));
935 }
936 
937 int main(int argc, char** argv)
938 {
939  testing::InitGoogleTest(&argc, argv);
940  return RUN_ALL_TESTS();
941 }
LoadPlanningModelsPr2::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_constraint_samplers.cpp:107
LoadPlanningModelsPr2::TearDown
void TearDown() override
Definition: test_constraints.cpp:53
kinematic_constraints::JointConstraint
Class for handling single DOF joint constraints.
Definition: kinematic_constraint.h:203
kinematic_constraints::JointConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two joint constraints are the same.
Definition: kinematic_constraint.cpp:261
kinematic_constraints::JointConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:273
kinematic_constraints::JointConstraint::clear
void clear() override
Clear the stored constraint.
Definition: kinematic_constraint.cpp:311
kinematic_constraints::JointConstraint::configure
bool configure(const moveit_msgs::msg::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::msg::JointConstraint.
Definition: kinematic_constraint.cpp:130
kinematic_constraints::JointConstraint::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition: kinematic_constraint.cpp:306
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set....
Definition: kinematic_constraint.h:872
kinematic_constraints::KinematicConstraintSet::clear
void clear()
Clear the stored constraints.
Definition: kinematic_constraint.cpp:1177
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:1295
kinematic_constraints::KinematicConstraintSet::empty
bool empty() const
Returns whether or not there are any constraints in the set.
Definition: kinematic_constraint.h:1061
kinematic_constraints::KinematicConstraintSet::add
bool add(const moveit_msgs::msg::Constraints &c, const moveit::core::Transforms &tf)
Add all known constraints.
Definition: kinematic_constraint.cpp:1250
kinematic_constraints::KinematicConstraintSet::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result.
Definition: kinematic_constraint.cpp:1259
kinematic_constraints::KinematicConstraint::getConstraintWeight
double getConstraintWeight() const
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
Definition: kinematic_constraint.h:158
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition: kinematic_constraint.h:348
kinematic_constraints::OrientationConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two orientation constraints are the same.
Definition: kinematic_constraint.cpp:637
kinematic_constraints::OrientationConstraint::mobileReferenceFrame
bool mobileReferenceFrame() const
Whether or not a mobile reference frame is being employed.
Definition: kinematic_constraint.h:430
kinematic_constraints::OrientationConstraint::configure
bool configure(const moveit_msgs::msg::OrientationConstraint &oc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::msg::OrientationConstraint.
Definition: kinematic_constraint.cpp:558
kinematic_constraints::OrientationConstraint::getLinkModel
const moveit::core::LinkModel * getLinkModel() const
Gets the subject link model.
Definition: kinematic_constraint.h:408
kinematic_constraints::OrientationConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:670
kinematic_constraints::PositionConstraint
Class for constraints on the XYZ position of a link.
Definition: kinematic_constraint.h:515
kinematic_constraints::PositionConstraint::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition: kinematic_constraint.cpp:553
kinematic_constraints::PositionConstraint::clear
void clear() override
Clear the stored constraint.
Definition: kinematic_constraint.cpp:542
kinematic_constraints::PositionConstraint::configure
bool configure(const moveit_msgs::msg::PositionConstraint &pc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::msg::PositionConstraint.
Definition: kinematic_constraint.cpp:338
kinematic_constraints::PositionConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:498
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:643
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:440
kinematic_constraints::PositionConstraint::hasLinkOffset
bool hasLinkOffset() const
If the constraint is enabled and the link offset is substantially different than zero.
Definition: kinematic_constraint.h:607
kinematic_constraints::VisibilityConstraint
Class for constraints on the visibility relationship between a sensor and a target.
Definition: kinematic_constraint.h:761
kinematic_constraints::VisibilityConstraint::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:1042
kinematic_constraints::VisibilityConstraint::configure
bool configure(const moveit_msgs::msg::VisibilityConstraint &vc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::msg::VisibilityConstraint.
Definition: kinematic_constraint.cpp:780
moveit::core::LinkModel::getName
const std::string & getName() const
The name of this link.
Definition: link_model.h:86
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
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:370
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1339
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:656
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:361
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:515
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:59
moveit::core::Transforms::isFixedFrame
virtual bool isFixedFrame(const std::string &frame) const
Check whether a frame stays constant as the state of the robot model changes. This is true for any tr...
Definition: transforms.cpp:92
kinematic_constraint.h
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
pick.p
p
Definition: pick.py:62
robot_model_test_utils.h
kinematic_constraints::ConstraintEvaluationResult
Struct for containing the results of constraint evaluation.
Definition: kinematic_constraint.h:56
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
Whether or not the constraint or constraints were satisfied.
Definition: kinematic_constraint.h:70
kinematic_constraints::ConstraintEvaluationResult::distance
double distance
The distance evaluation from the constraint or constraints.
Definition: kinematic_constraint.h:71
TEST_F
TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
Definition: test_constraints.cpp:61
main
int main(int argc, char **argv)
Definition: test_constraints.cpp:937
Generated by doxygen 1.9.1