moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_constraints.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, E. Gil Jones */
36
37#include <moveit/kinematic_constraints/kinematic_constraint.h>
38#include <gtest/gtest.h>
39#include <urdf_parser/urdf_parser.h>
40#include <fstream>
41#include <tf2_eigen/tf2_eigen.hpp>
42#include <math.h>
43#include <moveit/utils/robot_model_test_utils.h>
44
45class LoadPlanningModelsPr2 : public testing::Test
46{
47protected:
48 void SetUp() override
49 {
50 robot_model_ = moveit::core::loadTestingRobotModel("pr2");
51 }
52
53 void TearDown() override
54 {
55 }
56
57protected:
58 moveit::core::RobotModelPtr robot_model_;
59};
60
61TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
62{
63 moveit::core::RobotState robot_state(robot_model_);
64 robot_state.setToDefaultValues();
65 moveit::core::Transforms tf(robot_model_->getModelFrame());
66
67 kinematic_constraints::JointConstraint jc(robot_model_);
68 moveit_msgs::msg::JointConstraint jcm;
69 jcm.joint_name = "head_pan_joint";
70 jcm.position = 0.4;
71 jcm.tolerance_above = 0.1;
72 jcm.tolerance_below = 0.05;
73
74 EXPECT_TRUE(jc.configure(jcm));
75 // weight should have been changed to 1.0
76 EXPECT_NEAR(jc.getConstraintWeight(), 1.0, std::numeric_limits<double>::epsilon());
77
78 // tests that the default state is outside the bounds
79 // given that the default state is at 0.0
80 EXPECT_TRUE(jc.configure(jcm));
81 kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(robot_state);
82 EXPECT_FALSE(p1.satisfied);
83 EXPECT_NEAR(p1.distance, jcm.position, 1e-6);
84
85 // tests that when we set the state within the bounds
86 // the constraint is satisfied
87 double jval = 0.41;
88 robot_state.setJointPositions(jcm.joint_name, &jval);
89 kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(robot_state);
90 EXPECT_TRUE(p2.satisfied);
91 EXPECT_NEAR(p2.distance, 0.01, 1e-6);
92
93 // exactly equal to the low bound is fine too
94 jval = 0.35;
95 robot_state.setJointPositions(jcm.joint_name, &jval);
96 EXPECT_TRUE(jc.decide(robot_state).satisfied);
97
98 // and so is less than epsilon when there's no other source of error
99 // jvals[jcm.joint_name] = 0.35-std::numeric_limits<double>::epsilon();
100 jval = 0.35 - std::numeric_limits<double>::epsilon();
101
102 robot_state.setJointPositions(jcm.joint_name, &jval);
103 EXPECT_TRUE(jc.decide(robot_state).satisfied);
104
105 // but this is too much
106 jval = 0.35 - 3 * std::numeric_limits<double>::epsilon();
107 robot_state.setJointPositions(jcm.joint_name, &jval);
108 EXPECT_FALSE(jc.decide(robot_state).satisfied);
109
110 // negative value makes configuration fail
111 jcm.tolerance_below = -0.05;
112 EXPECT_FALSE(jc.configure(jcm));
113
114 jcm.tolerance_below = 0.05;
115 EXPECT_TRUE(jc.configure(jcm));
116
117 // still satisfied at a slightly different state
118 jval = 0.46;
119 robot_state.setJointPositions(jcm.joint_name, &jval);
120 EXPECT_TRUE(jc.decide(robot_state).satisfied);
121
122 // still satisfied at a slightly different state
123 jval = 0.501;
124 robot_state.setJointPositions(jcm.joint_name, &jval);
125 EXPECT_FALSE(jc.decide(robot_state).satisfied);
126
127 // still satisfied at a slightly different state
128 jval = 0.39;
129 robot_state.setJointPositions(jcm.joint_name, &jval);
130 EXPECT_TRUE(jc.decide(robot_state).satisfied);
131
132 // outside the bounds
133 jval = 0.34;
134 robot_state.setJointPositions(jcm.joint_name, &jval);
135 EXPECT_FALSE(jc.decide(robot_state).satisfied);
136
137 // testing equality
138 kinematic_constraints::JointConstraint jc2(robot_model_);
139 EXPECT_TRUE(jc2.configure(jcm));
140 EXPECT_TRUE(jc2.enabled());
141 EXPECT_TRUE(jc.equal(jc2, 1e-12));
142
143 // if name not equal, not equal
144 jcm.joint_name = "head_tilt_joint";
145 EXPECT_TRUE(jc2.configure(jcm));
146 EXPECT_FALSE(jc.equal(jc2, 1e-12));
147
148 // if different, test margin behavior
149 jcm.joint_name = "head_pan_joint";
150 jcm.position = 0.3;
151 EXPECT_TRUE(jc2.configure(jcm));
152 EXPECT_FALSE(jc.equal(jc2, 1e-12));
153 // exactly equal is still false
154 EXPECT_FALSE(jc.equal(jc2, .1));
155 EXPECT_TRUE(jc.equal(jc2, .101));
156
157 // no name makes this false
158 jcm.joint_name = "";
159 jcm.position = 0.4;
160 EXPECT_FALSE(jc2.configure(jcm));
161 EXPECT_FALSE(jc2.enabled());
162 EXPECT_FALSE(jc.equal(jc2, 1e-12));
163
164 // no DOF makes this false
165 jcm.joint_name = "base_footprint_joint";
166 EXPECT_FALSE(jc2.configure(jcm));
167
168 // clear means not enabled
169 jcm.joint_name = "head_pan_joint";
170 EXPECT_TRUE(jc2.configure(jcm));
171 jc2.clear();
172 EXPECT_FALSE(jc2.enabled());
173 EXPECT_FALSE(jc.equal(jc2, 1e-12));
174}
175
176TEST_F(LoadPlanningModelsPr2, JointConstraintsCont)
177{
178 moveit::core::RobotState robot_state(robot_model_);
179 robot_state.setToDefaultValues();
180 robot_state.update();
181 moveit::core::Transforms tf(robot_model_->getModelFrame());
182
183 kinematic_constraints::JointConstraint jc(robot_model_);
184 moveit_msgs::msg::JointConstraint jcm;
185
186 jcm.joint_name = "l_wrist_roll_joint";
187 jcm.position = 0.0;
188 jcm.tolerance_above = 0.04;
189 jcm.tolerance_below = 0.02;
190 jcm.weight = 1.0;
191
192 EXPECT_TRUE(jc.configure(jcm));
193
194 std::map<std::string, double> jvals;
195
196 // state should have zeros, and work
197 EXPECT_TRUE(jc.decide(robot_state).satisfied);
198
199 // within the above tolerance
200 jvals[jcm.joint_name] = .03;
201 robot_state.setVariablePositions(jvals);
202 robot_state.update();
203 EXPECT_TRUE(jc.decide(robot_state).satisfied);
204
205 // outside the above tolerance
206 jvals[jcm.joint_name] = .05;
207 robot_state.setVariablePositions(jvals);
208 robot_state.update();
209 EXPECT_FALSE(jc.decide(robot_state).satisfied);
210
211 // inside the below tolerance
212 jvals[jcm.joint_name] = -.01;
213 robot_state.setVariablePositions(jvals);
214 EXPECT_TRUE(jc.decide(robot_state).satisfied);
215
216 // outside the below tolerance
217 jvals[jcm.joint_name] = -.03;
218 robot_state.setVariablePositions(jvals);
219 EXPECT_FALSE(jc.decide(robot_state).satisfied);
220
221 // now testing wrap around from positive to negative
222 jcm.position = 3.14;
223 EXPECT_TRUE(jc.configure(jcm));
224
225 // testing that wrap works
226 jvals[jcm.joint_name] = 3.17;
227 robot_state.setVariablePositions(jvals);
228 kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(robot_state);
229 EXPECT_TRUE(p1.satisfied);
230 EXPECT_NEAR(p1.distance, 0.03, 1e-6);
231
232 // testing that negative wrap works
233 jvals[jcm.joint_name] = -3.14;
234 robot_state.setVariablePositions(jvals);
235 kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(robot_state);
236 EXPECT_TRUE(p2.satisfied);
237 EXPECT_NEAR(p2.distance, 0.003185, 1e-4);
238
239 // over bound testing
240 jvals[jcm.joint_name] = 3.19;
241 robot_state.setVariablePositions(jvals);
242 EXPECT_FALSE(jc.decide(robot_state).satisfied);
243
244 // reverses to other direction
245 // but still tested using above tolerance
246 jvals[jcm.joint_name] = -3.11;
247 robot_state.setVariablePositions(jvals);
248 EXPECT_TRUE(jc.decide(robot_state).satisfied);
249
250 // outside of the bound given the wrap
251 jvals[jcm.joint_name] = -3.09;
252 robot_state.setVariablePositions(jvals);
253 EXPECT_FALSE(jc.decide(robot_state).satisfied);
254
255 // lower tolerance testing
256 // within bounds
257 jvals[jcm.joint_name] = 3.13;
258 robot_state.setVariablePositions(jvals);
259 EXPECT_TRUE(jc.decide(robot_state).satisfied);
260
261 // within outside
262 jvals[jcm.joint_name] = 3.11;
263 robot_state.setVariablePositions(jvals);
264 EXPECT_FALSE(jc.decide(robot_state).satisfied);
265
266 // testing the other direction
267 jcm.position = -3.14;
268 EXPECT_TRUE(jc.configure(jcm));
269
270 // should be governed by above tolerance
271 jvals[jcm.joint_name] = -3.11;
272 robot_state.setVariablePositions(jvals);
273 EXPECT_TRUE(jc.decide(robot_state).satisfied);
274
275 // outside upper bound
276 jvals[jcm.joint_name] = -3.09;
277 robot_state.setVariablePositions(jvals);
278 EXPECT_FALSE(jc.decide(robot_state).satisfied);
279
280 // governed by lower bound
281 jvals[jcm.joint_name] = 3.13;
282 robot_state.setVariablePositions(jvals);
283 EXPECT_TRUE(jc.decide(robot_state).satisfied);
284
285 // outside lower bound (but would be inside upper)
286 jvals[jcm.joint_name] = 3.12;
287 robot_state.setVariablePositions(jvals);
288 EXPECT_TRUE(jc.decide(robot_state).satisfied);
289
290 // testing wrap
291 jcm.position = 6.28;
292 EXPECT_TRUE(jc.configure(jcm));
293
294 // should wrap to zero
295 jvals[jcm.joint_name] = 0.0;
296 robot_state.setVariablePositions(jvals);
297 EXPECT_TRUE(jc.decide(robot_state).satisfied);
298
299 // should wrap to close and test to be near
300 moveit_msgs::msg::JointConstraint jcm2 = jcm;
301 jcm2.position = -6.28;
302 kinematic_constraints::JointConstraint jc2(robot_model_);
303 EXPECT_TRUE(jc.configure(jcm2));
304 jc.equal(jc2, .02);
305}
306
307TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF)
308{
309 moveit::core::RobotState robot_state(robot_model_);
310 robot_state.setToDefaultValues();
311
312 kinematic_constraints::JointConstraint jc(robot_model_);
313 moveit_msgs::msg::JointConstraint jcm;
314 jcm.joint_name = "world_joint";
315 jcm.position = 3.14;
316 jcm.tolerance_above = 0.1;
317 jcm.tolerance_below = 0.05;
318 jcm.weight = 1.0;
319
320 // shouldn't work for multi-dof without local name
321 EXPECT_FALSE(jc.configure(jcm));
322
323 // this should, and function like any other single joint constraint
324 jcm.joint_name = "world_joint/x";
325 EXPECT_TRUE(jc.configure(jcm));
326
327 std::map<std::string, double> jvals;
328 jvals[jcm.joint_name] = 3.2;
329 robot_state.setVariablePositions(jvals);
330 kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(robot_state);
331 EXPECT_TRUE(p1.satisfied);
332
333 jvals[jcm.joint_name] = 3.25;
334 robot_state.setVariablePositions(jvals);
335 kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(robot_state);
336 EXPECT_FALSE(p2.satisfied);
337
338 jvals[jcm.joint_name] = -3.14;
339 robot_state.setVariablePositions(jvals);
340 kinematic_constraints::ConstraintEvaluationResult p3 = jc.decide(robot_state);
341 EXPECT_FALSE(p3.satisfied);
342
343 // theta is continuous
344 jcm.joint_name = "world_joint/theta";
345 EXPECT_TRUE(jc.configure(jcm));
346
347 jvals[jcm.joint_name] = -3.14;
348 robot_state.setVariablePositions(jvals);
349 kinematic_constraints::ConstraintEvaluationResult p4 = jc.decide(robot_state);
350 EXPECT_TRUE(p4.satisfied);
351
352 jvals[jcm.joint_name] = 3.25;
353 robot_state.setVariablePositions(jvals);
354 kinematic_constraints::ConstraintEvaluationResult p5 = jc.decide(robot_state);
355 EXPECT_FALSE(p5.satisfied);
356}
357
358TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed)
359{
360 moveit::core::RobotState robot_state(robot_model_);
361 robot_state.setToDefaultValues();
362 robot_state.update(true);
363 moveit::core::Transforms tf(robot_model_->getModelFrame());
364 kinematic_constraints::PositionConstraint pc(robot_model_);
365 moveit_msgs::msg::PositionConstraint pcm;
366
367 // empty certainly means false
368 EXPECT_FALSE(pc.configure(pcm, tf));
369
370 pcm.link_name = "l_wrist_roll_link";
371 pcm.target_point_offset.x = 0;
372 pcm.target_point_offset.y = 0;
373 pcm.target_point_offset.z = 0;
374 pcm.constraint_region.primitives.resize(1);
375 pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
376
377 // no dimensions, so no valid regions
378 EXPECT_FALSE(pc.configure(pcm, tf));
379
380 pcm.constraint_region.primitives[0].dimensions.resize(1);
381 pcm.constraint_region.primitives[0].dimensions[0] = 0.2;
382
383 // no pose, so no valid region
384 EXPECT_FALSE(pc.configure(pcm, tf));
385
386 pcm.constraint_region.primitive_poses.resize(1);
387 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
388 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
389 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
390 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
391 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
392 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
393 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
394 pcm.weight = 1.0;
395
396 // intentionally leaving header frame blank to test behavior
397 EXPECT_FALSE(pc.configure(pcm, tf));
398
399 pcm.header.frame_id = robot_model_->getModelFrame();
400 EXPECT_TRUE(pc.configure(pcm, tf));
401 EXPECT_FALSE(pc.mobileReferenceFrame());
402
403 EXPECT_TRUE(pc.decide(robot_state).satisfied);
404
405 std::map<std::string, double> jvals;
406 jvals["torso_lift_joint"] = 0.4;
407 robot_state.setVariablePositions(jvals);
408 robot_state.update();
409 EXPECT_FALSE(pc.decide(robot_state).satisfied);
410 EXPECT_TRUE(pc.equal(pc, 1e-12));
411
412 // arbitrary offset that puts it back into the pose range
413 pcm.target_point_offset.x = 0;
414 pcm.target_point_offset.y = 0;
415 pcm.target_point_offset.z = .15;
416
417 EXPECT_TRUE(pc.configure(pcm, tf));
418 EXPECT_TRUE(pc.hasLinkOffset());
419 EXPECT_TRUE(pc.decide(robot_state).satisfied);
420
421 pc.clear();
422 EXPECT_FALSE(pc.enabled());
423
424 // invalid quaternion results in zero quaternion
425 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
426 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
427 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
428 pcm.constraint_region.primitive_poses[0].orientation.w = 0.0;
429
430 EXPECT_TRUE(pc.configure(pcm, tf));
431 EXPECT_TRUE(pc.decide(robot_state).satisfied);
432}
433
434TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile)
435{
436 moveit::core::RobotState robot_state(robot_model_);
437 robot_state.setToDefaultValues();
438 moveit::core::Transforms tf(robot_model_->getModelFrame());
439 robot_state.update();
440
441 kinematic_constraints::PositionConstraint pc(robot_model_);
442 moveit_msgs::msg::PositionConstraint pcm;
443
444 pcm.link_name = "l_wrist_roll_link";
445 pcm.target_point_offset.x = 0;
446 pcm.target_point_offset.y = 0;
447 pcm.target_point_offset.z = 0;
448
449 pcm.constraint_region.primitives.resize(1);
450 pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
451 pcm.constraint_region.primitives[0].dimensions.resize(1);
452 pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.38 * 2.0;
453
454 pcm.header.frame_id = "r_wrist_roll_link";
455
456 pcm.constraint_region.primitive_poses.resize(1);
457 pcm.constraint_region.primitive_poses[0].position.x = 0.0;
458 pcm.constraint_region.primitive_poses[0].position.y = 0.6;
459 pcm.constraint_region.primitive_poses[0].position.z = 0.0;
460 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
461 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
462 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
463 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
464 pcm.weight = 1.0;
465
466 EXPECT_FALSE(tf.isFixedFrame(pcm.link_name));
467 EXPECT_TRUE(pc.configure(pcm, tf));
468 EXPECT_TRUE(pc.mobileReferenceFrame());
469
470 EXPECT_TRUE(pc.decide(robot_state).satisfied);
471
472 pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
473 pcm.constraint_region.primitives[0].dimensions.resize(3);
474 pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.1;
475 pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 0.1;
476 pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 0.1;
477 EXPECT_TRUE(pc.configure(pcm, tf));
478
479 std::map<std::string, double> jvals;
480 jvals["l_shoulder_pan_joint"] = 0.4;
481 robot_state.setVariablePositions(jvals);
482 robot_state.update();
483 EXPECT_TRUE(pc.decide(robot_state).satisfied);
484 EXPECT_TRUE(pc.equal(pc, 1e-12));
485
486 jvals["l_shoulder_pan_joint"] = -0.4;
487 robot_state.setVariablePositions(jvals);
488 robot_state.update();
489 EXPECT_FALSE(pc.decide(robot_state).satisfied);
490
491 // adding a second constrained region makes this work
492 pcm.constraint_region.primitive_poses.resize(2);
493 pcm.constraint_region.primitive_poses[1].position.x = 0.0;
494 pcm.constraint_region.primitive_poses[1].position.y = 0.1;
495 pcm.constraint_region.primitive_poses[1].position.z = 0.0;
496 pcm.constraint_region.primitive_poses[1].orientation.x = 0.0;
497 pcm.constraint_region.primitive_poses[1].orientation.y = 0.0;
498 pcm.constraint_region.primitive_poses[1].orientation.z = 0.0;
499 pcm.constraint_region.primitive_poses[1].orientation.w = 1.0;
500
501 pcm.constraint_region.primitives.resize(2);
502 pcm.constraint_region.primitives[1].type = shape_msgs::msg::SolidPrimitive::BOX;
503 pcm.constraint_region.primitives[1].dimensions.resize(3);
504 pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.1;
505 pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 0.1;
506 pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 0.1;
507 EXPECT_TRUE(pc.configure(pcm, tf));
508 EXPECT_TRUE(pc.decide(robot_state, false).satisfied);
509}
510
511TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality)
512{
513 moveit::core::RobotState robot_state(robot_model_);
514 robot_state.setToDefaultValues();
515 moveit::core::Transforms tf(robot_model_->getModelFrame());
516
517 kinematic_constraints::PositionConstraint pc(robot_model_);
518 kinematic_constraints::PositionConstraint pc2(robot_model_);
519 moveit_msgs::msg::PositionConstraint pcm;
520
521 pcm.link_name = "l_wrist_roll_link";
522 pcm.target_point_offset.x = 0;
523 pcm.target_point_offset.y = 0;
524 pcm.target_point_offset.z = 0;
525
526 pcm.constraint_region.primitives.resize(2);
527 pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
528 pcm.constraint_region.primitives[0].dimensions.resize(1);
529 pcm.constraint_region.primitives[0].dimensions[0] = 0.38 * 2.0;
530 pcm.constraint_region.primitives[1].type = shape_msgs::msg::SolidPrimitive::BOX;
531 pcm.constraint_region.primitives[1].dimensions.resize(3);
532 pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 2.0;
533 pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 2.0;
534 pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 2.0;
535
536 pcm.header.frame_id = "r_wrist_roll_link";
537 pcm.constraint_region.primitive_poses.resize(2);
538 pcm.constraint_region.primitive_poses[0].position.x = 0.0;
539 pcm.constraint_region.primitive_poses[0].position.y = 0.6;
540 pcm.constraint_region.primitive_poses[0].position.z = 0.0;
541 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
542 pcm.constraint_region.primitive_poses[1].position.x = 2.0;
543 pcm.constraint_region.primitive_poses[1].position.y = 0.0;
544 pcm.constraint_region.primitive_poses[1].position.z = 0.0;
545 pcm.constraint_region.primitive_poses[1].orientation.w = 1.0;
546 pcm.weight = 1.0;
547
548 EXPECT_TRUE(pc.configure(pcm, tf));
549 EXPECT_TRUE(pc2.configure(pcm, tf));
550
551 EXPECT_TRUE(pc.equal(pc2, .001));
552 EXPECT_TRUE(pc2.equal(pc, .001));
553
554 // putting regions in different order
555 moveit_msgs::msg::PositionConstraint pcm2 = pcm;
556 pcm2.constraint_region.primitives[0] = pcm.constraint_region.primitives[1];
557 pcm2.constraint_region.primitives[1] = pcm.constraint_region.primitives[0];
558
559 pcm2.constraint_region.primitive_poses[0] = pcm.constraint_region.primitive_poses[1];
560 pcm2.constraint_region.primitive_poses[1] = pcm.constraint_region.primitive_poses[0];
561
562 EXPECT_TRUE(pc2.configure(pcm2, tf));
563 EXPECT_TRUE(pc.equal(pc2, .001));
564 EXPECT_TRUE(pc2.equal(pc, .001));
565
566 // messing with one value breaks it
567 pcm2.constraint_region.primitive_poses[0].position.z = .01;
568 EXPECT_TRUE(pc2.configure(pcm2, tf));
569 EXPECT_FALSE(pc.equal(pc2, .001));
570 EXPECT_FALSE(pc2.equal(pc, .001));
571 EXPECT_TRUE(pc.equal(pc2, .1));
572 EXPECT_TRUE(pc2.equal(pc, .1));
573
574 // adding an identical third shape to the last one is ok
575 pcm2.constraint_region.primitive_poses[0].position.z = 0.0;
576 pcm2.constraint_region.primitives.resize(3);
577 pcm2.constraint_region.primitive_poses.resize(3);
578 pcm2.constraint_region.primitives[2] = pcm2.constraint_region.primitives[0];
579 pcm2.constraint_region.primitive_poses[2] = pcm2.constraint_region.primitive_poses[0];
580 EXPECT_TRUE(pc2.configure(pcm2, tf));
581 EXPECT_TRUE(pc.equal(pc2, .001));
582 EXPECT_TRUE(pc2.equal(pc, .001));
583
584 // but if we change it, it's not
585 pcm2.constraint_region.primitives[2].dimensions[0] = 3.0;
586 EXPECT_TRUE(pc2.configure(pcm2, tf));
587 EXPECT_FALSE(pc.equal(pc2, .001));
588 EXPECT_FALSE(pc2.equal(pc, .001));
589
590 // changing the shape also changes it
591 pcm2.constraint_region.primitives[2].dimensions[0] = pcm2.constraint_region.primitives[0].dimensions[0];
592 pcm2.constraint_region.primitives[2].type = shape_msgs::msg::SolidPrimitive::SPHERE;
593 EXPECT_TRUE(pc2.configure(pcm2, tf));
594 EXPECT_FALSE(pc.equal(pc2, .001));
595}
596
597TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)
598{
599 moveit::core::RobotState robot_state(robot_model_);
600 robot_state.setToDefaultValues();
601 robot_state.update();
602 moveit::core::Transforms tf(robot_model_->getModelFrame());
603
604 kinematic_constraints::OrientationConstraint oc(robot_model_);
605
606 moveit_msgs::msg::OrientationConstraint ocm;
607
608 EXPECT_FALSE(oc.configure(ocm, tf));
609
610 ocm.link_name = "r_wrist_roll_link";
611
612 // all we currently have to specify is the link name to get a valid constraint
613 EXPECT_TRUE(oc.configure(ocm, tf));
614
615 ocm.header.frame_id = robot_model_->getModelFrame();
616 ocm.orientation.x = 0.0;
617 ocm.orientation.y = 0.0;
618 ocm.orientation.z = 0.0;
619 ocm.orientation.w = 1.0;
620 ocm.absolute_x_axis_tolerance = 0.1;
621 ocm.absolute_y_axis_tolerance = 0.1;
622 ocm.absolute_z_axis_tolerance = 0.1;
623 ocm.weight = 1.0;
624
625 EXPECT_TRUE(oc.configure(ocm, tf));
626 EXPECT_FALSE(oc.mobileReferenceFrame());
627
628 EXPECT_FALSE(oc.decide(robot_state).satisfied);
629
630 ocm.header.frame_id = ocm.link_name;
631 EXPECT_TRUE(oc.configure(ocm, tf));
632
633 EXPECT_TRUE(oc.decide(robot_state).satisfied);
634 EXPECT_TRUE(oc.equal(oc, 1e-12));
635 EXPECT_TRUE(oc.mobileReferenceFrame());
636
637 ASSERT_TRUE(oc.getLinkModel());
638
639 geometry_msgs::msg::Pose p = tf2::toMsg(robot_state.getGlobalLinkTransform(oc.getLinkModel()->getName()));
640
641 ocm.orientation = p.orientation;
642 ocm.header.frame_id = robot_model_->getModelFrame();
643 EXPECT_TRUE(oc.configure(ocm, tf));
644 EXPECT_TRUE(oc.decide(robot_state).satisfied);
645
646 std::map<std::string, double> jvals;
647 jvals["r_wrist_roll_joint"] = .05;
648 robot_state.setVariablePositions(jvals);
649 robot_state.update();
650 EXPECT_TRUE(oc.decide(robot_state).satisfied);
651
652 jvals["r_wrist_roll_joint"] = .11;
653 robot_state.setVariablePositions(jvals);
654 robot_state.update();
655 EXPECT_FALSE(oc.decide(robot_state).satisfied);
656
657 // rotation by pi does not wrap to zero
658 jvals["r_wrist_roll_joint"] = M_PI;
659 robot_state.setVariablePositions(jvals);
660 robot_state.update();
661 EXPECT_FALSE(oc.decide(robot_state).satisfied);
662}
663
664TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple)
665{
666 moveit::core::RobotState robot_state(robot_model_);
667 robot_state.setToDefaultValues();
668 robot_state.update();
669 moveit::core::Transforms tf(robot_model_->getModelFrame());
670
671 kinematic_constraints::VisibilityConstraint vc(robot_model_);
672 moveit_msgs::msg::VisibilityConstraint vcm;
673
674 EXPECT_FALSE(vc.configure(vcm, tf));
675
676 vcm.sensor_pose.header.frame_id = "base_footprint";
677 vcm.sensor_pose.pose.position.z = -1.0;
678 vcm.sensor_pose.pose.orientation.x = 0.0;
679 vcm.sensor_pose.pose.orientation.y = 1.0;
680 vcm.sensor_pose.pose.orientation.z = 0.0;
681 vcm.sensor_pose.pose.orientation.w = 0.0;
682
683 vcm.target_pose.header.frame_id = "base_footprint";
684 vcm.target_pose.pose.position.z = -2.0;
685 vcm.target_pose.pose.orientation.y = 0.0;
686 vcm.target_pose.pose.orientation.w = 1.0;
687
688 vcm.target_radius = .2;
689 vcm.cone_sides = 10;
690 vcm.max_view_angle = 0.0;
691 vcm.max_range_angle = 0.0;
692 vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_Z;
693 vcm.weight = 1.0;
694
695 EXPECT_TRUE(vc.configure(vcm, tf));
696 // sensor and target are perfectly lined up
697 EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
698
699 vcm.max_view_angle = .1;
700
701 // true, even with view angle
702 EXPECT_TRUE(vc.configure(vcm, tf));
703 EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
704
705 // very slight angle, so still ok
706 vcm.target_pose.pose.orientation.y = 0.03;
707 vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2));
708 EXPECT_TRUE(vc.configure(vcm, tf));
709 EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
710
711 // a little bit more puts it over
712 vcm.target_pose.pose.orientation.y = 0.06;
713 vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2));
714 EXPECT_TRUE(vc.configure(vcm, tf));
715 EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
716}
717
718TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2)
719{
720 moveit::core::RobotState robot_state(robot_model_);
721 robot_state.setToDefaultValues();
722 robot_state.update();
723 moveit::core::Transforms tf(robot_model_->getModelFrame());
724
725 kinematic_constraints::VisibilityConstraint vc(robot_model_);
726 moveit_msgs::msg::VisibilityConstraint vcm;
727
728 vcm.sensor_pose.header.frame_id = "narrow_stereo_optical_frame";
729 vcm.sensor_pose.pose.position.z = 0.05;
730 vcm.sensor_pose.pose.orientation.w = 1.0;
731
732 vcm.target_pose.header.frame_id = "l_gripper_r_finger_tip_link";
733 vcm.target_pose.pose.position.z = 0.03;
734 vcm.target_pose.pose.orientation.w = 1.0;
735
736 vcm.cone_sides = 10;
737 vcm.max_view_angle = 0.0;
738 vcm.max_range_angle = 0.0;
739 vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_Z;
740 vcm.weight = 1.0;
741
742 // false because target radius is 0.0
743 EXPECT_FALSE(vc.configure(vcm, tf));
744
745 // this is all fine
746 vcm.target_radius = .05;
747 EXPECT_TRUE(vc.configure(vcm, tf));
748 EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
749
750 // this moves into collision with the cone, and should register false
751 std::map<std::string, double> state_values;
752 state_values["l_shoulder_lift_joint"] = .5;
753 state_values["r_shoulder_pan_joint"] = .5;
754 state_values["r_elbow_flex_joint"] = -1.4;
755 robot_state.setVariablePositions(state_values);
756 robot_state.update();
757 EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
758
759 // this moves far enough away that it's fine
760 state_values["r_shoulder_pan_joint"] = .4;
761 robot_state.setVariablePositions(state_values);
762 robot_state.update();
763 EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
764
765 // this is in collision with the arm, but now the cone, and should be fine
766 state_values["l_shoulder_lift_joint"] = 0;
767 state_values["r_shoulder_pan_joint"] = .5;
768 state_values["r_elbow_flex_joint"] = -.6;
769 robot_state.setVariablePositions(state_values);
770 robot_state.update();
771 EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
772
773 // this shouldn't matter
774 vcm.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_X;
775 EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
776
777 robot_state.setToDefaultValues();
778 robot_state.update();
779 // just hits finger tip
780 vcm.target_radius = .01;
781 vcm.target_pose.pose.position.z = 0.00;
782 vcm.target_pose.pose.position.x = 0.035;
783 EXPECT_TRUE(vc.configure(vcm, tf));
784 EXPECT_TRUE(vc.decide(robot_state, true).satisfied);
785
786 // larger target means it also hits finger
787 vcm.target_radius = .05;
788 EXPECT_TRUE(vc.configure(vcm, tf));
789 EXPECT_FALSE(vc.decide(robot_state, true).satisfied);
790}
791
792TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet)
793{
794 moveit::core::RobotState robot_state(robot_model_);
795 robot_state.setToDefaultValues();
796 moveit::core::Transforms tf(robot_model_->getModelFrame());
797
798 kinematic_constraints::KinematicConstraintSet kcs(robot_model_);
799 EXPECT_TRUE(kcs.empty());
800
801 moveit_msgs::msg::JointConstraint jcm;
802 jcm.joint_name = "head_pan_joint";
803 jcm.position = 0.4;
804 jcm.tolerance_above = 0.1;
805 jcm.tolerance_below = 0.05;
806 jcm.weight = 1.0;
807
808 // this is a valid constraint
809 std::vector<moveit_msgs::msg::JointConstraint> jcv;
810 jcv.push_back(jcm);
811 EXPECT_TRUE(kcs.add(jcv));
812
813 // but it isn't satisfied in the default state
814 EXPECT_FALSE(kcs.decide(robot_state).satisfied);
815
816 // now it is
817 std::map<std::string, double> jvals;
818 jvals[jcm.joint_name] = 0.41;
819 robot_state.setVariablePositions(jvals);
820 robot_state.update();
821 EXPECT_TRUE(kcs.decide(robot_state).satisfied);
822
823 // adding another constraint for a different joint
824 EXPECT_FALSE(kcs.empty());
825 kcs.clear();
826 EXPECT_TRUE(kcs.empty());
827 jcv.push_back(jcm);
828 jcv.back().joint_name = "head_tilt_joint";
829 EXPECT_TRUE(kcs.add(jcv));
830
831 // now this one isn't satisfied
832 EXPECT_FALSE(kcs.decide(robot_state).satisfied);
833
834 // now it is
835 jvals[jcv.back().joint_name] = 0.41;
836 robot_state.setVariablePositions(jvals);
837 EXPECT_TRUE(kcs.decide(robot_state).satisfied);
838
839 // changing one joint outside the bounds makes it unsatisfied
840 jvals[jcv.back().joint_name] = 0.51;
841 robot_state.setVariablePositions(jvals);
842 EXPECT_FALSE(kcs.decide(robot_state).satisfied);
843
844 // one invalid constraint makes the add return false
845 kcs.clear();
846 jcv.back().joint_name = "no_joint";
847 EXPECT_FALSE(kcs.add(jcv));
848
849 // but we can still evaluate it successfully for the remaining constraint
850 EXPECT_TRUE(kcs.decide(robot_state).satisfied);
851
852 // violating the remaining good constraint changes this
853 jvals["head_pan_joint"] = 0.51;
854 robot_state.setVariablePositions(jvals);
855 EXPECT_FALSE(kcs.decide(robot_state).satisfied);
856}
857
858TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality)
859{
860 moveit::core::RobotState robot_state(robot_model_);
861 robot_state.setToDefaultValues();
862 moveit::core::Transforms tf(robot_model_->getModelFrame());
863
864 kinematic_constraints::KinematicConstraintSet kcs(robot_model_);
865 kinematic_constraints::KinematicConstraintSet kcs2(robot_model_);
866
867 moveit_msgs::msg::JointConstraint jcm;
868 jcm.joint_name = "head_pan_joint";
869 jcm.position = 0.4;
870 jcm.tolerance_above = 0.1;
871 jcm.tolerance_below = 0.05;
872 jcm.weight = 1.0;
873
874 moveit_msgs::msg::PositionConstraint pcm;
875 pcm.link_name = "l_wrist_roll_link";
876 pcm.target_point_offset.x = 0;
877 pcm.target_point_offset.y = 0;
878 pcm.target_point_offset.z = 0;
879 pcm.constraint_region.primitives.resize(1);
880 pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
881 pcm.constraint_region.primitives[0].dimensions.resize(1);
882 pcm.constraint_region.primitives[0].dimensions[0] = 0.2;
883
884 pcm.constraint_region.primitive_poses.resize(1);
885 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
886 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
887 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
888 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
889 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
890 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
891 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
892 pcm.weight = 1.0;
893
894 pcm.header.frame_id = robot_model_->getModelFrame();
895
896 // this is a valid constraint
897 std::vector<moveit_msgs::msg::JointConstraint> jcv;
898 jcv.push_back(jcm);
899 EXPECT_TRUE(kcs.add(jcv));
900
901 std::vector<moveit_msgs::msg::PositionConstraint> pcv;
902 pcv.push_back(pcm);
903 EXPECT_TRUE(kcs.add(pcv, tf));
904
905 // now adding in reverse order
906 EXPECT_TRUE(kcs2.add(pcv, tf));
907 EXPECT_TRUE(kcs2.add(jcv));
908
909 // should be true
910 EXPECT_TRUE(kcs.equal(kcs2, .001));
911 EXPECT_TRUE(kcs2.equal(kcs, .001));
912
913 // adding another copy of one of the constraints doesn't change anything
914 jcv.push_back(jcm);
915 EXPECT_TRUE(kcs2.add(jcv));
916
917 EXPECT_TRUE(kcs.equal(kcs2, .001));
918 EXPECT_TRUE(kcs2.equal(kcs, .001));
919
920 jcm.joint_name = "head_pan_joint";
921 jcm.position = 0.35;
922 jcm.tolerance_above = 0.1;
923 jcm.tolerance_below = 0.05;
924 jcm.weight = 1.0;
925
926 jcv.push_back(jcm);
927 EXPECT_TRUE(kcs2.add(jcv));
928
929 EXPECT_FALSE(kcs.equal(kcs2, .001));
930 EXPECT_FALSE(kcs2.equal(kcs, .001));
931
932 // but they are within this margin
933 EXPECT_TRUE(kcs.equal(kcs2, .1));
934 EXPECT_TRUE(kcs2.equal(kcs, .1));
935}
936
937int main(int argc, char** argv)
938{
939 testing::InitGoogleTest(&argc, argv);
940 return RUN_ALL_TESTS();
941}
LoadPlanningModelsPr2::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition test_constraint_samplers.cpp:107
kinematic_constraints::JointConstraint
Class for handling single DOF joint constraints.
Definition kinematic_constraint.h:203
kinematic_constraints::JointConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two joint constraints are the same.
Definition kinematic_constraint.cpp:275
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::clear
void clear() override
Clear the stored constraint.
Definition kinematic_constraint.cpp:333
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::JointConstraint::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition kinematic_constraint.cpp:328
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::clear
void clear()
Clear the stored constraints.
Definition kinematic_constraint.cpp:1221
kinematic_constraints::KinematicConstraintSet::equal
bool equal(const KinematicConstraintSet &other, double margin) const
Whether or not another KinematicConstraintSet is equal to this one.
Definition kinematic_constraint.cpp:1339
kinematic_constraints::KinematicConstraintSet::empty
bool empty() const
Returns whether or not there are any constraints in the set.
Definition kinematic_constraint.h:1085
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::KinematicConstraint::getConstraintWeight
double getConstraintWeight() const
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
Definition kinematic_constraint.h:158
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition kinematic_constraint.h:355
kinematic_constraints::OrientationConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two orientation constraints are the same.
Definition kinematic_constraint.cpp:675
kinematic_constraints::OrientationConstraint::getLinkModel
const moveit::core::LinkModel * getLinkModel() const
Gets the subject link model.
Definition kinematic_constraint.h:415
kinematic_constraints::OrientationConstraint::mobileReferenceFrame
bool mobileReferenceFrame() const
Whether or not a mobile reference frame is being employed.
Definition kinematic_constraint.h:437
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::enabled
bool enabled() const override
This function returns true if this constraint is configured and able to decide whether states do meet...
Definition kinematic_constraint.cpp:589
kinematic_constraints::PositionConstraint::clear
void clear() override
Clear the stored constraint.
Definition kinematic_constraint.cpp:578
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
kinematic_constraints::PositionConstraint::mobileReferenceFrame
bool mobileReferenceFrame() const
If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false.
Definition kinematic_constraint.h:662
kinematic_constraints::PositionConstraint::equal
bool equal(const KinematicConstraint &other, double margin) const override
Check if two constraints are the same. For position constraints this means that:
Definition kinematic_constraint.cpp:462
kinematic_constraints::PositionConstraint::hasLinkOffset
bool hasLinkOffset() const
If the constraint is enabled and the link offset is substantially different than zero.
Definition kinematic_constraint.h:626
kinematic_constraints::VisibilityConstraint
Class for constraints on the visibility relationship between a sensor and a target.
Definition kinematic_constraint.h:780
kinematic_constraints::VisibilityConstraint::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:1069
kinematic_constraints::VisibilityConstraint::configure
bool configure(const moveit_msgs::msg::VisibilityConstraint &vc, const moveit::core::Transforms &tf)
Configure the constraint based on a moveit_msgs::msg::VisibilityConstraint.
Definition kinematic_constraint.cpp:814
moveit::core::LinkModel::getName
const std::string & getName() const
The name of this link.
Definition link_model.h:86
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
Definition robot_state.cpp:349
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::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition robot_state.h:1246
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
Definition robot_state.h:515
moveit::core::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition transforms.h:59
moveit::core::Transforms::isFixedFrame
virtual bool isFixedFrame(const std::string &frame) const
Check whether a frame stays constant as the state of the robot model changes. This is true for any tr...
Definition transforms.cpp:98
kinematic_constraint.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
robot_model_test_utils.h
kinematic_constraints::ConstraintEvaluationResult
Struct for containing the results of constraint evaluation.
Definition kinematic_constraint.h:56
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
Whether or not the constraint or constraints were satisfied.
Definition kinematic_constraint.h:70
kinematic_constraints::ConstraintEvaluationResult::distance
double distance
The distance evaluation from the constraint or constraints.
Definition kinematic_constraint.h:71
TEST_F
TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
Definition test_constraints.cpp:61
main
int main(int argc, char **argv)
Definition test_constraints.cpp:937
Generated by doxygen 1.9.8