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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Jan 18 2022 03:23:45