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 
43 #include <moveit_msgs/DisplayTrajectory.h>
45 #include <moveit_resources/config.h>
46 
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 
57 
58 class LoadPlanningModelsPr2 : public testing::Test
59 {
60 protected:
61  kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const robot_model::JointModelGroup* jmg)
62  {
63  {
65  }
66  }
67 
68  kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const robot_model::JointModelGroup* jmg)
69  {
70  {
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());
95 
97 
99  pr2_kinematics_plugin_right_arm_->initialize("", "right_arm", "torso_lift_link", "r_wrist_roll_link", .01);
100 
102 
104  pr2_kinematics_plugin_left_arm_->initialize("", "left_arm", "torso_lift_link", "l_wrist_roll_link", .01);
105 
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 
118  };
119 
120  void TearDown() override
121  {
122  }
123 
124 protected:
125  urdf::ModelInterfaceSharedPtr urdf_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_;
133 };
134 
135 TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
136 {
138  ks.setToDefaultValues();
139 
141  ks_const.setToDefaultValues();
142 
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 
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));
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));
177 
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
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
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 
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 
306  EXPECT_FALSE(ik_bad.isValid());
307 
310  EXPECT_FALSE(iks.isValid());
311 
313 
314  pcm.header.frame_id = kmodel->getModelFrame();
315  EXPECT_TRUE(pc.configure(pcm, tf));
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));
327 
328  // solver for base doesn't cover group
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
337  EXPECT_FALSE(iks.isValid());
338 }
339 
340 TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler)
341 {
343  ks.setToDefaultValues();
344  ks.update();
346  ks_const.setToDefaultValues();
347  ks_const.update();
348 
349  robot_state::Transforms& tf = ps->getTransformsNonConst();
350 
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 
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 {
386  ks.setToDefaultValues();
387  ks.update();
389  ks_const.setToDefaultValues();
390  ks_const.update();
391 
392  robot_state::Transforms& tf = ps->getTransformsNonConst();
393 
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 
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 
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 
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 
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 {
465  ks.setToDefaultValues();
466  ks.update();
467 
469  ks_const.setToDefaultValues();
470  ks_const.update();
471 
472  robot_state::Transforms& tf = ps->getTransformsNonConst();
473 
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 
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 
545  EXPECT_TRUE(pc.configure(pcm, tf));
546 
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
563  dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[0].get());
564  EXPECT_TRUE(jcs);
565  EXPECT_EQ(jcs->getJointModelGroup()->getName(), "arms_and_torso");
566 
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 
589  EXPECT_TRUE(pc2.configure(pcm, tf));
590 
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 
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 {
633  ks.setToDefaultValues();
634  ks.update();
636  ks_const.setToDefaultValues();
637  ks_const.update();
638 
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 =
683  EXPECT_TRUE(s != nullptr);
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 
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 {
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 =
733  EXPECT_FALSE(static_cast<bool>(s));
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 
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
805  EXPECT_TRUE(static_cast<bool>(s));
807  dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
808  EXPECT_TRUE(jcs);
809 
810  con.position_constraints[0].link_name = "l_wrist_roll_link";
812  EXPECT_TRUE(static_cast<bool>(s));
813  jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
814  EXPECT_FALSE(jcs);
816  EXPECT_FALSE(iks);
817 
818  // we should get a union constraint sampler
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
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()));
849 
850  con.orientation_constraints[0].link_name = "l_wrist_roll_link";
851 
852  // now they both are good
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";
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);
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 
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 {
888  ks.setToDefaultValues();
889  ks.update();
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 
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 =
943 
945  dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
946  ASSERT_TRUE(ucs);
947 
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 {
964  ks.setToDefaultValues();
965  ks.update();
967  ks_const.setToDefaultValues();
968  ks_const.update();
969 
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 
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 
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 
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 
1013  jcs.configure(js);
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 =
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 =
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 =
1107  EXPECT_TRUE(static_cast<bool>(s));
1109  dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
1110  EXPECT_TRUE(ucs);
1111 
1113  kset.add(c, tf);
1114 
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 }
planning_scene::PlanningScenePtr ps
pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_right_arm_
robot_model::RobotModelPtr kmodel
bool configure(const moveit_msgs::OrientationConstraint &oc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::OrientationConstraint.
A structure for potentially holding a position constraint and an orientation constraint for use durin...
#define ROS_INFO_NAMED(name,...)
Class for constraints on the orientation of a link.
bool configure(const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf)
Configure the constraint based on a moveit_msgs::PositionConstraint.
XmlRpcServer s
const std::string & getName() const
Get the name of the joint group.
Class for handling single DOF joint constraints.
A class that contains many different constraints, and can check RobotState *versus the full set...
int main(int argc, char **argv)
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
Produces a sample from all configured samplers.
This class exists as a union of constraint samplers. It contains a vector of constraint samplers...
const kinematic_constraints::OrientationConstraintPtr & getOrientationConstraint() const
Gets the orientation constraint associated with this sampler.
robot_model::SolverAllocatorFn func_left_arm
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
#define EXPECT_NEAR(a, b, prec)
kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const robot_model::JointModelGroup *jmg)
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
std::size_t getUnconstrainedJointCount() const
Gets the number of unconstrained joints - joint that have no additional bound beyond the joint limits...
std::size_t getConstrainedJointCount() const
Gets the number of constrained joints - joints that have an additional bound beyond the joint limits...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
Add all known constraints.
pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_left_arm_
bool satisfied
Whether or not the constraint or constraints were satisfied.
#define EXPECT_FALSE(args)
#define EXPECT_EQ(a, b)
void update(bool force=false)
Update all transforms.
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures a joint constraint given a Constraints message.
const robot_model::JointModelGroup * getJointModelGroup() const
Gets the joint model group.
TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
This class maintains the representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
#define ROS_INFO(...)
urdf::ModelInterfaceSharedPtr urdf_model
static void init()
boost::function< kinematics::KinematicsBasePtr(const JointModelGroup *)> SolverAllocatorFn
Function type that allocates a kinematics solver for a particular group.
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
const kinematic_constraints::PositionConstraintPtr & getPositionConstraint() const
Gets the position constraint associated with this sampler.
A class that allows the sampling of IK constraints.
robot_model::SolverAllocatorFn func_right_arm
def xml_string(rootXml, addHeader=True)
Class for constraints on the XYZ position of a link.
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...
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.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
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
#define EXPECT_TRUE(args)
kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const robot_model::JointModelGroup *jmg)
bool configure(const moveit_msgs::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::JointConstraint.
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result...
virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Decide whether the constraint is satisfied in the indicated state.
const std::vector< ConstraintSamplerPtr > & getSamplers() const
Gets the sorted internal list of constraint samplers.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05