moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_constraint_samplers.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 */
36 
37 #include <moveit/planning_scene/planning_scene.h>
38 #include <moveit/kinematic_constraints/kinematic_constraint.h>
39 #include <moveit/constraint_samplers/default_constraint_samplers.h>
40 #include <moveit/constraint_samplers/union_constraint_sampler.h>
41 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
42 #include <moveit/constraint_samplers/constraint_sampler_tools.h>
43 #include <moveit/robot_state/conversions.h>
44 #include <moveit/utils/robot_model_test_utils.h>
45 
46 #include <geometric_shapes/shape_operations.h>
47 #include <visualization_msgs/msg/marker_array.hpp>
48 
49 #include <gmock/gmock.h>
50 #include <gtest/gtest.h>
51 #include <urdf_parser/urdf_parser.h>
52 #include <fstream>
53 #include <functional>
54 
55 #include "pr2_arm_kinematics_plugin.h"
56 
57 class LoadPlanningModelsPr2 : public testing::Test
58 {
59 protected:
60  kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const moveit::core::JointModelGroup* /*jmg*/)
61  {
62  {
63  return pr2_kinematics_plugin_right_arm_;
64  }
65  }
66 
67  kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const moveit::core::JointModelGroup* /*jmg*/)
68  {
69  {
70  return pr2_kinematics_plugin_left_arm_;
71  }
72  }
73 
74  void SetUp() override
75  {
76  node_ = rclcpp::Node::make_shared("test_constraint_samplers");
77  robot_model_ = moveit::core::loadTestingRobotModel("pr2");
78 
79  pr2_kinematics_plugin_right_arm_ = std::make_shared<pr2_arm_kinematics::PR2ArmKinematicsPlugin>();
80  pr2_kinematics_plugin_right_arm_->initialize(node_, *robot_model_, "right_arm", "torso_lift_link",
81  { "r_wrist_roll_link" }, .01);
82 
83  pr2_kinematics_plugin_left_arm_ = std::make_shared<pr2_arm_kinematics::PR2ArmKinematicsPlugin>();
84  pr2_kinematics_plugin_left_arm_->initialize(node_, *robot_model_, "left_arm", "torso_lift_link",
85  { "l_wrist_roll_link" }, .01);
86 
87  func_right_arm_ = [this](const moveit::core::JointModelGroup* jmg) { return getKinematicsSolverRightArm(jmg); };
88  func_left_arm_ = [this](const moveit::core::JointModelGroup* jmg) { return getKinematicsSolverLeftArm(jmg); };
89 
90  std::map<std::string, moveit::core::SolverAllocatorFn> allocators;
91  allocators["right_arm"] = func_right_arm_;
92  allocators["left_arm"] = func_left_arm_;
93  allocators["whole_body"] = func_right_arm_;
94  allocators["base"] = func_left_arm_;
95 
96  robot_model_->setKinematicsAllocators(allocators);
97 
98  ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
99  };
100 
101  void TearDown() override
102  {
103  }
104 
105 protected:
106  rclcpp::Node::SharedPtr node_;
107  moveit::core::RobotModelPtr robot_model_;
108  planning_scene::PlanningScenePtr ps_;
109  pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_right_arm_;
110  pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_left_arm_;
111  moveit::core::SolverAllocatorFn func_right_arm_;
112  moveit::core::SolverAllocatorFn func_left_arm_;
113 };
114 
115 TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
116 {
117  moveit::core::RobotState ks(robot_model_);
118  ks.setToDefaultValues();
119 
120  moveit::core::RobotState ks_const(robot_model_);
121  ks_const.setToDefaultValues();
122 
123  kinematic_constraints::JointConstraint jc1(robot_model_);
124  moveit_msgs::msg::JointConstraint jcm1;
125  // leaving off joint name
126  jcm1.position = 0.42;
127  jcm1.tolerance_above = 0.01;
128  jcm1.tolerance_below = 0.05;
129  jcm1.weight = 1.0;
130  EXPECT_FALSE(jc1.configure(jcm1));
131 
132  std::vector<kinematic_constraints::JointConstraint> js;
133  js.push_back(jc1);
134 
135  constraint_samplers::JointConstraintSampler jcs(ps_, "right_arm");
136  // no valid constraints
137  EXPECT_FALSE(jcs.configure(js));
138 
139  // testing that this does the right thing
140  jcm1.joint_name = "r_shoulder_pan_joint";
141  EXPECT_TRUE(jc1.configure(jcm1));
142  js.push_back(jc1);
143  EXPECT_TRUE(jcs.configure(js));
144  EXPECT_EQ(jcs.getConstrainedJointCount(), 1u);
145  EXPECT_EQ(jcs.getUnconstrainedJointCount(), 6u);
146  EXPECT_TRUE(jcs.sample(ks, ks, 1));
147 
148  for (int t = 0; t < 100; ++t)
149  {
150  EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
151  EXPECT_TRUE(jc1.decide(ks).satisfied);
152  }
153 
154  // redoing the configure leads to 6 unconstrained variables as well
155  EXPECT_TRUE(jcs.configure(js));
156  EXPECT_EQ(jcs.getUnconstrainedJointCount(), 6u);
157 
158  kinematic_constraints::JointConstraint jc2(robot_model_);
159 
160  moveit_msgs::msg::JointConstraint jcm2;
161  jcm2.joint_name = "r_shoulder_pan_joint";
162  jcm2.position = 0.54;
163  jcm2.tolerance_above = 0.01;
164  jcm2.tolerance_below = 0.01;
165  jcm2.weight = 1.0;
166  EXPECT_TRUE(jc2.configure(jcm2));
167  js.push_back(jc2);
168 
169  // creating a constraint that conflicts with the other (leaves no sampleable region)
170  EXPECT_FALSE(jcs.configure(js));
171  EXPECT_FALSE(jcs.sample(ks, ks_const, 1));
172 
173  // we can't sample for a different group
174  constraint_samplers::JointConstraintSampler jcs2(ps_, "arms");
175  jcs2.configure(js);
176  EXPECT_FALSE(jcs2.sample(ks, ks_const, 1));
177 
178  // not ok to not have any references to joints in this group in the constraints
179  constraint_samplers::JointConstraintSampler jcs3(ps_, "left_arm");
180  EXPECT_FALSE(jcs3.configure(js));
181 
182  // testing that the most restrictive bounds are used
183  js.clear();
184 
185  jcm1.position = .4;
186  jcm1.tolerance_above = .05;
187  jcm1.tolerance_below = .05;
188  jcm2.position = .4;
189  jcm2.tolerance_above = .1;
190  jcm2.tolerance_below = .1;
191  EXPECT_TRUE(jc1.configure(jcm1));
192  EXPECT_TRUE(jc2.configure(jcm2));
193  js.push_back(jc1);
194  js.push_back(jc2);
195 
196  EXPECT_TRUE(jcs.configure(js));
197 
198  // should always be within narrower constraints
199  for (int t = 0; t < 100; ++t)
200  {
201  EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
202  EXPECT_TRUE(jc1.decide(ks).satisfied);
203  }
204 
205  // too narrow range outside of joint limits
206  js.clear();
207 
208  jcm1.position = -3.1;
209  jcm1.tolerance_above = .05;
210  jcm1.tolerance_below = .05;
211 
212  // the joint configuration corrects this
213  EXPECT_TRUE(jc1.configure(jcm1));
214  js.push_back(jc1);
215  EXPECT_TRUE(jcs.configure(js));
216 
217  // partially overlapping regions will also work
218  js.clear();
219  jcm1.position = .35;
220  jcm1.tolerance_above = .05;
221  jcm1.tolerance_below = .05;
222  jcm2.position = .45;
223  jcm2.tolerance_above = .05;
224  jcm2.tolerance_below = .05;
225  EXPECT_TRUE(jc1.configure(jcm1));
226  EXPECT_TRUE(jc2.configure(jcm2));
227  js.push_back(jc1);
228  js.push_back(jc2);
229 
230  // leads to a min and max of .4, so all samples should be exactly .4
231  EXPECT_TRUE(jcs.configure(js));
232  for (int t = 0; t < 100; ++t)
233  {
234  EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
235  std::map<std::string, double> var_values;
236  EXPECT_NEAR(ks.getVariablePosition("r_shoulder_pan_joint"), .4, std::numeric_limits<double>::epsilon());
237  EXPECT_TRUE(jc1.decide(ks).satisfied);
238  EXPECT_TRUE(jc2.decide(ks).satisfied);
239  }
240 
241  // this leads to a larger sampleable region
242  jcm1.position = .38;
243  jcm2.position = .42;
244  EXPECT_TRUE(jc1.configure(jcm1));
245  EXPECT_TRUE(jc2.configure(jcm2));
246  js.push_back(jc1);
247  js.push_back(jc2);
248  EXPECT_TRUE(jcs.configure(js));
249  for (int t = 0; t < 100; ++t)
250  {
251  EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
252  EXPECT_TRUE(jc1.decide(ks).satisfied);
253  EXPECT_TRUE(jc2.decide(ks).satisfied);
254  }
255 }
256 
257 TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple)
258 {
259  moveit::core::Transforms& tf = ps_->getTransformsNonConst();
260 
261  kinematic_constraints::PositionConstraint pc(robot_model_);
262  moveit_msgs::msg::PositionConstraint pcm;
263 
264  pcm.link_name = "l_wrist_roll_link";
265  pcm.target_point_offset.x = 0;
266  pcm.target_point_offset.y = 0;
267  pcm.target_point_offset.z = 0;
268  pcm.constraint_region.primitives.resize(1);
269  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
270  pcm.constraint_region.primitives[0].dimensions.resize(1);
271  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
272 
273  pcm.constraint_region.primitive_poses.resize(1);
274  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
275  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
276  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
277  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
278  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
279  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
280  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
281  pcm.weight = 1.0;
282 
283  EXPECT_FALSE(pc.configure(pcm, tf));
284 
285  constraint_samplers::IKConstraintSampler ik_bad(ps_, "l_arm");
286  EXPECT_FALSE(ik_bad.isValid());
287 
288  constraint_samplers::IKConstraintSampler iks(ps_, "left_arm");
289  EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose()));
290  EXPECT_FALSE(iks.isValid());
291 
292  EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
293 
294  pcm.header.frame_id = robot_model_->getModelFrame();
295  EXPECT_TRUE(pc.configure(pcm, tf));
296  EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
297 
298  // ik link not in this group
299  constraint_samplers::IKConstraintSampler ik_bad_2(ps_, "right_arm");
300  EXPECT_FALSE(ik_bad_2.configure(constraint_samplers::IKSamplingPose(pc)));
301  EXPECT_FALSE(ik_bad_2.isValid());
302 
303  // not the ik link
304  pcm.link_name = "l_shoulder_pan_link";
305  EXPECT_TRUE(pc.configure(pcm, tf));
306  EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
307 
308  // solver for base doesn't cover group
309  constraint_samplers::IKConstraintSampler ik_base(ps_, "base");
310  pcm.link_name = "l_wrist_roll_link";
311  EXPECT_TRUE(pc.configure(pcm, tf));
312  EXPECT_FALSE(ik_base.configure(constraint_samplers::IKSamplingPose(pc)));
313  EXPECT_FALSE(ik_base.isValid());
314 
315  // shouldn't work as no direct constraint solver
316  constraint_samplers::IKConstraintSampler ik_arms(ps_, "arms");
317  EXPECT_FALSE(iks.isValid());
318 }
319 
320 TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler)
321 {
322  moveit::core::RobotState ks(robot_model_);
323  ks.setToDefaultValues();
324  ks.update();
325  moveit::core::RobotState ks_const(robot_model_);
326  ks_const.setToDefaultValues();
327  ks_const.update();
328 
329  moveit::core::Transforms& tf = ps_->getTransformsNonConst();
330 
331  kinematic_constraints::OrientationConstraint oc(robot_model_);
332  moveit_msgs::msg::OrientationConstraint ocm;
333 
334  ocm.link_name = "r_wrist_roll_link";
335  ocm.header.frame_id = ocm.link_name;
336  ocm.orientation.x = 0.5;
337  ocm.orientation.y = 0.5;
338  ocm.orientation.z = 0.5;
339  ocm.orientation.w = 0.5;
340  ocm.absolute_x_axis_tolerance = 0.01;
341  ocm.absolute_y_axis_tolerance = 0.01;
342  ocm.absolute_z_axis_tolerance = 0.01;
343  ocm.weight = 1.0;
344  ocm.parameterization = moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES;
345 
346  EXPECT_TRUE(oc.configure(ocm, tf));
347 
348  bool p1 = oc.decide(ks).satisfied;
349  EXPECT_FALSE(p1);
350 
351  ocm.header.frame_id = robot_model_->getModelFrame();
352  EXPECT_TRUE(oc.configure(ocm, tf));
353 
354  constraint_samplers::IKConstraintSampler iks(ps_, "right_arm");
355  EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(oc)));
356  for (int t = 0; t < 100; ++t)
357  {
358  ks.update();
359  EXPECT_TRUE(iks.sample(ks, ks_const, 100));
360  EXPECT_TRUE(oc.decide(ks).satisfied);
361  }
362 
363  // test another parameterization for orientation constraints
364  ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
365  EXPECT_TRUE(oc.configure(ocm, tf));
366 
367  EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(oc)));
368  for (int t = 0; t < 100; ++t)
369  {
370  ks.update();
371  EXPECT_TRUE(iks.sample(ks, ks_const, 100));
372  EXPECT_TRUE(oc.decide(ks).satisfied);
373  }
374 }
375 
376 TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid)
377 {
378  moveit::core::RobotState ks(robot_model_);
379  ks.setToDefaultValues();
380  ks.update();
381  moveit::core::RobotState ks_const(robot_model_);
382  ks_const.setToDefaultValues();
383  ks_const.update();
384 
385  moveit::core::Transforms& tf = ps_->getTransformsNonConst();
386 
387  kinematic_constraints::PositionConstraint pc(robot_model_);
388  moveit_msgs::msg::PositionConstraint pcm;
389 
390  pcm.link_name = "l_wrist_roll_link";
391  pcm.target_point_offset.x = 0;
392  pcm.target_point_offset.y = 0;
393  pcm.target_point_offset.z = 0;
394  pcm.constraint_region.primitives.resize(1);
395  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
396  pcm.constraint_region.primitives[0].dimensions.resize(1);
397  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
398 
399  pcm.header.frame_id = robot_model_->getModelFrame();
400 
401  pcm.constraint_region.primitive_poses.resize(1);
402  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
403  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
404  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
405  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
406  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
407  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
408  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
409  pcm.weight = 1.0;
410 
411  EXPECT_TRUE(pc.configure(pcm, tf));
412 
413  kinematic_constraints::OrientationConstraint oc(robot_model_);
414  moveit_msgs::msg::OrientationConstraint ocm;
415 
416  ocm.link_name = "l_wrist_roll_link";
417  ocm.header.frame_id = robot_model_->getModelFrame();
418  ocm.orientation.x = 0.0;
419  ocm.orientation.y = 0.0;
420  ocm.orientation.z = 0.0;
421  ocm.orientation.w = 1.0;
422  ocm.absolute_x_axis_tolerance = 0.2;
423  ocm.absolute_y_axis_tolerance = 0.1;
424  ocm.absolute_z_axis_tolerance = 0.4;
425  ocm.weight = 1.0;
426 
427  EXPECT_TRUE(oc.configure(ocm, tf));
428 
429  constraint_samplers::IKConstraintSampler iks1(ps_, "left_arm");
430  EXPECT_TRUE(iks1.configure(constraint_samplers::IKSamplingPose(pc, oc)));
431  for (int t = 0; t < 100; ++t)
432  {
433  EXPECT_TRUE(iks1.sample(ks, ks_const, 100));
434  EXPECT_TRUE(pc.decide(ks).satisfied);
435  EXPECT_TRUE(oc.decide(ks).satisfied);
436  }
437 
438  constraint_samplers::IKConstraintSampler iks2(ps_, "left_arm");
439  EXPECT_TRUE(iks2.configure(constraint_samplers::IKSamplingPose(pc)));
440  for (int t = 0; t < 100; ++t)
441  {
442  EXPECT_TRUE(iks2.sample(ks, ks_const, 100));
443  EXPECT_TRUE(pc.decide(ks).satisfied);
444  }
445 
446  constraint_samplers::IKConstraintSampler iks3(ps_, "left_arm");
447  EXPECT_TRUE(iks3.configure(constraint_samplers::IKSamplingPose(oc)));
448  for (int t = 0; t < 100; ++t)
449  {
450  EXPECT_TRUE(iks3.sample(ks, ks_const, 100));
451  EXPECT_TRUE(oc.decide(ks).satisfied);
452  }
453 }
454 
455 TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
456 {
457  moveit::core::RobotState ks(robot_model_);
458  ks.setToDefaultValues();
459  ks.update();
460 
461  moveit::core::RobotState ks_const(robot_model_);
462  ks_const.setToDefaultValues();
463  ks_const.update();
464 
465  moveit::core::Transforms& tf = ps_->getTransformsNonConst();
466 
467  kinematic_constraints::JointConstraint jc1(robot_model_);
468 
469  std::map<std::string, double> state_values;
470 
471  moveit_msgs::msg::JointConstraint torso_constraint;
472  torso_constraint.joint_name = "torso_lift_joint";
473  torso_constraint.position = ks.getVariablePosition("torso_lift_joint");
474  torso_constraint.tolerance_above = 0.01;
475  torso_constraint.tolerance_below = 0.01;
476  torso_constraint.weight = 1.0;
477  EXPECT_TRUE(jc1.configure(torso_constraint));
478 
479  kinematic_constraints::JointConstraint jc2(robot_model_);
480  moveit_msgs::msg::JointConstraint jcm2;
481  jcm2.joint_name = "r_elbow_flex_joint";
482  jcm2.position = ks.getVariablePosition("r_elbow_flex_joint");
483  jcm2.tolerance_above = 0.01;
484  jcm2.tolerance_below = 0.01;
485  jcm2.weight = 1.0;
486  EXPECT_TRUE(jc2.configure(jcm2));
487 
488  moveit_msgs::msg::PositionConstraint pcm;
489 
490  pcm.link_name = "l_wrist_roll_link";
491  pcm.target_point_offset.x = 0;
492  pcm.target_point_offset.y = 0;
493  pcm.target_point_offset.z = 0;
494  pcm.constraint_region.primitives.resize(1);
495  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
496  pcm.constraint_region.primitives[0].dimensions.resize(1);
497  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
498 
499  pcm.constraint_region.primitive_poses.resize(1);
500  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
501  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
502  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
503  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
504  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
505  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
506  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
507  pcm.weight = 1.0;
508 
509  pcm.header.frame_id = robot_model_->getModelFrame();
510 
511  moveit_msgs::msg::OrientationConstraint ocm;
512 
513  ocm.link_name = "l_wrist_roll_link";
514  ocm.header.frame_id = robot_model_->getModelFrame();
515  ocm.orientation.x = 0.0;
516  ocm.orientation.y = 0.0;
517  ocm.orientation.z = 0.0;
518  ocm.orientation.w = 1.0;
519  ocm.absolute_x_axis_tolerance = 0.2;
520  ocm.absolute_y_axis_tolerance = 0.1;
521  ocm.absolute_z_axis_tolerance = 0.4;
522  ocm.weight = 1.0;
523 
524  std::vector<kinematic_constraints::JointConstraint> js;
525  js.push_back(jc1);
526 
527  constraint_samplers::JointConstraintSamplerPtr jcsp(
528  new constraint_samplers::JointConstraintSampler(ps_, "arms_and_torso"));
529  EXPECT_TRUE(jcsp->configure(js));
530 
531  std::vector<kinematic_constraints::JointConstraint> js2;
532  js2.push_back(jc2);
533 
534  constraint_samplers::JointConstraintSamplerPtr jcsp2 =
535  std::make_shared<constraint_samplers::JointConstraintSampler>(ps_, "arms");
536  EXPECT_TRUE(jcsp2->configure(js2));
537 
538  kinematic_constraints::PositionConstraint pc(robot_model_);
539  EXPECT_TRUE(pc.configure(pcm, tf));
540 
541  kinematic_constraints::OrientationConstraint oc(robot_model_);
542  EXPECT_TRUE(oc.configure(ocm, tf));
543 
544  constraint_samplers::IKConstraintSamplerPtr iksp =
545  std::make_shared<constraint_samplers::IKConstraintSampler>(ps_, "left_arm");
546  EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
547  EXPECT_TRUE(iksp->isValid());
548 
549  std::vector<constraint_samplers::ConstraintSamplerPtr> cspv;
550  cspv.push_back(jcsp2);
551  cspv.push_back(iksp);
552  cspv.push_back(jcsp);
553 
554  constraint_samplers::UnionConstraintSampler ucs(ps_, "arms_and_torso", cspv);
555 
556  // should have reordered to place whole body first
557  constraint_samplers::JointConstraintSampler* jcs =
558  dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[0].get());
559  EXPECT_TRUE(jcs);
560  EXPECT_EQ(jcs->getJointModelGroup()->getName(), "arms_and_torso");
561 
562  constraint_samplers::JointConstraintSampler* jcs2 =
563  dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[1].get());
564  EXPECT_TRUE(jcs2);
565  EXPECT_EQ(jcs2->getJointModelGroup()->getName(), "arms");
566 
567  for (int t = 0; t < 100; ++t)
568  {
569  EXPECT_TRUE(ucs.sample(ks, ks_const, 100));
570  ks.update();
571  ks.updateLinkTransforms(); // Returned samples have dirty link transforms.
572  ks_const.update();
573  EXPECT_TRUE(jc1.decide(ks).satisfied);
574  EXPECT_TRUE(jc2.decide(ks).satisfied);
575  EXPECT_TRUE(pc.decide(ks).satisfied);
576  }
577 
578  // now we add a position constraint on right arm
579  pcm.link_name = "r_wrist_roll_link";
580  ocm.link_name = "r_wrist_roll_link";
581  cspv.clear();
582 
583  kinematic_constraints::PositionConstraint pc2(robot_model_);
584  EXPECT_TRUE(pc2.configure(pcm, tf));
585 
586  kinematic_constraints::OrientationConstraint oc2(robot_model_);
587  EXPECT_TRUE(oc2.configure(ocm, tf));
588 
589  constraint_samplers::IKConstraintSamplerPtr iksp2 =
590  std::make_shared<constraint_samplers::IKConstraintSampler>(ps_, "right_arm");
591  EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2)));
592  EXPECT_TRUE(iksp2->isValid());
593 
594  // totally disjoint, so should break ties based on alphabetical order
595  cspv.clear();
596  cspv.push_back(iksp2);
597  cspv.push_back(iksp);
598 
599  constraint_samplers::UnionConstraintSampler ucs2(ps_, "arms_and_torso", cspv);
600 
601  constraint_samplers::IKConstraintSampler* ikcs_test =
602  dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs2.getSamplers()[0].get());
603  ASSERT_TRUE(ikcs_test);
604  EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "left_arm");
605 
606  // now we make left depends on right, right should stay first
607  pcm.link_name = "l_wrist_roll_link";
608  ocm.link_name = "l_wrist_roll_link";
609  pcm.header.frame_id = "r_wrist_roll_link";
610  ocm.header.frame_id = "r_wrist_roll_link";
611  EXPECT_TRUE(pc.configure(pcm, tf));
612  EXPECT_TRUE(oc.configure(ocm, tf));
613  ASSERT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
614 
615  cspv.clear();
616  cspv.push_back(iksp2);
617  cspv.push_back(iksp);
618 
619  constraint_samplers::UnionConstraintSampler ucs3(ps_, "arms_and_torso", cspv);
620 
621  ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs3.getSamplers()[0].get());
622  EXPECT_TRUE(ikcs_test);
623  EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "right_arm");
624 }
625 
626 TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager)
627 {
628  moveit::core::RobotState ks(robot_model_);
629  ks.setToDefaultValues();
630  ks.update();
631  moveit::core::RobotState ks_const(robot_model_);
632  ks_const.setToDefaultValues();
633  ks_const.update();
634 
635  kinematic_constraints::PositionConstraint pc(robot_model_);
636 
637  moveit_msgs::msg::PositionConstraint pcm;
638  pcm.link_name = "l_wrist_roll_link";
639  pcm.target_point_offset.x = 0;
640  pcm.target_point_offset.y = 0;
641  pcm.target_point_offset.z = 0;
642  pcm.constraint_region.primitives.resize(1);
643  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
644  pcm.constraint_region.primitives[0].dimensions.resize(1);
645  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
646 
647  pcm.header.frame_id = robot_model_->getModelFrame();
648 
649  pcm.constraint_region.primitive_poses.resize(1);
650  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
651  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
652  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
653  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
654  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
655  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
656  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
657  pcm.weight = 1.0;
658 
659  moveit_msgs::msg::OrientationConstraint ocm;
660 
661  ocm.link_name = "l_wrist_roll_link";
662  ocm.header.frame_id = robot_model_->getModelFrame();
663  ocm.orientation.x = 0.0;
664  ocm.orientation.y = 0.0;
665  ocm.orientation.z = 0.0;
666  ocm.orientation.w = 1.0;
667  ocm.absolute_x_axis_tolerance = 0.2;
668  ocm.absolute_y_axis_tolerance = 0.1;
669  ocm.absolute_z_axis_tolerance = 0.4;
670  ocm.weight = 1.0;
671 
672  // test the automatic construction of constraint sampler
673  moveit_msgs::msg::Constraints c;
674  c.position_constraints.push_back(pcm);
675  c.orientation_constraints.push_back(ocm);
676 
677  constraint_samplers::ConstraintSamplerPtr s =
678  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", c);
679  EXPECT_TRUE(s != nullptr);
680  constraint_samplers::IKConstraintSampler* iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
681  ASSERT_TRUE(iks);
682  ASSERT_TRUE(static_cast<bool>(iks->getPositionConstraint()));
683  ASSERT_TRUE(static_cast<bool>(iks->getOrientationConstraint()));
684 
685  static const int NT = 100;
686  int succ = 0;
687  for (int t = 0; t < NT; ++t)
688  {
689  EXPECT_TRUE(s->sample(ks, ks_const, 100));
690  EXPECT_TRUE(iks->getPositionConstraint()->decide(ks).satisfied);
691  EXPECT_TRUE(iks->getOrientationConstraint()->decide(ks).satisfied);
692  if (s->sample(ks, ks_const, 1))
693  succ++;
694  }
695  RCLCPP_INFO(rclcpp::get_logger("test_constraint_samplers"),
696  "Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf",
697  static_cast<double>(succ) / static_cast<double>(NT));
698 
699  // add additional ocm with smaller volume
700  ocm.absolute_x_axis_tolerance = 0.1;
701 
702  c.orientation_constraints.push_back(ocm);
703 
704  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", c);
705  EXPECT_TRUE(s != nullptr);
706 
707  iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
708  ASSERT_TRUE(iks);
709  ASSERT_TRUE(static_cast<bool>(iks->getOrientationConstraint()));
710  EXPECT_NEAR(iks->getOrientationConstraint()->getXAxisTolerance(), .1, .0001);
711 }
712 
713 TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager)
714 {
715  moveit::core::RobotState ks(robot_model_);
716  ks.setToDefaultValues();
717  ks.update();
718 
719  moveit_msgs::msg::Constraints con;
720  con.joint_constraints.resize(1);
721 
722  con.joint_constraints[0].joint_name = "l_shoulder_pan_joint";
723  con.joint_constraints[0].position = 0.54;
724  con.joint_constraints[0].tolerance_above = 0.01;
725  con.joint_constraints[0].tolerance_below = 0.01;
726  con.joint_constraints[0].weight = 1.0;
727 
728  constraint_samplers::ConstraintSamplerPtr s =
729  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "right_arm", con);
730  EXPECT_FALSE(static_cast<bool>(s));
731  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
732  EXPECT_TRUE(static_cast<bool>(s));
733 
734  con.joint_constraints.resize(7);
735 
736  con.joint_constraints[1].joint_name = "l_shoulder_lift_joint";
737  con.joint_constraints[1].position = 0.54;
738  con.joint_constraints[1].tolerance_above = 0.01;
739  con.joint_constraints[1].tolerance_below = 0.01;
740  con.joint_constraints[1].weight = 1.0;
741 
742  con.joint_constraints[2].joint_name = "l_upper_arm_roll_joint";
743  con.joint_constraints[2].position = 0.54;
744  con.joint_constraints[2].tolerance_above = 0.01;
745  con.joint_constraints[2].tolerance_below = 0.01;
746  con.joint_constraints[2].weight = 1.0;
747 
748  con.joint_constraints[3].joint_name = "l_elbow_flex_joint";
749  con.joint_constraints[3].position = -0.54;
750  con.joint_constraints[3].tolerance_above = 0.01;
751  con.joint_constraints[3].tolerance_below = 0.01;
752  con.joint_constraints[3].weight = 1.0;
753 
754  con.joint_constraints[4].joint_name = "l_forearm_roll_joint";
755  con.joint_constraints[4].position = 0.54;
756  con.joint_constraints[4].tolerance_above = 0.01;
757  con.joint_constraints[4].tolerance_below = 0.01;
758  con.joint_constraints[4].weight = 1.0;
759 
760  con.joint_constraints[5].joint_name = "l_wrist_flex_joint";
761  con.joint_constraints[5].position = -0.54;
762  con.joint_constraints[5].tolerance_above = 0.05;
763  con.joint_constraints[5].tolerance_below = 0.05;
764  con.joint_constraints[5].weight = 1.0;
765 
766  // an extra constraint on one link, but this shouldn't change anything
767  con.joint_constraints[6].joint_name = "l_wrist_flex_joint";
768  con.joint_constraints[6].position = -0.56;
769  con.joint_constraints[6].tolerance_above = 0.01;
770  con.joint_constraints[6].tolerance_below = 0.01;
771  con.joint_constraints[6].weight = 1.0;
772 
773  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
774  EXPECT_TRUE(static_cast<bool>(s));
775 
776  con.position_constraints.resize(1);
777 
778  // intentionally making wrong wrist
779  con.position_constraints[0].link_name = "r_wrist_roll_link";
780  con.position_constraints[0].target_point_offset.x = 0;
781  con.position_constraints[0].target_point_offset.y = 0;
782  con.position_constraints[0].target_point_offset.z = 0;
783  con.position_constraints[0].constraint_region.primitives.resize(1);
784  con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
785  con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
786  con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
787 
788  con.position_constraints[0].header.frame_id = robot_model_->getModelFrame();
789 
790  con.position_constraints[0].constraint_region.primitive_poses.resize(1);
791  con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
792  con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
793  con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
794  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
795  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
796  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
797  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
798  con.position_constraints[0].weight = 1.0;
799 
800  // this still works, but we should get a JointConstraintSampler
801  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
802  EXPECT_TRUE(static_cast<bool>(s));
803  constraint_samplers::JointConstraintSampler* jcs =
804  dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
805  EXPECT_TRUE(jcs);
806 
807  con.position_constraints[0].link_name = "l_wrist_roll_link";
808  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
809  EXPECT_TRUE(static_cast<bool>(s));
810  jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
811  EXPECT_FALSE(jcs);
812  constraint_samplers::IKConstraintSampler* iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
813  EXPECT_FALSE(iks);
814 
815  // we should get a union constraint sampler
816  constraint_samplers::UnionConstraintSampler* ucs =
817  dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
818  EXPECT_TRUE(ucs);
819 
820  con.orientation_constraints.resize(1);
821 
822  // again, screwing this up intentionally
823  con.orientation_constraints[0].link_name = "r_wrist_roll_link";
824  con.orientation_constraints[0].header.frame_id = robot_model_->getModelFrame();
825  con.orientation_constraints[0].orientation.x = 0.0;
826  con.orientation_constraints[0].orientation.y = 0.0;
827  con.orientation_constraints[0].orientation.z = 0.0;
828  con.orientation_constraints[0].orientation.w = 1.0;
829  con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
830  con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
831  con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
832  con.orientation_constraints[0].weight = 1.0;
833 
834  // we still get an IK sampler with just the position constraint
835  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
836  EXPECT_TRUE(static_cast<bool>(s));
837  ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
838  ASSERT_TRUE(ucs);
839  jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs->getSamplers()[0].get());
840  iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
841 
842  ASSERT_TRUE(iks);
843  ASSERT_TRUE(jcs);
844  EXPECT_TRUE(static_cast<bool>(iks->getPositionConstraint()));
845  EXPECT_FALSE(iks->getOrientationConstraint());
846 
847  con.orientation_constraints[0].link_name = "l_wrist_roll_link";
848 
849  // now they both are good
850  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
851  EXPECT_TRUE(static_cast<bool>(s));
852  ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
853  iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
854  ASSERT_TRUE(iks);
855  EXPECT_TRUE(static_cast<bool>(iks->getPositionConstraint()));
856  EXPECT_TRUE(static_cast<bool>(iks->getOrientationConstraint()));
857 
858  // now just the orientation constraint is good
859  con.position_constraints[0].link_name = "r_wrist_roll_link";
860  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
861  ASSERT_TRUE(static_cast<bool>(s));
862  ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
863  iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
864  ASSERT_TRUE(iks);
865  EXPECT_FALSE(iks->getPositionConstraint());
866  EXPECT_TRUE(static_cast<bool>(iks->getOrientationConstraint()));
867 
868  // now if we constraint all the joints, we get a joint constraint sampler
869  con.joint_constraints.resize(8);
870  con.joint_constraints[7].joint_name = "l_wrist_roll_joint";
871  con.joint_constraints[7].position = 0.54;
872  con.joint_constraints[7].tolerance_above = 0.01;
873  con.joint_constraints[7].tolerance_below = 0.01;
874  con.joint_constraints[7].weight = 1.0;
875 
876  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
877  EXPECT_TRUE(static_cast<bool>(s));
878  jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
879  ASSERT_TRUE(jcs);
880 }
881 
882 TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager)
883 {
884  moveit::core::RobotState ks(robot_model_);
885  ks.setToDefaultValues();
886  ks.update();
887  moveit::core::RobotState ks_const(robot_model_);
888  ks_const.setToDefaultValues();
889  ks_const.update();
890 
891  moveit_msgs::msg::Constraints con;
892  con.joint_constraints.resize(1);
893 
894  con.joint_constraints[0].joint_name = "torso_lift_joint";
895  con.joint_constraints[0].position = ks.getVariablePosition("torso_lift_joint");
896  con.joint_constraints[0].tolerance_above = 0.01;
897  con.joint_constraints[0].tolerance_below = 0.01;
898  con.joint_constraints[0].weight = 1.0;
899 
900  kinematic_constraints::JointConstraint jc(robot_model_);
901  EXPECT_TRUE(jc.configure(con.joint_constraints[0]));
902 
903  con.position_constraints.resize(1);
904 
905  con.position_constraints[0].link_name = "l_wrist_roll_link";
906  con.position_constraints[0].target_point_offset.x = 0;
907  con.position_constraints[0].target_point_offset.y = 0;
908  con.position_constraints[0].target_point_offset.z = 0;
909  con.position_constraints[0].constraint_region.primitives.resize(1);
910  con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
911  con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
912  con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
913 
914  con.position_constraints[0].constraint_region.primitive_poses.resize(1);
915  con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
916  con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
917  con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
918  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
919  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
920  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
921  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
922  con.position_constraints[0].weight = 1.0;
923 
924  con.position_constraints[0].header.frame_id = robot_model_->getModelFrame();
925 
926  con.orientation_constraints.resize(1);
927  con.orientation_constraints[0].link_name = "l_wrist_roll_link";
928  con.orientation_constraints[0].header.frame_id = robot_model_->getModelFrame();
929  con.orientation_constraints[0].orientation.x = 0.0;
930  con.orientation_constraints[0].orientation.y = 0.0;
931  con.orientation_constraints[0].orientation.z = 0.0;
932  con.orientation_constraints[0].orientation.w = 1.0;
933  con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
934  con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
935  con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
936  con.orientation_constraints[0].weight = 1.0;
937 
938  constraint_samplers::ConstraintSamplerPtr s =
939  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms_and_torso", con);
940 
941  constraint_samplers::UnionConstraintSampler* ucs =
942  dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
943  ASSERT_TRUE(ucs);
944 
945  constraint_samplers::IKConstraintSampler* ikcs_test =
946  dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
947  ASSERT_TRUE(ikcs_test);
948 
949  for (int t = 0; t < 1; ++t)
950  {
951  EXPECT_TRUE(s->sample(ks, ks_const, 100));
952  EXPECT_TRUE(jc.decide(ks).satisfied);
953  EXPECT_TRUE(ikcs_test->getPositionConstraint()->decide(ks).satisfied);
954  EXPECT_TRUE(ikcs_test->getOrientationConstraint()->decide(ks).satisfied);
955  }
956 }
957 
958 TEST_F(LoadPlanningModelsPr2, SubgroupJointConstraintsSamplerManager)
959 {
960  moveit::core::RobotState ks(robot_model_);
961  ks.setToDefaultValues();
962  ks.update();
963  moveit::core::RobotState ks_const(robot_model_);
964  ks_const.setToDefaultValues();
965  ks_const.update();
966 
967  kinematic_constraints::JointConstraint jc1(robot_model_);
968  moveit_msgs::msg::JointConstraint jcm1;
969  jcm1.joint_name = "head_pan_joint";
970  jcm1.position = 0.42;
971  jcm1.tolerance_above = 0.01;
972  jcm1.tolerance_below = 0.05;
973  jcm1.weight = 1.0;
974  EXPECT_TRUE(jc1.configure(jcm1));
975 
976  kinematic_constraints::JointConstraint jc2(robot_model_);
977  moveit_msgs::msg::JointConstraint jcm2;
978  jcm2.joint_name = "l_shoulder_pan_joint";
979  jcm2.position = 0.9;
980  jcm2.tolerance_above = 0.1;
981  jcm2.tolerance_below = 0.05;
982  jcm2.weight = 1.0;
983  EXPECT_TRUE(jc2.configure(jcm2));
984 
985  kinematic_constraints::JointConstraint jc3(robot_model_);
986  moveit_msgs::msg::JointConstraint jcm3;
987  jcm3.joint_name = "r_wrist_roll_joint";
988  jcm3.position = 0.7;
989  jcm3.tolerance_above = 0.14;
990  jcm3.tolerance_below = 0.005;
991  jcm3.weight = 1.0;
992  EXPECT_TRUE(jc3.configure(jcm3));
993 
994  kinematic_constraints::JointConstraint jc4(robot_model_);
995  moveit_msgs::msg::JointConstraint jcm4;
996  jcm4.joint_name = "torso_lift_joint";
997  jcm4.position = 0.2;
998  jcm4.tolerance_above = 0.09;
999  jcm4.tolerance_below = 0.01;
1000  jcm4.weight = 1.0;
1001  EXPECT_TRUE(jc4.configure(jcm4));
1002 
1003  std::vector<kinematic_constraints::JointConstraint> js;
1004  js.push_back(jc1);
1005  js.push_back(jc2);
1006  js.push_back(jc3);
1007  js.push_back(jc4);
1008 
1009  constraint_samplers::JointConstraintSampler jcs(ps_, "arms");
1010  jcs.configure(js);
1011  EXPECT_EQ(jcs.getConstrainedJointCount(), 2u);
1012  EXPECT_EQ(jcs.getUnconstrainedJointCount(), 12u);
1013 
1014  for (int t = 0; t < 100; ++t)
1015  {
1016  EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
1017  EXPECT_TRUE(jc2.decide(ks).satisfied);
1018  EXPECT_TRUE(jc3.decide(ks).satisfied);
1019  }
1020 
1021  // test the automatic construction of constraint sampler
1022  moveit_msgs::msg::Constraints c;
1023 
1024  // no constraints should give no sampler
1025  constraint_samplers::ConstraintSamplerPtr s0 =
1026  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms", c);
1027  EXPECT_TRUE(s0 == nullptr);
1028 
1029  // add the constraints
1030  c.joint_constraints.push_back(jcm1);
1031  c.joint_constraints.push_back(jcm2);
1032  c.joint_constraints.push_back(jcm3);
1033  c.joint_constraints.push_back(jcm4);
1034 
1035  constraint_samplers::ConstraintSamplerPtr s =
1036  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms", c);
1037  EXPECT_TRUE(s != nullptr);
1038 
1039  // test the generated sampler
1040  for (int t = 0; t < 1000; ++t)
1041  {
1042  EXPECT_TRUE(s->sample(ks, ks_const, 1));
1043  EXPECT_TRUE(jc2.decide(ks).satisfied);
1044  EXPECT_TRUE(jc3.decide(ks).satisfied);
1045  }
1046 }
1047 
1048 TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler)
1049 {
1050  moveit_msgs::msg::Constraints c;
1051 
1052  moveit_msgs::msg::PositionConstraint pcm;
1053  pcm.link_name = "l_wrist_roll_link";
1054  pcm.target_point_offset.x = 0;
1055  pcm.target_point_offset.y = 0;
1056  pcm.target_point_offset.z = 0;
1057 
1058  pcm.constraint_region.primitives.resize(1);
1059  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
1060  pcm.constraint_region.primitives[0].dimensions.resize(1);
1061  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
1062 
1063  pcm.header.frame_id = robot_model_->getModelFrame();
1064 
1065  pcm.constraint_region.primitive_poses.resize(1);
1066  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
1067  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
1068  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
1069  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
1070  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
1071  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
1072  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
1073  pcm.weight = 1.0;
1074  c.position_constraints.push_back(pcm);
1075 
1076  moveit_msgs::msg::OrientationConstraint ocm;
1077  ocm.link_name = "l_wrist_roll_link";
1078  ocm.header.frame_id = robot_model_->getModelFrame();
1079  ocm.orientation.x = 0.0;
1080  ocm.orientation.y = 0.0;
1081  ocm.orientation.z = 0.0;
1082  ocm.orientation.w = 1.0;
1083  ocm.absolute_x_axis_tolerance = 0.2;
1084  ocm.absolute_y_axis_tolerance = 0.1;
1085  ocm.absolute_z_axis_tolerance = 0.4;
1086  ocm.weight = 1.0;
1087  c.orientation_constraints.push_back(ocm);
1088 
1089  ocm.link_name = "r_wrist_roll_link";
1090  ocm.header.frame_id = robot_model_->getModelFrame();
1091  ocm.orientation.x = 0.0;
1092  ocm.orientation.y = 0.0;
1093  ocm.orientation.z = 0.0;
1094  ocm.orientation.w = 1.0;
1095  ocm.absolute_x_axis_tolerance = 0.01;
1096  ocm.absolute_y_axis_tolerance = 0.01;
1097  ocm.absolute_z_axis_tolerance = 0.01;
1098  ocm.weight = 1.0;
1099  c.orientation_constraints.push_back(ocm);
1100 
1101  moveit::core::Transforms& tf = ps_->getTransformsNonConst();
1102  constraint_samplers::ConstraintSamplerPtr s =
1103  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms", c);
1104  EXPECT_TRUE(static_cast<bool>(s));
1105  constraint_samplers::UnionConstraintSampler* ucs =
1106  dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
1107  EXPECT_TRUE(ucs);
1108 
1109  kinematic_constraints::KinematicConstraintSet kset(robot_model_);
1110  kset.add(c, tf);
1111 
1112  moveit::core::RobotState ks(robot_model_);
1113  ks.setToDefaultValues();
1114  ks.update();
1115  moveit::core::RobotState ks_const(robot_model_);
1116  ks_const.setToDefaultValues();
1117  ks_const.update();
1118 
1119  static const int NT = 100;
1120  int succ = 0;
1121  for (int t = 0; t < NT; ++t)
1122  {
1123  EXPECT_TRUE(s->sample(ks, ks_const, 1000));
1124  EXPECT_TRUE(kset.decide(ks).satisfied);
1125  if (s->sample(ks, ks_const, 1))
1126  succ++;
1127  }
1128  RCLCPP_INFO(rclcpp::get_logger("pr2_arm_kinematics_plugin"),
1129  "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf",
1130  static_cast<double>(succ) / static_cast<double>(NT));
1131 }
1132 
1133 TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSeeded)
1134 {
1135  constraint_samplers::JointConstraintSampler seeded_sampler1(ps_, "right_arm", 314159);
1136  kinematic_constraints::JointConstraint jc(robot_model_);
1137  moveit_msgs::msg::JointConstraint jcm;
1138  jcm.position = 0.42;
1139  jcm.tolerance_above = 0.01;
1140  jcm.tolerance_below = 0.05;
1141  jcm.weight = 1.0;
1142  jcm.joint_name = "r_shoulder_pan_joint";
1143  EXPECT_TRUE(jc.configure(jcm));
1144  std::vector<kinematic_constraints::JointConstraint> js;
1145  js.push_back(jc);
1146  EXPECT_TRUE(seeded_sampler1.configure(js));
1147 
1148  moveit::core::RobotState ks(robot_model_);
1149  ks.setToDefaultValues();
1150  EXPECT_TRUE(seeded_sampler1.sample(ks, ks, 1));
1151  const double* joint_positions = ks.getVariablePositions();
1152  const std::vector<double> joint_positions_v(joint_positions, joint_positions + ks.getVariableCount());
1153 
1154  constraint_samplers::JointConstraintSampler seeded_sampler2(ps_, "right_arm", 314159);
1155  EXPECT_TRUE(seeded_sampler2.configure(js));
1156  ks.setToDefaultValues();
1157  EXPECT_TRUE(seeded_sampler2.sample(ks, ks, 1));
1158  const double* joint_positions2 = ks.getVariablePositions();
1159  const std::vector<double> joint_positions_v2(joint_positions2, joint_positions2 + ks.getVariableCount());
1160  using namespace testing;
1161  EXPECT_THAT(joint_positions_v, ContainerEq(joint_positions_v2));
1162 
1163  constraint_samplers::JointConstraintSampler seeded_sampler3(ps_, "right_arm");
1164  EXPECT_TRUE(seeded_sampler3.configure(js));
1165  ks.setToDefaultValues();
1166  EXPECT_TRUE(seeded_sampler3.sample(ks, ks, 5));
1167  const double* joint_positions3 = ks.getVariablePositions();
1168  const std::vector<double> joint_positions_v3(joint_positions3, joint_positions3 + ks.getVariableCount());
1169  EXPECT_THAT(joint_positions_v, Not(ContainerEq(joint_positions_v3)));
1170  EXPECT_THAT(joint_positions_v2, Not(ContainerEq(joint_positions_v3)));
1171 }
1172 
1173 TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSeeded)
1174 {
1175  kinematic_constraints::PositionConstraint pc(robot_model_);
1176  moveit_msgs::msg::PositionConstraint pcm;
1177 
1178  pcm.link_name = "l_wrist_roll_link";
1179  pcm.target_point_offset.x = 0;
1180  pcm.target_point_offset.y = 0;
1181  pcm.target_point_offset.z = 0;
1182  pcm.constraint_region.primitives.resize(1);
1183  pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
1184  pcm.constraint_region.primitives[0].dimensions.resize(1);
1185  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
1186 
1187  pcm.constraint_region.primitive_poses.resize(1);
1188  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
1189  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
1190  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
1191  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
1192  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
1193  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
1194  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
1195  pcm.weight = 1.0;
1196 
1197  pcm.header.frame_id = robot_model_->getModelFrame();
1198  moveit::core::Transforms& tf = ps_->getTransformsNonConst();
1199  EXPECT_TRUE(pc.configure(pcm, tf));
1200 
1201  constraint_samplers::IKConstraintSampler seeded_sampler1(ps_, "left_arm", 265358);
1202  EXPECT_TRUE(seeded_sampler1.configure(constraint_samplers::IKSamplingPose(pc)));
1203 
1204  moveit::core::RobotState ks(robot_model_);
1205  ks.setToDefaultValues();
1206  ks.update();
1207 
1208  EXPECT_TRUE(seeded_sampler1.sample(ks, ks, 1));
1209  ks.update();
1210  bool found = false;
1211  const Eigen::Isometry3d root_to_left_tool1 = ks.getFrameTransform("l_gripper_tool_frame", &found);
1212  EXPECT_TRUE(found);
1213 
1214  constraint_samplers::IKConstraintSampler seeded_sampler2(ps_, "left_arm", 265358);
1215  EXPECT_TRUE(seeded_sampler2.configure(constraint_samplers::IKSamplingPose(pc)));
1216  ks.setToDefaultValues();
1217  ks.update();
1218  EXPECT_TRUE(seeded_sampler2.sample(ks, ks, 1));
1219  ks.update();
1220  found = false;
1221  const Eigen::Isometry3d root_to_left_tool2 = ks.getFrameTransform("l_gripper_tool_frame", &found);
1222  EXPECT_TRUE(found);
1223 
1224  constraint_samplers::IKConstraintSampler seeded_sampler3(ps_, "left_arm");
1225  EXPECT_TRUE(seeded_sampler3.configure(constraint_samplers::IKSamplingPose(pc)));
1226  ks.setToDefaultValues();
1227  ks.update();
1228  EXPECT_TRUE(seeded_sampler3.sample(ks, ks, 5));
1229  ks.update();
1230  found = false;
1231  const Eigen::Isometry3d root_to_left_tool3 = ks.getFrameTransform("l_gripper_tool_frame", &found);
1232  EXPECT_TRUE(found);
1233 
1234  EXPECT_TRUE((root_to_left_tool1 * root_to_left_tool2.inverse()).matrix().isIdentity(1e-7));
1235  EXPECT_FALSE((root_to_left_tool1 * root_to_left_tool3.inverse()).matrix().isIdentity(1e-7));
1236  EXPECT_FALSE((root_to_left_tool2 * root_to_left_tool3.inverse()).matrix().isIdentity(1e-7));
1237 }
1238 
1239 int main(int argc, char** argv)
1240 {
1241  rclcpp::init(argc, argv);
1242  testing::InitGoogleTest(&argc, argv);
1243  return RUN_ALL_TESTS();
1244 }
LoadPlanningModelsPr2::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_constraint_samplers.cpp:107
LoadPlanningModelsPr2::getKinematicsSolverLeftArm
kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const moveit::core::JointModelGroup *)
Definition: test_constraint_samplers.cpp:67
LoadPlanningModelsPr2::getKinematicsSolverRightArm
kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const moveit::core::JointModelGroup *)
Definition: test_constraint_samplers.cpp:60
LoadPlanningModelsPr2::node_
rclcpp::Node::SharedPtr node_
Definition: test_constraint_samplers.cpp:106
LoadPlanningModelsPr2::func_left_arm_
moveit::core::SolverAllocatorFn func_left_arm_
Definition: test_constraint_samplers.cpp:112
LoadPlanningModelsPr2::pr2_kinematics_plugin_right_arm_
pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_right_arm_
Definition: test_constraint_samplers.cpp:109
LoadPlanningModelsPr2::ps_
planning_scene::PlanningScenePtr ps_
Definition: test_constraint_samplers.cpp:108
LoadPlanningModelsPr2::func_right_arm_
moveit::core::SolverAllocatorFn func_right_arm_
Definition: test_constraint_samplers.cpp:111
LoadPlanningModelsPr2::pr2_kinematics_plugin_left_arm_
pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_left_arm_
Definition: test_constraint_samplers.cpp:110
constraint_samplers::ConstraintSamplerManager::selectDefaultSampler
static ConstraintSamplerPtr selectDefaultSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const moveit_msgs::msg::Constraints &constr)
Default logic to select a ConstraintSampler given a constraints message.
Definition: constraint_sampler_manager.cpp:69
constraint_samplers::ConstraintSampler::getJointModelGroup
const moveit::core::JointModelGroup * getJointModelGroup() const
Gets the joint model group.
Definition: constraint_sampler.h:107
constraint_samplers::ConstraintSampler::isValid
bool isValid() const
Returns whether or not the constraint sampler is valid or not. To be valid, the joint model group mus...
Definition: constraint_sampler.h:226
constraint_samplers::IKConstraintSampler
A class that allows the sampling of IK constraints.
Definition: default_constraint_samplers.h:313
constraint_samplers::IKConstraintSampler::configure
bool configure(const moveit_msgs::msg::Constraints &constr) override
Configures the IK constraint given a constraints message.
Definition: default_constraint_samplers.cpp:295
constraint_samplers::IKConstraintSampler::getPositionConstraint
const kinematic_constraints::PositionConstraintPtr & getPositionConstraint() const
Gets the position constraint associated with this sampler.
Definition: default_constraint_samplers.h:427
constraint_samplers::IKConstraintSampler::getOrientationConstraint
const kinematic_constraints::OrientationConstraintPtr & getOrientationConstraint() const
Gets the orientation constraint associated with this sampler.
Definition: default_constraint_samplers.h:437
constraint_samplers::IKConstraintSampler::sample
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) override
Produces an IK sample.
Definition: default_constraint_samplers.cpp:581
constraint_samplers::JointConstraintSampler
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
Definition: default_constraint_samplers.h:60
constraint_samplers::JointConstraintSampler::getUnconstrainedJointCount
std::size_t getUnconstrainedJointCount() const
Gets the number of unconstrained joints - joint that have no additional bound beyond the joint limits...
Definition: default_constraint_samplers.h:161
constraint_samplers::JointConstraintSampler::configure
bool configure(const moveit_msgs::msg::Constraints &constr) override
Configures a joint constraint given a Constraints message.
Definition: default_constraint_samplers.cpp:54
constraint_samplers::JointConstraintSampler::sample
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &ks, unsigned int max_attempts) override
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
Definition: default_constraint_samplers.cpp:169
constraint_samplers::JointConstraintSampler::getConstrainedJointCount
std::size_t getConstrainedJointCount() const
Gets the number of constrained joints - joints that have an additional bound beyond the joint limits.
Definition: default_constraint_samplers.h:150
constraint_samplers::UnionConstraintSampler
This class exists as a union of constraint samplers. It contains a vector of constraint samplers,...
Definition: union_constraint_sampler.h:58
constraint_samplers::UnionConstraintSampler::sample
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) override
Produces a sample from all configured samplers.
Definition: union_constraint_sampler.cpp:145
constraint_samplers::UnionConstraintSampler::getSamplers
const std::vector< ConstraintSamplerPtr > & getSamplers() const
Gets the sorted internal list of constraint samplers.
Definition: union_constraint_sampler.h:107
kinematic_constraints::JointConstraint
Class for handling single DOF joint constraints.
Definition: kinematic_constraint.h:203
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:289
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:140
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set....
Definition: kinematic_constraint.h:877
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:1294
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:1303
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition: kinematic_constraint.h:348
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:594
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:706
kinematic_constraints::PositionConstraint
Class for constraints on the XYZ position of a link.
Definition: kinematic_constraint.h:515
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:360
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:522
moveit::core::JointModelGroup::getName
const std::string & getName() const
Get the name of the joint group.
Definition: joint_model_group.h:120
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
moveit::core::RobotState::getVariablePositions
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully....
Definition: robot_state.h:147
moveit::core::RobotState::updateLinkTransforms
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
Definition: robot_state.cpp:775
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1285
moveit::core::RobotState::getVariablePosition
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:207
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:730
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:339
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:110
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:59
constraint_sampler_manager.h
constraint_sampler_tools.h
default_constraint_samplers.h
kinematic_constraint.h
planning_scene.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:63
moveit::core::SolverAllocatorFn
std::function< kinematics::KinematicsBasePtr(const JointModelGroup *)> SolverAllocatorFn
Function type that allocates a kinematics solver for a particular group.
Definition: joint_model_group.h:53
pr2_arm_kinematics_plugin.h
robot_model_test_utils.h
constraint_samplers::IKSamplingPose
A structure for potentially holding a position constraint and an orientation constraint for use durin...
Definition: default_constraint_samplers.h:230
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
Whether or not the constraint or constraints were satisfied.
Definition: kinematic_constraint.h:70
TEST_F
TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
Definition: test_constraint_samplers.cpp:115
main
int main(int argc, char **argv)
Definition: test_constraint_samplers.cpp:1239
union_constraint_sampler.h
Generated by doxygen 1.9.1