moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
47 class SphericalRobot : public testing::Test
48 {
49 protected:
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 
75 protected:
76  moveit::core::RobotModelPtr robot_model_;
77 };
78 
79 class FloatingJointRobot : public testing::Test
80 {
81 protected:
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 
129 inline Eigen::Quaterniond xyz_intrinsix_to_quat(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 
136 inline Eigen::Quaterniond rotation_vector_to_quat(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 
144 void 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 
152 TEST_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 
179 TEST_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 
224 TEST_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 
272 TEST_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
326 TEST_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 
352 TEST_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 
380 TEST_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, xyz_intrinsix_to_quat(0.1, 0.2, -0.3));
419  EXPECT_TRUE(oc_euler.decide(robot_state).satisfied);
420 
421  setRobotEndEffectorOrientation(robot_state, xyz_intrinsix_to_quat(0.1, 0.2, -0.6));
422  EXPECT_FALSE(oc_euler.decide(robot_state).satisfied);
423 
424  setRobotEndEffectorOrientation(robot_state, rotation_vector_to_quat(0.1, 0.2, -0.3));
425  EXPECT_TRUE(oc_rotvec.decide(robot_state).satisfied);
426 
427  setRobotEndEffectorOrientation(robot_state, rotation_vector_to_quat(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(xyz_intrinsix_to_quat(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(xyz_intrinsix_to_quat(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(rotation_vector_to_quat(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(rotation_vector_to_quat(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 
478 int main(int argc, char** argv)
479 {
480  testing::InitGoogleTest(&argc, argv);
481  return RUN_ALL_TESTS();
482 }
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:348
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:558
kinematic_constraints::OrientationConstraint::getParameterizationType
int getParameterizationType() const
Definition: kinematic_constraint.h:481
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:670
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition: robot_model_test_utils.h:85
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:116
moveit::core::RobotModelBuilder::isValid
bool isValid()
Returns true if the building process so far has been valid.
Definition: robot_model_test_utils.cpp:366
moveit::core::RobotModelBuilder::build
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
Definition: robot_model_test_utils.cpp:371
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:370
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:1339
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:605
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:656
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:361
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
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
pick.p
p
Definition: pick.py:62
pick.robot
robot
Definition: pick.py:53
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:478
rotation_vector_to_quat
Eigen::Quaterniond rotation_vector_to_quat(double rx, double ry, double rz)
Definition: test_orientation_constraints.cpp:136
xyz_intrinsix_to_quat
Eigen::Quaterniond xyz_intrinsix_to_quat(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.1