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

moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Nov 3 2024 03:26:15