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

moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33