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

moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Dec 24 2024 03:27:15