moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_orientation_constraints.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2020, KU Leuven
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 KU Leuven 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: Jeroen De Maeyer */
36
37#include <moveit/kinematic_constraints/kinematic_constraint.h>
38#include <gtest/gtest.h>
39
40#include <urdf_parser/urdf_parser.h>
41#include <tf2_eigen/tf2_eigen.hpp>
42#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
43#include <math.h>
44
45#include <moveit/utils/robot_model_test_utils.h>
46
47class SphericalRobot : public testing::Test
48{
49protected:
50 void SetUp() override
51 {
52 moveit::core::RobotModelBuilder builder("robot", "base_link");
53 geometry_msgs::msg::Pose origin;
54 origin.orientation.w = 1;
55 builder.addChain("base_link->roll", "revolute", { origin }, urdf::Vector3(1, 0, 0));
56 builder.addChain("roll->pitch", "revolute", { origin }, urdf::Vector3(0, 1, 0));
57 builder.addChain("pitch->yaw", "revolute", { origin }, urdf::Vector3(0, 0, 1));
58 ASSERT_TRUE(builder.isValid());
59 robot_model_ = builder.build();
60 }
61
62 std::map<std::string, double> getJointValues(const double roll, const double pitch, const double yaw)
63 {
64 std::map<std::string, double> jvals;
65 jvals["base_link-roll-joint"] = roll;
66 jvals["roll-pitch-joint"] = pitch;
67 jvals["pitch-yaw-joint"] = yaw;
68 return jvals;
69 }
70
71 void TearDown() override
72 {
73 }
74
75protected:
76 moveit::core::RobotModelPtr robot_model_;
77};
78
79class FloatingJointRobot : public testing::Test
80{
81protected:
83 moveit::core::RobotModelPtr robot_model_;
84
94 Eigen::Matrix<double, 8, 4> valid_euler_data_;
95 Eigen::Matrix<double, 8, 4> valid_rotvec_data_;
96
97 void SetUp() override
98 {
99 // create robot
100 moveit::core::RobotModelBuilder robot("floating_robot", "base_link");
101 robot.addChain("base_link->ee", "floating");
102 robot.addGroupChain("base_link", "ee", "group1");
103
104 ASSERT_TRUE(robot.isValid());
105 robot_model_ = robot.build();
106
107 // hardcoded test data created with an external python script
108 valid_euler_data_ << -0.1712092272140422, -0.2853625129991958, -0.1712092272140422, 0.9273311367620117,
109 -0.28536251299919585, -0.17120922721404216, 0.28536251299919585, 0.8987902273981057, 0.28536251299919585,
110 -0.17120922721404216, -0.28536251299919585, 0.8987902273981057, 0.1712092272140422, -0.2853625129991958,
111 0.1712092272140422, 0.9273311367620117, -0.28536251299919585, 0.17120922721404216, -0.28536251299919585,
112 0.8987902273981057, -0.1712092272140422, 0.2853625129991958, 0.1712092272140422, 0.9273311367620117,
113 0.1712092272140422, 0.2853625129991958, -0.1712092272140422, 0.9273311367620117, 0.28536251299919585,
114 0.17120922721404216, 0.28536251299919585, 0.8987902273981057;
115 valid_rotvec_data_ << -0.23771285949073287, -0.23771285949073287, -0.23771285949073287, 0.911305541132181,
116 -0.2401272988734743, -0.2401272988734743, 0.0, 0.9405731022474852, -0.23771285949073287, -0.23771285949073287,
117 0.23771285949073287, 0.911305541132181, 0.0, -0.24012729887347428, -0.24012729887347428, 0.9405731022474851,
118 0.0, -0.24012729887347428, 0.24012729887347428, 0.9405731022474851, 0.23771285949073287, -0.23771285949073287,
119 -0.23771285949073287, 0.911305541132181, 0.2401272988734743, -0.2401272988734743, 0.0, 0.9405731022474852,
120 0.23771285949073287, -0.23771285949073287, 0.23771285949073287, 0.911305541132181;
121 }
122
123 void TearDown() override
124 {
125 }
126};
127
129inline Eigen::Quaterniond xyzIntrinsixToQuaternion(double rx, double ry, double rz)
130{
131 return Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) *
132 Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());
133}
134
136inline Eigen::Quaterniond rotationVectorToQuaternion(double rx, double ry, double rz)
137{
138 Eigen::Vector3d v{ rx, ry, rz };
139 Eigen::Matrix3d m{ Eigen::AngleAxisd(v.norm(), v.normalized()) };
140 return Eigen::Quaterniond{ m };
141}
142
144void setRobotEndEffectorOrientation(moveit::core::RobotState& robot_state, const Eigen::Quaterniond& quat)
145{
146 Eigen::VectorXd joint_values(7);
147 joint_values << 0.0, 0.0, 0.0, quat.x(), quat.y(), quat.z(), quat.w();
148 robot_state.setJointGroupPositions("group1", joint_values);
149 robot_state.update();
150}
151
152TEST_F(SphericalRobot, Test1)
153{
154 kinematic_constraints::OrientationConstraint oc(robot_model_);
155
156 moveit::core::Transforms tf(robot_model_->getModelFrame());
157 moveit_msgs::msg::OrientationConstraint ocm;
158
159 ocm.link_name = "yaw";
160 ocm.header.frame_id = robot_model_->getModelFrame();
161 ocm.orientation.x = 0.0;
162 ocm.orientation.y = 0.0;
163 ocm.orientation.z = 0.0;
164 ocm.orientation.w = 1.0;
165 ocm.absolute_x_axis_tolerance = 0.8;
166 ocm.absolute_y_axis_tolerance = 0.8;
167 ocm.absolute_z_axis_tolerance = 3.15;
168 ocm.weight = 1.0;
169
170 moveit::core::RobotState robot_state(robot_model_);
171 // This's identical to a -1.57rad rotation around Z-axis
172 robot_state.setVariablePositions(getJointValues(3.140208, 3.141588, 1.570821));
173 robot_state.update();
174
175 EXPECT_TRUE(oc.configure(ocm, tf));
176 EXPECT_TRUE(oc.decide(robot_state).satisfied);
177}
178
179TEST_F(SphericalRobot, Test2)
180{
181 kinematic_constraints::OrientationConstraint oc(robot_model_);
182
183 moveit::core::Transforms tf(robot_model_->getModelFrame());
184 moveit_msgs::msg::OrientationConstraint ocm;
185
186 ocm.link_name = "yaw";
187 ocm.header.frame_id = robot_model_->getModelFrame();
188 ocm.orientation.x = 0.0;
189 ocm.orientation.y = 0.0;
190 ocm.orientation.z = 0.0;
191 ocm.orientation.w = 1.0;
192 ocm.absolute_x_axis_tolerance = 0.2;
193 ocm.absolute_y_axis_tolerance = 2.0;
194 ocm.absolute_z_axis_tolerance = 0.2;
195 ocm.weight = 1.0;
196
197 moveit::core::RobotState robot_state(robot_model_);
198 // Singularity: roll + yaw = theta
199 // These violate either absolute_x_axis_tolerance or absolute_z_axis_tolerance
200 robot_state.setVariablePositions(getJointValues(0.15, M_PI_2, 0.15));
201 robot_state.update();
202 EXPECT_TRUE(oc.configure(ocm, tf));
203 EXPECT_FALSE(oc.decide(robot_state).satisfied);
204
205 robot_state.setVariablePositions(getJointValues(0.21, M_PI_2, 0.0));
206 robot_state.update();
207 EXPECT_TRUE(oc.configure(ocm, tf));
208 EXPECT_FALSE(oc.decide(robot_state).satisfied);
209
210 robot_state.setVariablePositions(getJointValues(0.0, M_PI_2, 0.21));
211 robot_state.update();
212 EXPECT_TRUE(oc.configure(ocm, tf));
213 EXPECT_FALSE(oc.decide(robot_state).satisfied);
214
215 // Singularity: roll - yaw = theta
216 // This's identical to -pi/2 pitch rotation
217 robot_state.setVariablePositions(getJointValues(0.15, -M_PI_2, 0.15));
218 robot_state.update();
219
220 EXPECT_TRUE(oc.configure(ocm, tf));
221 EXPECT_TRUE(oc.decide(robot_state).satisfied);
222}
223
224TEST_F(SphericalRobot, Test3)
225{
226 kinematic_constraints::OrientationConstraint oc(robot_model_);
227
228 moveit::core::Transforms tf(robot_model_->getModelFrame());
229 moveit_msgs::msg::OrientationConstraint ocm;
230
231 ocm.link_name = "yaw";
232 ocm.header.frame_id = robot_model_->getModelFrame();
233 ocm.orientation.x = 0.0;
234 ocm.orientation.y = 0.0;
235 ocm.orientation.z = 0.0;
236 ocm.orientation.w = 1.0;
237 ocm.absolute_x_axis_tolerance = 0.2;
238 ocm.absolute_y_axis_tolerance = 2.0;
239 ocm.absolute_z_axis_tolerance = 0.3;
240 ocm.weight = 1.0;
241
242 moveit::core::RobotState robot_state(robot_model_);
243 // Singularity: roll + yaw = theta
244
245 // These tests violate absolute_x_axis_tolerance
246 robot_state.setVariablePositions(getJointValues(0.21, M_PI_2, 0.0));
247 robot_state.update();
248 EXPECT_TRUE(oc.configure(ocm, tf));
249 EXPECT_FALSE(oc.decide(robot_state).satisfied);
250
251 robot_state.setVariablePositions(getJointValues(0.0, M_PI_2, 0.21));
252 robot_state.update();
253 EXPECT_TRUE(oc.configure(ocm, tf));
254 EXPECT_FALSE(oc.decide(robot_state).satisfied);
255
256 ocm.absolute_x_axis_tolerance = 0.3;
257 ocm.absolute_y_axis_tolerance = 2.0;
258 ocm.absolute_z_axis_tolerance = 0.2;
259
260 // These tests violate absolute_z_axis_tolerance
261 robot_state.setVariablePositions(getJointValues(0.21, M_PI_2, 0.0));
262 robot_state.update();
263 EXPECT_TRUE(oc.configure(ocm, tf));
264 EXPECT_FALSE(oc.decide(robot_state).satisfied);
265
266 robot_state.setVariablePositions(getJointValues(0.0, M_PI_2, 0.21));
267 robot_state.update();
268 EXPECT_TRUE(oc.configure(ocm, tf));
269 EXPECT_FALSE(oc.decide(robot_state).satisfied);
270}
271
272TEST_F(SphericalRobot, Test4)
273{
274 kinematic_constraints::OrientationConstraint oc(robot_model_);
275
276 moveit::core::Transforms tf(robot_model_->getModelFrame());
277 moveit_msgs::msg::OrientationConstraint ocm;
278
279 ocm.link_name = "yaw";
280 ocm.header.frame_id = robot_model_->getModelFrame();
281 ocm.orientation.x = 0.0;
282 ocm.orientation.y = 0.0;
283 ocm.orientation.z = 0.0;
284 ocm.orientation.w = 1.0;
285 ocm.absolute_x_axis_tolerance = 0.2;
286 ocm.absolute_y_axis_tolerance = 2.0;
287 ocm.absolute_z_axis_tolerance = 0.3;
288 ocm.weight = 1.0;
289
290 moveit::core::RobotState robot_state(robot_model_);
291 // Singularity: roll + yaw = theta
292
293 // These tests violate absolute_x_axis_tolerance
294 robot_state.setVariablePositions(getJointValues(0.21, -M_PI_2, 0.0));
295 robot_state.update();
296 EXPECT_TRUE(oc.configure(ocm, tf));
297 EXPECT_FALSE(oc.decide(robot_state).satisfied);
298
299 robot_state.setVariablePositions(getJointValues(0.0, -M_PI_2, 0.21));
300 robot_state.update();
301 EXPECT_TRUE(oc.configure(ocm, tf));
302 EXPECT_FALSE(oc.decide(robot_state).satisfied);
303
304 ocm.absolute_x_axis_tolerance = 0.3;
305 ocm.absolute_y_axis_tolerance = 2.0;
306 ocm.absolute_z_axis_tolerance = 0.2;
307
308 // These tests violate absolute_z_axis_tolerance
309 robot_state.setVariablePositions(getJointValues(0.21, -M_PI_2, 0.0));
310 robot_state.update();
311 EXPECT_TRUE(oc.configure(ocm, tf));
312 EXPECT_FALSE(oc.decide(robot_state).satisfied);
313
314 robot_state.setVariablePositions(getJointValues(0.0, -M_PI_2, 0.21));
315 robot_state.update();
316 EXPECT_TRUE(oc.configure(ocm, tf));
317 EXPECT_FALSE(oc.decide(robot_state).satisfied);
318
319 robot_state.setVariablePositions(getJointValues(0.5, -M_PI_2, 0.21));
320 robot_state.update();
321 EXPECT_TRUE(oc.configure(ocm, tf));
322 EXPECT_FALSE(oc.decide(robot_state).satisfied);
323}
324
325// Both the current and the desired orientations are in singularities
326TEST_F(SphericalRobot, Test5)
327{
328 kinematic_constraints::OrientationConstraint oc(robot_model_);
329
330 moveit::core::Transforms tf(robot_model_->getModelFrame());
331 moveit_msgs::msg::OrientationConstraint ocm;
332
333 moveit::core::RobotState robot_state(robot_model_);
334 robot_state.setVariablePositions(getJointValues(0.0, M_PI_2, 0.0));
335 robot_state.update();
336
337 ocm.link_name = "yaw";
338 ocm.header.frame_id = robot_model_->getModelFrame();
339 ocm.orientation = tf2::toMsg(Eigen::Quaterniond(robot_state.getGlobalLinkTransform(ocm.link_name).linear()));
340 ocm.absolute_x_axis_tolerance = 0.0;
341 ocm.absolute_y_axis_tolerance = 0.0;
342 ocm.absolute_z_axis_tolerance = 1.0;
343 ocm.weight = 1.0;
344
345 robot_state.setVariablePositions(getJointValues(0.2, M_PI_2, 0.3));
346 robot_state.update();
347
348 EXPECT_TRUE(oc.configure(ocm, tf));
349 EXPECT_TRUE(oc.decide(robot_state, true).satisfied);
350}
351
352TEST_F(FloatingJointRobot, TestDefaultParameterization)
353{
354 // Simple test that checks whether the configuration defaults to the expected parameterization
355 // if we put an invalid type in the message.
356 moveit::core::Transforms tf(robot_model_->getModelFrame());
357
358 moveit_msgs::msg::OrientationConstraint ocm;
359 ocm.link_name = "ee";
360 ocm.header.frame_id = robot_model_->getModelFrame();
361 ocm.orientation.y = 0.0;
362 ocm.orientation.z = 0.0;
363 ocm.orientation.w = 1.0;
364 ocm.absolute_x_axis_tolerance = 0.5;
365 ocm.absolute_y_axis_tolerance = 0.5;
366 ocm.absolute_z_axis_tolerance = 0.5;
367 ocm.weight = 1.0;
368
369 // set to non-existing parameterization on purpose
370 ocm.parameterization = 99;
371
372 // configuring the constraints should still work
373 kinematic_constraints::OrientationConstraint oc(robot_model_);
374 EXPECT_TRUE(oc.configure(ocm, tf));
375
376 // check if the expected default parameterization was set
377 EXPECT_EQ(oc.getParameterizationType(), moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES);
378}
379
380TEST_F(FloatingJointRobot, OrientationConstraintsParameterization)
381{
382 // load and initialize robot model
383 moveit::core::RobotState robot_state(robot_model_);
384 robot_state.setToDefaultValues();
385 robot_state.update();
386
387 // center the orientation constraints around the current orientation of the link
388 geometry_msgs::msg::Pose p = tf2::toMsg(robot_state.getGlobalLinkTransform("ee"));
389
390 // we also need the name of the base link
391 moveit::core::Transforms tf(robot_model_->getModelFrame());
392
393 // create message to configure orientation constraints
394 moveit_msgs::msg::OrientationConstraint ocm;
395 ocm.link_name = "ee";
396 ocm.header.frame_id = robot_model_->getModelFrame();
397 ocm.orientation = p.orientation;
398 ocm.absolute_x_axis_tolerance = 0.5;
399 ocm.absolute_y_axis_tolerance = 0.5;
400 ocm.absolute_z_axis_tolerance = 0.5;
401 ocm.weight = 1.0;
402
403 // create orientation constraints with the xyz_euler_angle parameterization
404 kinematic_constraints::OrientationConstraint oc_euler(robot_model_);
405 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES;
406 EXPECT_TRUE(oc_euler.configure(ocm, tf));
407
408 // create orientation constraints with the rotation_vector parameterization
409 kinematic_constraints::OrientationConstraint oc_rotvec(robot_model_);
410 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
411 EXPECT_TRUE(oc_rotvec.configure(ocm, tf));
412
413 // Constraints should be satisfied for current state because we created them around the current orientation
414 EXPECT_TRUE(oc_euler.decide(robot_state, true).satisfied);
415 EXPECT_TRUE(oc_rotvec.decide(robot_state, true).satisfied);
416
417 // some trivial test cases
418 setRobotEndEffectorOrientation(robot_state, xyzIntrinsixToQuaternion(0.1, 0.2, -0.3));
419 EXPECT_TRUE(oc_euler.decide(robot_state).satisfied);
420
421 setRobotEndEffectorOrientation(robot_state, xyzIntrinsixToQuaternion(0.1, 0.2, -0.6));
422 EXPECT_FALSE(oc_euler.decide(robot_state).satisfied);
423
424 setRobotEndEffectorOrientation(robot_state, rotationVectorToQuaternion(0.1, 0.2, -0.3));
425 EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied);
426
427 setRobotEndEffectorOrientation(robot_state, rotationVectorToQuaternion(0.1, 0.2, -0.6));
428 EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied);
429
430 // more extensive testing using the test data hardcoded at the top of this file
431 Eigen::VectorXd joint_values(7);
432 joint_values << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
433 for (Eigen::Index i_row{ 0 }; i_row < valid_euler_data_.rows(); ++i_row)
434 {
435 joint_values[3] = valid_euler_data_(i_row, 0);
436 joint_values[4] = valid_euler_data_(i_row, 1);
437 joint_values[5] = valid_euler_data_(i_row, 2);
438 joint_values[6] = valid_euler_data_(i_row, 3);
439 robot_state.setJointGroupPositions("group1", joint_values);
440 robot_state.update();
441 EXPECT_TRUE(oc_euler.decide(robot_state).satisfied);
442 EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied);
443
444 joint_values[3] = valid_rotvec_data_(i_row, 0);
445 joint_values[4] = valid_rotvec_data_(i_row, 1);
446 joint_values[5] = valid_rotvec_data_(i_row, 2);
447 joint_values[6] = valid_rotvec_data_(i_row, 3);
448 robot_state.setJointGroupPositions("group1", joint_values);
449 robot_state.update();
450 EXPECT_FALSE(oc_euler.decide(robot_state).satisfied);
451 EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied);
452 }
453
454 // and now some simple test cases where we change the nominal orientation of the constraints,
455 // instead of changing the orientation of the robot
456 robot_state.setToDefaultValues();
457 robot_state.update();
458
459 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES;
460 ocm.orientation = tf2::toMsg(xyzIntrinsixToQuaternion(0.1, 0.2, -0.3));
461 EXPECT_TRUE(oc_euler.configure(ocm, tf));
462 EXPECT_TRUE(oc_euler.decide(robot_state).satisfied);
463
464 ocm.orientation = tf2::toMsg(xyzIntrinsixToQuaternion(0.1, 0.2, -0.6));
465 EXPECT_TRUE(oc_euler.configure(ocm, tf));
466 EXPECT_FALSE(oc_euler.decide(robot_state).satisfied);
467
468 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
469 ocm.orientation = tf2::toMsg(rotationVectorToQuaternion(0.1, 0.2, -0.3));
470 EXPECT_TRUE(oc_rotvec.configure(ocm, tf));
471 EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied);
472
473 ocm.orientation = tf2::toMsg(rotationVectorToQuaternion(0.1, 0.2, -0.6));
474 EXPECT_TRUE(oc_rotvec.configure(ocm, tf));
475 EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied);
476}
477
478TEST_F(FloatingJointRobot, ToleranceInRefFrame)
479{
480 moveit::core::RobotState robot_state(robot_model_);
481 robot_state.setToDefaultValues();
482 auto base = rotationVectorToQuaternion(M_PI / 2.0, 0, 0); // base rotation: 90° about x
483 setRobotEndEffectorOrientation(robot_state, base);
484 robot_state.update();
485
486 moveit::core::Transforms tf(robot_model_->getModelFrame());
487
488 // create message to configure orientation constraints
489 moveit_msgs::msg::OrientationConstraint ocm;
490 ocm.link_name = "ee";
491 ocm.header.frame_id = robot_model_->getModelFrame();
492 ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
493 ocm.orientation = tf2::toMsg(base);
494 ocm.absolute_x_axis_tolerance = 0.2;
495 ocm.absolute_y_axis_tolerance = 0.2;
496 ocm.absolute_z_axis_tolerance = 1.0;
497 ocm.weight = 1.0;
498
499 kinematic_constraints::OrientationConstraint oc(robot_model_);
500 EXPECT_TRUE(oc.configure(ocm, tf));
501
502 EXPECT_TRUE(oc.decide(robot_state).satisfied); // link and target are perfectly aligned
503
504 // strong rotation w.r.t. base frame is ok
505 auto delta = rotationVectorToQuaternion(0.1, 0.1, 0.9);
506 setRobotEndEffectorOrientation(robot_state, delta * base);
507 EXPECT_TRUE(oc.decide(robot_state).satisfied);
508
509 // strong rotation w.r.t. link frame is not ok
510 setRobotEndEffectorOrientation(robot_state, base * delta);
511 EXPECT_FALSE(oc.decide(robot_state).satisfied); // link and target are perfectly aligned
512}
513
514int main(int argc, char** argv)
515{
516 testing::InitGoogleTest(&argc, argv);
517 return RUN_ALL_TESTS();
518}
FloatingJointRobot::valid_rotvec_data_
Eigen::Matrix< double, 8, 4 > valid_rotvec_data_
Definition test_orientation_constraints.cpp:95
FloatingJointRobot::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition test_orientation_constraints.cpp:83
FloatingJointRobot::valid_euler_data_
Eigen::Matrix< double, 8, 4 > valid_euler_data_
Definition test_orientation_constraints.cpp:94
SphericalRobot::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition test_orientation_constraints.cpp:76
SphericalRobot::getJointValues
std::map< std::string, double > getJointValues(const double roll, const double pitch, const double yaw)
Definition test_orientation_constraints.cpp:62
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::getParameterizationType
int getParameterizationType() const
Definition kinematic_constraint.h:501
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
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition robot_model_test_utils.h:112
moveit::core::RobotModelBuilder::addChain
void addChain(const std::string &section, const std::string &type, const std::vector< geometry_msgs::msg::Pose > &joint_origins={}, urdf::Vector3 joint_axis=urdf::Vector3(1.0, 0.0, 0.0))
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
Definition robot_model_test_utils.cpp:178
moveit::core::RobotModelBuilder::isValid
bool isValid()
Returns true if the building process so far has been valid.
Definition robot_model_test_utils.cpp:448
moveit::core::RobotModelBuilder::build
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
Definition robot_model_test_utils.cpp:453
moveit::core::RobotModelBuilder::addGroupChain
void addGroupChain(const std::string &base_link, const std::string &tip_link, const std::string &name="")
Adds a new group using a chain of links. The group is the parent joint of each link in the chain.
Definition robot_model_test_utils.cpp:406
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::setJointGroupPositions
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition robot_state.h:583
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::Transforms
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition transforms.h:59
kinematic_constraint.h
robot_model_test_utils.h
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
Whether or not the constraint or constraints were satisfied.
Definition kinematic_constraint.h:70
TEST_F
TEST_F(SphericalRobot, Test1)
Definition test_orientation_constraints.cpp:152
main
int main(int argc, char **argv)
Definition test_orientation_constraints.cpp:514
rotationVectorToQuaternion
Eigen::Quaterniond rotationVectorToQuaternion(double rx, double ry, double rz)
Definition test_orientation_constraints.cpp:136
xyzIntrinsixToQuaternion
Eigen::Quaterniond xyzIntrinsixToQuaternion(double rx, double ry, double rz)
Definition test_orientation_constraints.cpp:129
setRobotEndEffectorOrientation
void setRobotEndEffectorOrientation(moveit::core::RobotState &robot_state, const Eigen::Quaterniond &quat)
Definition test_orientation_constraints.cpp:144
Generated by doxygen 1.9.8