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