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 <gtest/gtest.h>
50 #include <urdf_parser/urdf_parser.h>
51 #include <fstream>
52 #include <boost/bind.hpp>
53 
54 #include "pr2_arm_kinematics_plugin.h"
55 
56 class LoadPlanningModelsPr2 : public testing::Test
57 {
58 protected:
59  kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const robot_model::JointModelGroup* jmg)
60  {
61  {
62  return pr2_kinematics_plugin_right_arm_;
63  }
64  }
65 
66  kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const robot_model::JointModelGroup* jmg)
67  {
68  {
69  return pr2_kinematics_plugin_left_arm_;
70  }
71  }
72 
73  void SetUp() override
74  {
75  robot_model_ = moveit::core::loadTestingRobotModel("pr2");
76 
77  pr2_kinematics_plugin_right_arm_.reset(new 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_.reset(new 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_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1);
86  func_left_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1);
87 
88  std::map<std::string, robot_model::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_.reset(new planning_scene::PlanningScene(robot_model_));
97  };
98 
99  void TearDown() override
100  {
101  }
102 
103 protected:
104  robot_model::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_;
108  robot_model::SolverAllocatorFn func_right_arm_;
109  robot_model::SolverAllocatorFn func_left_arm_;
110 };
111 
112 TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
113 {
114  robot_state::RobotState ks(robot_model_);
115  ks.setToDefaultValues();
116 
117  robot_state::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
171  constraint_samplers::JointConstraintSampler jcs2(ps_, "arms");
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  robot_state::Transforms& tf = ps_->getTransformsNonConst();
257 
258  kinematic_constraints::PositionConstraint pc(robot_model_);
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");
286  EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose()));
287  EXPECT_FALSE(iks.isValid());
288 
289  EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
290 
291  pcm.header.frame_id = robot_model_->getModelFrame();
292  EXPECT_TRUE(pc.configure(pcm, tf));
293  EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
294 
295  // ik link not in this group
296  constraint_samplers::IKConstraintSampler ik_bad_2(ps_, "right_arm");
297  EXPECT_FALSE(ik_bad_2.configure(constraint_samplers::IKSamplingPose(pc)));
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));
303  EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
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));
309  EXPECT_FALSE(ik_base.configure(constraint_samplers::IKSamplingPose(pc)));
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  robot_state::RobotState ks(robot_model_);
320  ks.setToDefaultValues();
321  ks.update();
322  robot_state::RobotState ks_const(robot_model_);
323  ks_const.setToDefaultValues();
324  ks_const.update();
325 
326  robot_state::Transforms& tf = ps_->getTransformsNonConst();
327 
328  kinematic_constraints::OrientationConstraint oc(robot_model_);
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 
342  EXPECT_TRUE(oc.configure(ocm, tf));
343 
344  bool p1 = oc.decide(ks).satisfied;
345  EXPECT_FALSE(p1);
346 
347  ocm.header.frame_id = robot_model_->getModelFrame();
348  EXPECT_TRUE(oc.configure(ocm, tf));
349 
350  constraint_samplers::IKConstraintSampler iks(ps_, "right_arm");
351  EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(oc)));
352  for (int t = 0; t < 100; ++t)
353  {
354  ks.update();
355  EXPECT_TRUE(iks.sample(ks, ks_const, 100));
356  EXPECT_TRUE(oc.decide(ks).satisfied);
357  }
358 }
359 
360 TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid)
361 {
362  robot_state::RobotState ks(robot_model_);
363  ks.setToDefaultValues();
364  ks.update();
365  robot_state::RobotState ks_const(robot_model_);
366  ks_const.setToDefaultValues();
367  ks_const.update();
368 
369  robot_state::Transforms& tf = ps_->getTransformsNonConst();
370 
371  kinematic_constraints::PositionConstraint pc(robot_model_);
372  moveit_msgs::PositionConstraint pcm;
373 
374  pcm.link_name = "l_wrist_roll_link";
375  pcm.target_point_offset.x = 0;
376  pcm.target_point_offset.y = 0;
377  pcm.target_point_offset.z = 0;
378  pcm.constraint_region.primitives.resize(1);
379  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
380  pcm.constraint_region.primitives[0].dimensions.resize(1);
381  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
382 
383  pcm.header.frame_id = robot_model_->getModelFrame();
384 
385  pcm.constraint_region.primitive_poses.resize(1);
386  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
387  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
388  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
389  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
390  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
391  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
392  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
393  pcm.weight = 1.0;
394 
395  EXPECT_TRUE(pc.configure(pcm, tf));
396 
397  kinematic_constraints::OrientationConstraint oc(robot_model_);
398  moveit_msgs::OrientationConstraint ocm;
399 
400  ocm.link_name = "l_wrist_roll_link";
401  ocm.header.frame_id = robot_model_->getModelFrame();
402  ocm.orientation.x = 0.0;
403  ocm.orientation.y = 0.0;
404  ocm.orientation.z = 0.0;
405  ocm.orientation.w = 1.0;
406  ocm.absolute_x_axis_tolerance = 0.2;
407  ocm.absolute_y_axis_tolerance = 0.1;
408  ocm.absolute_z_axis_tolerance = 0.4;
409  ocm.weight = 1.0;
410 
411  EXPECT_TRUE(oc.configure(ocm, tf));
412 
413  constraint_samplers::IKConstraintSampler iks1(ps_, "left_arm");
414  EXPECT_TRUE(iks1.configure(constraint_samplers::IKSamplingPose(pc, oc)));
415  for (int t = 0; t < 100; ++t)
416  {
417  EXPECT_TRUE(iks1.sample(ks, ks_const, 100));
418  EXPECT_TRUE(pc.decide(ks).satisfied);
419  EXPECT_TRUE(oc.decide(ks).satisfied);
420  }
421 
422  constraint_samplers::IKConstraintSampler iks2(ps_, "left_arm");
423  EXPECT_TRUE(iks2.configure(constraint_samplers::IKSamplingPose(pc)));
424  for (int t = 0; t < 100; ++t)
425  {
426  EXPECT_TRUE(iks2.sample(ks, ks_const, 100));
427  EXPECT_TRUE(pc.decide(ks).satisfied);
428  }
429 
430  constraint_samplers::IKConstraintSampler iks3(ps_, "left_arm");
431  EXPECT_TRUE(iks3.configure(constraint_samplers::IKSamplingPose(oc)));
432  for (int t = 0; t < 100; ++t)
433  {
434  EXPECT_TRUE(iks3.sample(ks, ks_const, 100));
435  EXPECT_TRUE(oc.decide(ks).satisfied);
436  }
437 }
438 
439 TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
440 {
441  robot_state::RobotState ks(robot_model_);
442  ks.setToDefaultValues();
443  ks.update();
444 
445  robot_state::RobotState ks_const(robot_model_);
446  ks_const.setToDefaultValues();
447  ks_const.update();
448 
449  robot_state::Transforms& tf = ps_->getTransformsNonConst();
450 
451  kinematic_constraints::JointConstraint jc1(robot_model_);
452 
453  std::map<std::string, double> state_values;
454 
455  moveit_msgs::JointConstraint torso_constraint;
456  torso_constraint.joint_name = "torso_lift_joint";
457  torso_constraint.position = ks.getVariablePosition("torso_lift_joint");
458  torso_constraint.tolerance_above = 0.01;
459  torso_constraint.tolerance_below = 0.01;
460  torso_constraint.weight = 1.0;
461  EXPECT_TRUE(jc1.configure(torso_constraint));
462 
463  kinematic_constraints::JointConstraint jc2(robot_model_);
464  moveit_msgs::JointConstraint jcm2;
465  jcm2.joint_name = "r_elbow_flex_joint";
466  jcm2.position = ks.getVariablePosition("r_elbow_flex_joint");
467  jcm2.tolerance_above = 0.01;
468  jcm2.tolerance_below = 0.01;
469  jcm2.weight = 1.0;
470  EXPECT_TRUE(jc2.configure(jcm2));
471 
472  moveit_msgs::PositionConstraint pcm;
473 
474  pcm.link_name = "l_wrist_roll_link";
475  pcm.target_point_offset.x = 0;
476  pcm.target_point_offset.y = 0;
477  pcm.target_point_offset.z = 0;
478  pcm.constraint_region.primitives.resize(1);
479  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
480  pcm.constraint_region.primitives[0].dimensions.resize(1);
481  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
482 
483  pcm.constraint_region.primitive_poses.resize(1);
484  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
485  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
486  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
487  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
488  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
489  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
490  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
491  pcm.weight = 1.0;
492 
493  pcm.header.frame_id = robot_model_->getModelFrame();
494 
495  moveit_msgs::OrientationConstraint ocm;
496 
497  ocm.link_name = "l_wrist_roll_link";
498  ocm.header.frame_id = robot_model_->getModelFrame();
499  ocm.orientation.x = 0.0;
500  ocm.orientation.y = 0.0;
501  ocm.orientation.z = 0.0;
502  ocm.orientation.w = 1.0;
503  ocm.absolute_x_axis_tolerance = 0.2;
504  ocm.absolute_y_axis_tolerance = 0.1;
505  ocm.absolute_z_axis_tolerance = 0.4;
506  ocm.weight = 1.0;
507 
508  std::vector<kinematic_constraints::JointConstraint> js;
509  js.push_back(jc1);
510 
511  constraint_samplers::JointConstraintSamplerPtr jcsp(
512  new constraint_samplers::JointConstraintSampler(ps_, "arms_and_torso"));
513  EXPECT_TRUE(jcsp->configure(js));
514 
515  std::vector<kinematic_constraints::JointConstraint> js2;
516  js2.push_back(jc2);
517 
518  constraint_samplers::JointConstraintSamplerPtr jcsp2(new constraint_samplers::JointConstraintSampler(ps_, "arms"));
519  EXPECT_TRUE(jcsp2->configure(js2));
520 
521  kinematic_constraints::PositionConstraint pc(robot_model_);
522  EXPECT_TRUE(pc.configure(pcm, tf));
523 
524  kinematic_constraints::OrientationConstraint oc(robot_model_);
525  EXPECT_TRUE(oc.configure(ocm, tf));
526 
527  constraint_samplers::IKConstraintSamplerPtr iksp(new constraint_samplers::IKConstraintSampler(ps_, "left_arm"));
528  EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
529  EXPECT_TRUE(iksp->isValid());
530 
531  std::vector<constraint_samplers::ConstraintSamplerPtr> cspv;
532  cspv.push_back(jcsp2);
533  cspv.push_back(iksp);
534  cspv.push_back(jcsp);
535 
536  constraint_samplers::UnionConstraintSampler ucs(ps_, "arms_and_torso", cspv);
537 
538  // should have reordered to place whole body first
539  constraint_samplers::JointConstraintSampler* jcs =
540  dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[0].get());
541  EXPECT_TRUE(jcs);
542  EXPECT_EQ(jcs->getJointModelGroup()->getName(), "arms_and_torso");
543 
544  constraint_samplers::JointConstraintSampler* jcs2 =
545  dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[1].get());
546  EXPECT_TRUE(jcs2);
547  EXPECT_EQ(jcs2->getJointModelGroup()->getName(), "arms");
548 
549  for (int t = 0; t < 100; ++t)
550  {
551  EXPECT_TRUE(ucs.sample(ks, ks_const, 100));
552  ks.update();
553  ks.updateLinkTransforms(); // Returned samples have dirty link transforms.
554  ks_const.update();
555  EXPECT_TRUE(jc1.decide(ks).satisfied);
556  EXPECT_TRUE(jc2.decide(ks).satisfied);
557  EXPECT_TRUE(pc.decide(ks).satisfied);
558  }
559 
560  // now we add a position constraint on right arm
561  pcm.link_name = "r_wrist_roll_link";
562  ocm.link_name = "r_wrist_roll_link";
563  cspv.clear();
564 
565  kinematic_constraints::PositionConstraint pc2(robot_model_);
566  EXPECT_TRUE(pc2.configure(pcm, tf));
567 
568  kinematic_constraints::OrientationConstraint oc2(robot_model_);
569  EXPECT_TRUE(oc2.configure(ocm, tf));
570 
571  constraint_samplers::IKConstraintSamplerPtr iksp2(new constraint_samplers::IKConstraintSampler(ps_, "right_arm"));
572  EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2)));
573  EXPECT_TRUE(iksp2->isValid());
574 
575  // totally disjoint, so should break ties based on alphabetical order
576  cspv.clear();
577  cspv.push_back(iksp2);
578  cspv.push_back(iksp);
579 
580  constraint_samplers::UnionConstraintSampler ucs2(ps_, "arms_and_torso", cspv);
581 
582  constraint_samplers::IKConstraintSampler* ikcs_test =
583  dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs2.getSamplers()[0].get());
584  ASSERT_TRUE(ikcs_test);
585  EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "left_arm");
586 
587  // now we make left depends on right, right should stay first
588  pcm.link_name = "l_wrist_roll_link";
589  ocm.link_name = "l_wrist_roll_link";
590  pcm.header.frame_id = "r_wrist_roll_link";
591  ocm.header.frame_id = "r_wrist_roll_link";
592  EXPECT_TRUE(pc.configure(pcm, tf));
593  EXPECT_TRUE(oc.configure(ocm, tf));
594  ASSERT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
595 
596  cspv.clear();
597  cspv.push_back(iksp2);
598  cspv.push_back(iksp);
599 
600  constraint_samplers::UnionConstraintSampler ucs3(ps_, "arms_and_torso", cspv);
601 
602  ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs3.getSamplers()[0].get());
603  EXPECT_TRUE(ikcs_test);
604  EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "right_arm");
605 }
606 
607 TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager)
608 {
609  robot_state::RobotState ks(robot_model_);
610  ks.setToDefaultValues();
611  ks.update();
612  robot_state::RobotState ks_const(robot_model_);
613  ks_const.setToDefaultValues();
614  ks_const.update();
615 
616  kinematic_constraints::PositionConstraint pc(robot_model_);
617 
618  moveit_msgs::PositionConstraint pcm;
619  pcm.link_name = "l_wrist_roll_link";
620  pcm.target_point_offset.x = 0;
621  pcm.target_point_offset.y = 0;
622  pcm.target_point_offset.z = 0;
623  pcm.constraint_region.primitives.resize(1);
624  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
625  pcm.constraint_region.primitives[0].dimensions.resize(1);
626  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
627 
628  pcm.header.frame_id = robot_model_->getModelFrame();
629 
630  pcm.constraint_region.primitive_poses.resize(1);
631  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
632  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
633  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
634  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
635  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
636  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
637  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
638  pcm.weight = 1.0;
639 
640  moveit_msgs::OrientationConstraint ocm;
641 
642  ocm.link_name = "l_wrist_roll_link";
643  ocm.header.frame_id = robot_model_->getModelFrame();
644  ocm.orientation.x = 0.0;
645  ocm.orientation.y = 0.0;
646  ocm.orientation.z = 0.0;
647  ocm.orientation.w = 1.0;
648  ocm.absolute_x_axis_tolerance = 0.2;
649  ocm.absolute_y_axis_tolerance = 0.1;
650  ocm.absolute_z_axis_tolerance = 0.4;
651  ocm.weight = 1.0;
652 
653  // test the automatic construction of constraint sampler
654  moveit_msgs::Constraints c;
655  c.position_constraints.push_back(pcm);
656  c.orientation_constraints.push_back(ocm);
657 
658  constraint_samplers::ConstraintSamplerPtr s =
659  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", c);
660  EXPECT_TRUE(s != nullptr);
661  constraint_samplers::IKConstraintSampler* iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
662  ASSERT_TRUE(iks);
663  ASSERT_TRUE(static_cast<bool>(iks->getPositionConstraint()));
664  ASSERT_TRUE(static_cast<bool>(iks->getOrientationConstraint()));
665 
666  static const int NT = 100;
667  int succ = 0;
668  for (int t = 0; t < NT; ++t)
669  {
670  EXPECT_TRUE(s->sample(ks, ks_const, 100));
671  EXPECT_TRUE(iks->getPositionConstraint()->decide(ks).satisfied);
672  EXPECT_TRUE(iks->getOrientationConstraint()->decide(ks).satisfied);
673  if (s->sample(ks, ks_const, 1))
674  succ++;
675  }
676  ROS_INFO("Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf",
677  (double)succ / (double)NT);
678 
679  // add additional ocm with smaller volume
680  ocm.absolute_x_axis_tolerance = 0.1;
681 
682  c.orientation_constraints.push_back(ocm);
683 
684  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", c);
685  EXPECT_TRUE(s != nullptr);
686 
687  iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
688  ASSERT_TRUE(iks);
689  ASSERT_TRUE(static_cast<bool>(iks->getOrientationConstraint()));
690  EXPECT_NEAR(iks->getOrientationConstraint()->getXAxisTolerance(), .1, .0001);
691 }
692 
693 TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager)
694 {
695  robot_state::RobotState ks(robot_model_);
696  ks.setToDefaultValues();
697  ks.update();
698 
699  moveit_msgs::Constraints con;
700  con.joint_constraints.resize(1);
701 
702  con.joint_constraints[0].joint_name = "l_shoulder_pan_joint";
703  con.joint_constraints[0].position = 0.54;
704  con.joint_constraints[0].tolerance_above = 0.01;
705  con.joint_constraints[0].tolerance_below = 0.01;
706  con.joint_constraints[0].weight = 1.0;
707 
708  constraint_samplers::ConstraintSamplerPtr s =
709  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "right_arm", con);
710  EXPECT_FALSE(static_cast<bool>(s));
711  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
712  EXPECT_TRUE(static_cast<bool>(s));
713 
714  con.joint_constraints.resize(7);
715 
716  con.joint_constraints[1].joint_name = "l_shoulder_lift_joint";
717  con.joint_constraints[1].position = 0.54;
718  con.joint_constraints[1].tolerance_above = 0.01;
719  con.joint_constraints[1].tolerance_below = 0.01;
720  con.joint_constraints[1].weight = 1.0;
721 
722  con.joint_constraints[2].joint_name = "l_upper_arm_roll_joint";
723  con.joint_constraints[2].position = 0.54;
724  con.joint_constraints[2].tolerance_above = 0.01;
725  con.joint_constraints[2].tolerance_below = 0.01;
726  con.joint_constraints[2].weight = 1.0;
727 
728  con.joint_constraints[3].joint_name = "l_elbow_flex_joint";
729  con.joint_constraints[3].position = -0.54;
730  con.joint_constraints[3].tolerance_above = 0.01;
731  con.joint_constraints[3].tolerance_below = 0.01;
732  con.joint_constraints[3].weight = 1.0;
733 
734  con.joint_constraints[4].joint_name = "l_forearm_roll_joint";
735  con.joint_constraints[4].position = 0.54;
736  con.joint_constraints[4].tolerance_above = 0.01;
737  con.joint_constraints[4].tolerance_below = 0.01;
738  con.joint_constraints[4].weight = 1.0;
739 
740  con.joint_constraints[5].joint_name = "l_wrist_flex_joint";
741  con.joint_constraints[5].position = -0.54;
742  con.joint_constraints[5].tolerance_above = 0.05;
743  con.joint_constraints[5].tolerance_below = 0.05;
744  con.joint_constraints[5].weight = 1.0;
745 
746  // an extra constraint on one link, but this shouldn't change anything
747  con.joint_constraints[6].joint_name = "l_wrist_flex_joint";
748  con.joint_constraints[6].position = -0.56;
749  con.joint_constraints[6].tolerance_above = 0.01;
750  con.joint_constraints[6].tolerance_below = 0.01;
751  con.joint_constraints[6].weight = 1.0;
752 
753  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
754  EXPECT_TRUE(static_cast<bool>(s));
755 
756  con.position_constraints.resize(1);
757 
758  // intentionally making wrong wrist
759  con.position_constraints[0].link_name = "r_wrist_roll_link";
760  con.position_constraints[0].target_point_offset.x = 0;
761  con.position_constraints[0].target_point_offset.y = 0;
762  con.position_constraints[0].target_point_offset.z = 0;
763  con.position_constraints[0].constraint_region.primitives.resize(1);
764  con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
765  con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
766  con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
767 
768  con.position_constraints[0].header.frame_id = robot_model_->getModelFrame();
769 
770  con.position_constraints[0].constraint_region.primitive_poses.resize(1);
771  con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
772  con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
773  con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
774  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
775  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
776  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
777  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
778  con.position_constraints[0].weight = 1.0;
779 
780  // this still works, but we should get a JointConstraintSampler
781  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
782  EXPECT_TRUE(static_cast<bool>(s));
783  constraint_samplers::JointConstraintSampler* jcs =
784  dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
785  EXPECT_TRUE(jcs);
786 
787  con.position_constraints[0].link_name = "l_wrist_roll_link";
788  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
789  EXPECT_TRUE(static_cast<bool>(s));
790  jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
791  EXPECT_FALSE(jcs);
792  constraint_samplers::IKConstraintSampler* iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
793  EXPECT_FALSE(iks);
794 
795  // we should get a union constraint sampler
796  constraint_samplers::UnionConstraintSampler* ucs =
797  dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
798  EXPECT_TRUE(ucs);
799 
800  con.orientation_constraints.resize(1);
801 
802  // again, screwing this up intentionally
803  con.orientation_constraints[0].link_name = "r_wrist_roll_link";
804  con.orientation_constraints[0].header.frame_id = robot_model_->getModelFrame();
805  con.orientation_constraints[0].orientation.x = 0.0;
806  con.orientation_constraints[0].orientation.y = 0.0;
807  con.orientation_constraints[0].orientation.z = 0.0;
808  con.orientation_constraints[0].orientation.w = 1.0;
809  con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
810  con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
811  con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
812  con.orientation_constraints[0].weight = 1.0;
813 
814  // we still get an IK sampler with just the position constraint
815  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
816  EXPECT_TRUE(static_cast<bool>(s));
817  ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
818  ASSERT_TRUE(ucs);
819  jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs->getSamplers()[0].get());
820  iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
821 
822  ASSERT_TRUE(iks);
823  ASSERT_TRUE(jcs);
824  EXPECT_TRUE(static_cast<bool>(iks->getPositionConstraint()));
825  EXPECT_FALSE(iks->getOrientationConstraint());
826 
827  con.orientation_constraints[0].link_name = "l_wrist_roll_link";
828 
829  // now they both are good
830  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
831  EXPECT_TRUE(static_cast<bool>(s));
832  ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
833  iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
834  ASSERT_TRUE(iks);
835  EXPECT_TRUE(static_cast<bool>(iks->getPositionConstraint()));
836  EXPECT_TRUE(static_cast<bool>(iks->getOrientationConstraint()));
837 
838  // now just the orientation constraint is good
839  con.position_constraints[0].link_name = "r_wrist_roll_link";
840  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
841  ASSERT_TRUE(static_cast<bool>(s));
842  ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
843  iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
844  ASSERT_TRUE(iks);
845  EXPECT_FALSE(iks->getPositionConstraint());
846  EXPECT_TRUE(static_cast<bool>(iks->getOrientationConstraint()));
847 
848  // now if we constraint all the joints, we get a joint constraint sampler
849  con.joint_constraints.resize(8);
850  con.joint_constraints[7].joint_name = "l_wrist_roll_joint";
851  con.joint_constraints[7].position = 0.54;
852  con.joint_constraints[7].tolerance_above = 0.01;
853  con.joint_constraints[7].tolerance_below = 0.01;
854  con.joint_constraints[7].weight = 1.0;
855 
856  s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "left_arm", con);
857  EXPECT_TRUE(static_cast<bool>(s));
858  jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
859  ASSERT_TRUE(jcs);
860 }
861 
862 TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager)
863 {
864  robot_state::RobotState ks(robot_model_);
865  ks.setToDefaultValues();
866  ks.update();
867  robot_state::RobotState ks_const(robot_model_);
868  ks_const.setToDefaultValues();
869  ks_const.update();
870 
871  moveit_msgs::Constraints con;
872  con.joint_constraints.resize(1);
873 
874  con.joint_constraints[0].joint_name = "torso_lift_joint";
875  con.joint_constraints[0].position = ks.getVariablePosition("torso_lift_joint");
876  con.joint_constraints[0].tolerance_above = 0.01;
877  con.joint_constraints[0].tolerance_below = 0.01;
878  con.joint_constraints[0].weight = 1.0;
879 
880  kinematic_constraints::JointConstraint jc(robot_model_);
881  EXPECT_TRUE(jc.configure(con.joint_constraints[0]));
882 
883  con.position_constraints.resize(1);
884 
885  con.position_constraints[0].link_name = "l_wrist_roll_link";
886  con.position_constraints[0].target_point_offset.x = 0;
887  con.position_constraints[0].target_point_offset.y = 0;
888  con.position_constraints[0].target_point_offset.z = 0;
889  con.position_constraints[0].constraint_region.primitives.resize(1);
890  con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
891  con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
892  con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
893 
894  con.position_constraints[0].constraint_region.primitive_poses.resize(1);
895  con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
896  con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
897  con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
898  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
899  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
900  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
901  con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
902  con.position_constraints[0].weight = 1.0;
903 
904  con.position_constraints[0].header.frame_id = robot_model_->getModelFrame();
905 
906  con.orientation_constraints.resize(1);
907  con.orientation_constraints[0].link_name = "l_wrist_roll_link";
908  con.orientation_constraints[0].header.frame_id = robot_model_->getModelFrame();
909  con.orientation_constraints[0].orientation.x = 0.0;
910  con.orientation_constraints[0].orientation.y = 0.0;
911  con.orientation_constraints[0].orientation.z = 0.0;
912  con.orientation_constraints[0].orientation.w = 1.0;
913  con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
914  con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
915  con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
916  con.orientation_constraints[0].weight = 1.0;
917 
918  constraint_samplers::ConstraintSamplerPtr s =
919  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms_and_torso", con);
920 
921  constraint_samplers::UnionConstraintSampler* ucs =
922  dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
923  ASSERT_TRUE(ucs);
924 
925  constraint_samplers::IKConstraintSampler* ikcs_test =
926  dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
927  ASSERT_TRUE(ikcs_test);
928 
929  for (int t = 0; t < 1; ++t)
930  {
931  EXPECT_TRUE(s->sample(ks, ks_const, 100));
932  EXPECT_TRUE(jc.decide(ks).satisfied);
933  EXPECT_TRUE(ikcs_test->getPositionConstraint()->decide(ks).satisfied);
934  EXPECT_TRUE(ikcs_test->getOrientationConstraint()->decide(ks).satisfied);
935  }
936 }
937 
938 TEST_F(LoadPlanningModelsPr2, SubgroupJointConstraintsSamplerManager)
939 {
940  robot_state::RobotState ks(robot_model_);
941  ks.setToDefaultValues();
942  ks.update();
943  robot_state::RobotState ks_const(robot_model_);
944  ks_const.setToDefaultValues();
945  ks_const.update();
946 
947  kinematic_constraints::JointConstraint jc1(robot_model_);
948  moveit_msgs::JointConstraint jcm1;
949  jcm1.joint_name = "head_pan_joint";
950  jcm1.position = 0.42;
951  jcm1.tolerance_above = 0.01;
952  jcm1.tolerance_below = 0.05;
953  jcm1.weight = 1.0;
954  EXPECT_TRUE(jc1.configure(jcm1));
955 
956  kinematic_constraints::JointConstraint jc2(robot_model_);
957  moveit_msgs::JointConstraint jcm2;
958  jcm2.joint_name = "l_shoulder_pan_joint";
959  jcm2.position = 0.9;
960  jcm2.tolerance_above = 0.1;
961  jcm2.tolerance_below = 0.05;
962  jcm2.weight = 1.0;
963  EXPECT_TRUE(jc2.configure(jcm2));
964 
965  kinematic_constraints::JointConstraint jc3(robot_model_);
966  moveit_msgs::JointConstraint jcm3;
967  jcm3.joint_name = "r_wrist_roll_joint";
968  jcm3.position = 0.7;
969  jcm3.tolerance_above = 0.14;
970  jcm3.tolerance_below = 0.005;
971  jcm3.weight = 1.0;
972  EXPECT_TRUE(jc3.configure(jcm3));
973 
974  kinematic_constraints::JointConstraint jc4(robot_model_);
975  moveit_msgs::JointConstraint jcm4;
976  jcm4.joint_name = "torso_lift_joint";
977  jcm4.position = 0.2;
978  jcm4.tolerance_above = 0.09;
979  jcm4.tolerance_below = 0.01;
980  jcm4.weight = 1.0;
981  EXPECT_TRUE(jc4.configure(jcm4));
982 
983  std::vector<kinematic_constraints::JointConstraint> js;
984  js.push_back(jc1);
985  js.push_back(jc2);
986  js.push_back(jc3);
987  js.push_back(jc4);
988 
989  constraint_samplers::JointConstraintSampler jcs(ps_, "arms");
990  jcs.configure(js);
991  EXPECT_EQ(jcs.getConstrainedJointCount(), 2u);
992  EXPECT_EQ(jcs.getUnconstrainedJointCount(), 12u);
993 
994  for (int t = 0; t < 100; ++t)
995  {
996  EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
997  EXPECT_TRUE(jc2.decide(ks).satisfied);
998  EXPECT_TRUE(jc3.decide(ks).satisfied);
999  }
1000 
1001  // test the automatic construction of constraint sampler
1002  moveit_msgs::Constraints c;
1003 
1004  // no constraints should give no sampler
1005  constraint_samplers::ConstraintSamplerPtr s0 =
1006  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms", c);
1007  EXPECT_TRUE(s0 == nullptr);
1008 
1009  // add the constraints
1010  c.joint_constraints.push_back(jcm1);
1011  c.joint_constraints.push_back(jcm2);
1012  c.joint_constraints.push_back(jcm3);
1013  c.joint_constraints.push_back(jcm4);
1014 
1015  constraint_samplers::ConstraintSamplerPtr s =
1016  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms", c);
1017  EXPECT_TRUE(s != nullptr);
1018 
1019  // test the generated sampler
1020  for (int t = 0; t < 1000; ++t)
1021  {
1022  EXPECT_TRUE(s->sample(ks, ks_const, 1));
1023  EXPECT_TRUE(jc2.decide(ks).satisfied);
1024  EXPECT_TRUE(jc3.decide(ks).satisfied);
1025  }
1026 }
1027 
1028 TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler)
1029 {
1030  moveit_msgs::Constraints c;
1031 
1032  moveit_msgs::PositionConstraint pcm;
1033  pcm.link_name = "l_wrist_roll_link";
1034  pcm.target_point_offset.x = 0;
1035  pcm.target_point_offset.y = 0;
1036  pcm.target_point_offset.z = 0;
1037 
1038  pcm.constraint_region.primitives.resize(1);
1039  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
1040  pcm.constraint_region.primitives[0].dimensions.resize(1);
1041  pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
1042 
1043  pcm.header.frame_id = robot_model_->getModelFrame();
1044 
1045  pcm.constraint_region.primitive_poses.resize(1);
1046  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
1047  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
1048  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
1049  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
1050  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
1051  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
1052  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
1053  pcm.weight = 1.0;
1054  c.position_constraints.push_back(pcm);
1055 
1056  moveit_msgs::OrientationConstraint ocm;
1057  ocm.link_name = "l_wrist_roll_link";
1058  ocm.header.frame_id = robot_model_->getModelFrame();
1059  ocm.orientation.x = 0.0;
1060  ocm.orientation.y = 0.0;
1061  ocm.orientation.z = 0.0;
1062  ocm.orientation.w = 1.0;
1063  ocm.absolute_x_axis_tolerance = 0.2;
1064  ocm.absolute_y_axis_tolerance = 0.1;
1065  ocm.absolute_z_axis_tolerance = 0.4;
1066  ocm.weight = 1.0;
1067  c.orientation_constraints.push_back(ocm);
1068 
1069  ocm.link_name = "r_wrist_roll_link";
1070  ocm.header.frame_id = robot_model_->getModelFrame();
1071  ocm.orientation.x = 0.0;
1072  ocm.orientation.y = 0.0;
1073  ocm.orientation.z = 0.0;
1074  ocm.orientation.w = 1.0;
1075  ocm.absolute_x_axis_tolerance = 0.01;
1076  ocm.absolute_y_axis_tolerance = 0.01;
1077  ocm.absolute_z_axis_tolerance = 0.01;
1078  ocm.weight = 1.0;
1079  c.orientation_constraints.push_back(ocm);
1080 
1081  robot_state::Transforms& tf = ps_->getTransformsNonConst();
1082  constraint_samplers::ConstraintSamplerPtr s =
1083  constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps_, "arms", c);
1084  EXPECT_TRUE(static_cast<bool>(s));
1085  constraint_samplers::UnionConstraintSampler* ucs =
1086  dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
1087  EXPECT_TRUE(ucs);
1088 
1089  kinematic_constraints::KinematicConstraintSet kset(robot_model_);
1090  kset.add(c, tf);
1091 
1092  robot_state::RobotState ks(robot_model_);
1093  ks.setToDefaultValues();
1094  ks.update();
1095  robot_state::RobotState ks_const(robot_model_);
1096  ks_const.setToDefaultValues();
1097  ks_const.update();
1098 
1099  static const int NT = 100;
1100  int succ = 0;
1101  for (int t = 0; t < NT; ++t)
1102  {
1103  EXPECT_TRUE(s->sample(ks, ks_const, 1000));
1104  EXPECT_TRUE(kset.decide(ks).satisfied);
1105  if (s->sample(ks, ks_const, 1))
1106  succ++;
1107  }
1108  ROS_INFO_NAMED("pr2_arm_kinematics_plugin",
1109  "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf",
1110  (double)succ / (double)NT);
1111 }
1112 
1113 int main(int argc, char** argv)
1114 {
1115  testing::InitGoogleTest(&argc, argv);
1116  ros::Time::init();
1117  return RUN_ALL_TESTS();
1118 }
kinematic_constraints::OrientationConstraint::decide
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:650
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:43
LoadPlanningModelsPr2::pr2_kinematics_plugin_right_arm_
pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_right_arm_
Definition: test_constraint_samplers.cpp:106
union_constraint_sampler.h
constraint_samplers::IKConstraintSampler::getPositionConstraint
const kinematic_constraints::PositionConstraintPtr & getPositionConstraint() const
Gets the position constraint associated with this sampler.
Definition: default_constraint_samplers.h:390
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:52
kinematic_constraints::OrientationConstraint::configure
bool configure(const moveit_msgs::OrientationConstraint &oc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::OrientationConstraint.
Definition: kinematic_constraint.cpp:550
constraint_samplers::IKSamplingPose
A structure for potentially holding a position constraint and an orientation constraint for use durin...
Definition: default_constraint_samplers.h:210
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
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:246
kinematic_constraints::JointConstraint::decide
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:263
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition: kinematic_constraint.h:348
kinematic_constraints::PositionConstraint::configure
bool configure(const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::PositionConstraint.
Definition: kinematic_constraint.cpp:328
kinematic_constraints::PositionConstraint::decide
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const override
Decide whether the constraint is satisfied in the indicated state.
Definition: kinematic_constraint.cpp:490
pr2_arm_kinematics_plugin.h
s
XmlRpcServer s
kinematic_constraints::JointConstraint
Class for handling single DOF joint constraints.
Definition: kinematic_constraint.h:203
kinematic_constraints::KinematicConstraintSet
A class that contains many different constraints, and can check RobotState *versus the full set...
Definition: kinematic_constraint.h:861
main
int main(int argc, char **argv)
Definition: test_constraint_samplers.cpp:1113
constraint_samplers::UnionConstraintSampler::sample
bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts) override
Produces a sample from all configured samplers.
Definition: union_constraint_sampler.cpp:126
constraint_samplers::UnionConstraintSampler
This class exists as a union of constraint samplers. It contains a vector of constraint samplers...
Definition: union_constraint_sampler.h:57
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:57
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
LoadPlanningModelsPr2::getKinematicsSolverLeftArm
kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const robot_model::JointModelGroup *jmg)
Definition: test_constraint_samplers.cpp:66
t
geometry_msgs::TransformStamped t
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:60
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:346
kinematic_constraints::KinematicConstraintSet::add
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
Add all known constraints.
Definition: kinematic_constraint.cpp:1198
LoadPlanningModelsPr2::pr2_kinematics_plugin_left_arm_
pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_left_arm_
Definition: test_constraint_samplers.cpp:107
LoadPlanningModelsPr2::func_right_arm_
robot_model::SolverAllocatorFn func_right_arm_
Definition: test_constraint_samplers.cpp:108
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
Whether or not the constraint or constraints were satisfied.
Definition: kinematic_constraint.h:71
EXPECT_FALSE
#define EXPECT_FALSE(args)
EXPECT_EQ
#define EXPECT_EQ(a, b)
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:645
TEST_F
TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
Definition: test_constraint_samplers.cpp:112
planning_scene::PlanningScene
This class maintains the representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
Definition: planning_scene.h:86
ROS_INFO
#define ROS_INFO(...)
ros::Time::init
static void init()
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:53
moveit::core::JointModelGroup::getName
const std::string & getName() const
Get the name of the joint group.
Definition: joint_model_group.h:120
robot_model_test_utils.h
constraint_samplers::IKConstraintSampler
A class that allows the sampling of IK constraints.
Definition: default_constraint_samplers.h:293
kinematic_constraints::KinematicConstraintSet::decide
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result...
Definition: kinematic_constraint.cpp:1207
kinematic_constraints::PositionConstraint
Class for constraints on the XYZ position of a link.
Definition: kinematic_constraint.h:505
LoadPlanningModelsPr2::ps_
planning_scene::PlanningScenePtr ps_
Definition: test_constraint_samplers.cpp:105
constraint_samplers::UnionConstraintSampler::getSamplers
const std::vector< ConstraintSamplerPtr > & getSamplers() const
Gets the sorted internal list of constraint samplers.
Definition: union_constraint_sampler.h:107
constraint_sampler_tools.h
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
shape_operations.h
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:131
LoadPlanningModelsPr2::robot_model_
robot_model::RobotModelPtr robot_model_
Definition: test_constraint_samplers.cpp:104
pr2_arm_kinematics::PR2ArmKinematicsPlugin
Definition: pr2_arm_kinematics_plugin.h:132
constraint_sampler_manager.h
moveit::core::RobotState
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:128
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:686
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:142
constraint_samplers::JointConstraintSampler::sample
bool sample(robot_state::RobotState &state, const robot_state::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:148
EXPECT_TRUE
#define EXPECT_TRUE(args)
LoadPlanningModelsPr2::getKinematicsSolverRightArm
kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const robot_model::JointModelGroup *jmg)
Definition: test_constraint_samplers.cpp:59
default_constraint_samplers.h
planning_scene.h
kinematic_constraints::JointConstraint::configure
bool configure(const moveit_msgs::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::JointConstraint.
Definition: kinematic_constraint.cpp:120
constraint_samplers::ConstraintSampler::getJointModelGroup
const robot_model::JointModelGroup * getJointModelGroup() const
Gets the joint model group.
Definition: constraint_sampler.h:108
kinematic_constraint.h
LoadPlanningModelsPr2::func_left_arm_
robot_model::SolverAllocatorFn func_left_arm_
Definition: test_constraint_samplers.cpp:109
constraint_samplers::IKConstraintSampler::getOrientationConstraint
const kinematic_constraints::OrientationConstraintPtr & getOrientationConstraint() const
Gets the orientation constraint associated with this sampler.
Definition: default_constraint_samplers.h:400

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