moveit2
The MoveIt Motion Planning Framework for ROS 2.
unittest_trajectory_generator_circ.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018 Pilz GmbH & Co. KG
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 Pilz GmbH & Co. KG 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 #include <memory>
36 
37 #include <gtest/gtest.h>
38 
43 #include <tf2_eigen/tf2_eigen.hpp>
44 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
45 
50 #include "test_utils.h"
51 
52 #include <rclcpp/rclcpp.hpp>
53 
54 using namespace pilz_industrial_motion_planner;
56 
57 static const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
58 
59 class TrajectoryGeneratorCIRCTest : public testing::Test
60 {
61 protected:
66  void SetUp() override
67  {
68  rclcpp::NodeOptions node_options;
69  node_options.automatically_declare_parameters_from_overrides(true);
70  node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_circ", node_options);
71 
72  // load robot model
73  rm_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(node_);
74  robot_model_ = rm_loader_->getModel();
75  ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
76  planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
77 
78  // get parameters
79  ASSERT_TRUE(node_->has_parameter("testdata_file_name"));
80  node_->get_parameter<std::string>("testdata_file_name", test_data_file_name_);
81  ASSERT_TRUE(node_->has_parameter("planning_group"));
82  node_->get_parameter<std::string>("planning_group", planning_group_);
83  ASSERT_TRUE(node_->has_parameter("target_link"));
84  node_->get_parameter<std::string>("target_link", target_link_);
85  ASSERT_TRUE(node_->has_parameter("cartesian_position_tolerance"));
86  node_->get_parameter<double>("cartesian_position_tolerance", cartesian_position_tolerance_);
87  ASSERT_TRUE(node_->has_parameter("angular_acc_tolerance"));
88  node_->get_parameter<double>("angular_acc_tolerance", angular_acc_tolerance_);
89  ASSERT_TRUE(node_->has_parameter("rot_axis_norm_tolerance"));
90  node_->get_parameter<double>("rot_axis_norm_tolerance", rot_axis_norm_tolerance_);
91  ASSERT_TRUE(node_->has_parameter("acceleration_tolerance"));
92  node_->get_parameter<double>("acceleration_tolerance", acceleration_tolerance_);
93  ASSERT_TRUE(node_->has_parameter("other_tolerance"));
94  node_->get_parameter<double>("other_tolerance", other_tolerance_);
95 
96  // check robot model
97  testutils::checkRobotModel(robot_model_, planning_group_, target_link_);
98 
99  // load the test data provider
100  tdp_ = std::make_unique<pilz_industrial_motion_planner_testutils::XmlTestdataLoader>(test_data_file_name_);
101  ASSERT_NE(nullptr, tdp_) << "Failed to load test data by provider.";
102 
103  tdp_->setRobotModel(robot_model_);
104 
105  // create the limits container
108  node_, PARAM_NAMESPACE_LIMITS, robot_model_->getActiveJointModels());
109  CartesianLimit cart_limits;
110  // Cartesian limits are chose as such values to ease the manually compute the
111  // trajectory
112 
113  cart_limits.setMaxRotationalVelocity(1 * M_PI);
114  cart_limits.setMaxTranslationalAcceleration(1 * M_PI);
115  cart_limits.setMaxTranslationalDeceleration(1 * M_PI);
116  cart_limits.setMaxTranslationalVelocity(1 * M_PI);
117  planner_limits_.setJointLimits(joint_limits);
118  planner_limits_.setCartesianLimits(cart_limits);
119 
120  // initialize the LIN trajectory generator
121  circ_ = std::make_unique<TrajectoryGeneratorCIRC>(robot_model_, planner_limits_, planning_group_);
122  ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator";
123  }
124 
127  {
128  moveit_msgs::msg::MotionPlanResponse res_msg;
129  res.getMessage(res_msg);
130  EXPECT_TRUE(testutils::isGoalReached(res.trajectory_->getFirstWayPointPtr()->getRobotModel(),
131  res_msg.trajectory.joint_trajectory, req, other_tolerance_));
132 
133  EXPECT_TRUE(
134  testutils::checkJointTrajectory(res_msg.trajectory.joint_trajectory, planner_limits_.getJointLimitContainer()));
135 
136  EXPECT_EQ(req.path_constraints.position_constraints.size(), 1u);
137  EXPECT_EQ(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.size(), 1u);
138 
139  // Check that all point have the equal distance to the center
140  Eigen::Vector3d circ_center;
141  getCircCenter(req, res, circ_center);
142 
143  for (std::size_t i = 0; i < res.trajectory_->getWayPointCount(); ++i)
144  {
145  Eigen::Affine3d waypoint_pose = res.trajectory_->getWayPointPtr(i)->getFrameTransform(target_link_);
146  EXPECT_NEAR(
147  (res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation() - circ_center).norm(),
148  (circ_center - waypoint_pose.translation()).norm(), cartesian_position_tolerance_);
149  }
150 
151  // check translational and rotational paths
152  ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_, acceleration_tolerance_));
153  ASSERT_TRUE(testutils::checkCartesianRotationalPath(res.trajectory_, target_link_, angular_acc_tolerance_,
154  rot_axis_norm_tolerance_));
155 
156  for (size_t idx = 0; idx < res.trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx)
157  {
158  EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_);
159  EXPECT_NEAR(0.0, res.trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_);
160  }
161  }
162 
163  void TearDown() override
164  {
165  robot_model_.reset();
166  }
167 
170  {
171  if (req.path_constraints.name == "center")
172  {
173  tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
174  circ_center);
175  }
176  else if (req.path_constraints.name == "interim")
177  {
178  Eigen::Vector3d interim;
179  tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
180  interim);
181  Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation();
182  Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation();
183 
184  const Eigen::Vector3d t = interim - start;
185  const Eigen::Vector3d u = goal - start;
186  const Eigen::Vector3d v = goal - interim;
187 
188  const Eigen::Vector3d w = t.cross(u);
189 
190  ASSERT_GT(w.norm(), 1e-8) << "Circle center not well defined for given start, interim and goal.";
191 
192  circ_center = start + (u * t.dot(t) * u.dot(v) - t * u.dot(u) * t.dot(v)) * 0.5 / pow(w.norm(), 2);
193  }
194  }
195 
196 protected:
197  // ros stuff
198  rclcpp::Node::SharedPtr node_;
199  moveit::core::RobotModelConstPtr robot_model_;
200  std::unique_ptr<robot_model_loader::RobotModelLoader> rm_loader_;
201  planning_scene::PlanningSceneConstPtr planning_scene_;
202 
203  std::unique_ptr<TrajectoryGeneratorCIRC> circ_;
204  // test data provider
205  std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader> tdp_;
206 
207  // test parameters from parameter server
208  std::string planning_group_, target_link_, test_data_file_name_;
210  double cartesian_position_tolerance_, angular_acc_tolerance_, rot_axis_norm_tolerance_, acceleration_tolerance_,
213 };
214 
219 TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping)
220 {
221  {
222  std::shared_ptr<CircleNoPlane> cnp_ex{ new CircleNoPlane("") };
223  EXPECT_EQ(cnp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
224  }
225 
226  {
227  std::shared_ptr<CircleToSmall> cts_ex{ new CircleToSmall("") };
228  EXPECT_EQ(cts_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
229  }
230 
231  {
232  std::shared_ptr<CenterPointDifferentRadius> cpdr_ex{ new CenterPointDifferentRadius("") };
233  EXPECT_EQ(cpdr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
234  }
235 
236  {
237  std::shared_ptr<CircTrajectoryConversionFailure> ctcf_ex{ new CircTrajectoryConversionFailure("") };
238  EXPECT_EQ(ctcf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
239  }
240 
241  {
242  std::shared_ptr<UnknownPathConstraintName> upcn_ex{ new UnknownPathConstraintName("") };
243  EXPECT_EQ(upcn_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
244  }
245 
246  {
247  std::shared_ptr<NoPositionConstraints> npc_ex{ new NoPositionConstraints("") };
248  EXPECT_EQ(npc_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
249  }
250 
251  {
252  std::shared_ptr<NoPrimitivePose> npp_ex{ new NoPrimitivePose("") };
253  EXPECT_EQ(npp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
254  }
255 
256  {
257  std::shared_ptr<UnknownLinkNameOfAuxiliaryPoint> ulnoap_ex{ new UnknownLinkNameOfAuxiliaryPoint("") };
258  EXPECT_EQ(ulnoap_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME);
259  }
260 
261  {
262  std::shared_ptr<NumberOfConstraintsMismatch> nocm_ex{ new NumberOfConstraintsMismatch("") };
263  EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
264  }
265 
266  {
267  std::shared_ptr<CircJointMissingInStartState> cjmiss_ex{ new CircJointMissingInStartState("") };
268  EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
269  }
270 
271  {
272  std::shared_ptr<CircInverseForGoalIncalculable> cifgi_ex{ new CircInverseForGoalIncalculable("") };
273  EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
274  }
275 }
276 
281 {
282  LimitsContainer planner_limits;
283  EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits, planning_group_),
284  TrajectoryGeneratorInvalidLimitsException);
285 }
286 
291 TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState)
292 {
293  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
294 
295  planning_interface::MotionPlanRequest req{ circ.toRequest() };
296  EXPECT_GT(req.start_state.joint_state.name.size(), 1u);
297  req.start_state.joint_state.name.resize(1);
298  req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes
299 
301  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
302  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
303 }
304 
308 TEST_F(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity)
309 {
310  moveit_msgs::msg::MotionPlanRequest req{ tdp_->getCircJointCenterCart("circ1_center_2").toRequest() };
311 
312  // start state has non-zero velocity
313  req.start_state.joint_state.velocity.push_back(1.0);
315  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
316  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
317  req.start_state.joint_state.velocity.clear();
318 }
319 
321 {
322  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
323 
325  EXPECT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res));
326  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
327 }
328 
333 {
334  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
335 
336  circ.setVelocityScale(1.0);
338  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
339  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED);
340 }
341 
346 {
347  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
348 
349  circ.setAccelerationScale(1.0);
351  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
352  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED);
353 }
354 
359 TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithCenter)
360 {
361  // Define auxiliary point and goal to be the same as the start
362  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
363  circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
364  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 1e-8;
365  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1e-8;
366  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.z += 1e-8;
367  circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
368  circ.getGoalConfiguration().getPose().position.x -= 1e-8;
369  circ.getGoalConfiguration().getPose().position.y -= 1e-8;
370  circ.getGoalConfiguration().getPose().position.z -= 1e-8;
371 
373  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
374  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
375 }
376 
382 TEST_F(TrajectoryGeneratorCIRCTest, samePointsWithInterim)
383 {
384  // Define auxiliary point and goal to be the same as the start
385  auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
386  circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
387  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 1e-8;
388  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1e-8;
389  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.z += 1e-8;
390  circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
391  circ.getGoalConfiguration().getPose().position.x -= 1e-8;
392  circ.getGoalConfiguration().getPose().position.y -= 1e-8;
393  circ.getGoalConfiguration().getPose().position.z -= 1e-8;
394 
396  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
397  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
398 }
399 
404 {
405  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
406 
407  planning_interface::MotionPlanRequest req = circ.toRequest();
408 
409  req.path_constraints.position_constraints.clear();
410 
412  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
413  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
414 }
415 
420 {
421  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
422 
423  planning_interface::MotionPlanRequest req = circ.toRequest();
424 
425  req.path_constraints.name = "";
426 
428  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
429  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
430 }
431 
437 {
438  auto circ{ tdp_->getCircJointInterimCart("circ3_interim") };
439 
440  planning_interface::MotionPlanRequest req = circ.toRequest();
441 
442  req.path_constraints.position_constraints.front().link_name = "INVALID";
443 
445  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
446  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME);
447 }
448 
453 {
454  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
455  circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
456  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1;
457 
459  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
460  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
461 }
462 
469 {
470  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
471  circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
472  circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
473 
474  // Stretch start and goal pose along line
475  circ.getStartConfiguration().getPose().position.x -= 0.1;
476  circ.getGoalConfiguration().getPose().position.x += 0.1;
477 
479  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
480  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
481 }
482 
490 {
491  auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
492  circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
493  circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
494 
495  // Stretch start and goal pose along line
496  circ.getStartConfiguration().getPose().position.x -= 0.1;
497  circ.getGoalConfiguration().getPose().position.x += 0.1;
498 
500  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
501  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
502 }
503 
512 TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim)
513 {
514  // get the test data from xml
515  auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
516 
518  ASSERT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res));
519  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
520 }
521 
532 TEST_F(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim)
533 {
534  auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
535 
536  // alter start, interim and goal such that start/center and interim are
537  // colinear
538  circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
539  circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
540 
541  circ.getStartConfiguration().getPose().position.x -= 0.2;
542  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 0.2;
543  circ.getGoalConfiguration().getPose().position.y -= 0.2;
544 
545  circ.setAccelerationScale(0.05);
546  circ.setVelocityScale(0.05);
547 
548  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
549 
551  EXPECT_TRUE(circ_->generate(planning_scene_, req, res));
552  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
553  checkCircResult(req, res);
554 }
555 
565 TEST_F(TrajectoryGeneratorCIRCTest, interimLarger180Degree)
566 {
567  auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
568 
569  // alter start, interim and goal such that start/center and interim are
570  // colinear
571  circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
572  circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
573 
574  circ.getStartConfiguration().getPose().position.x -= 0.2;
575  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 0.14142136;
576  circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y -= 0.14142136;
577  circ.getGoalConfiguration().getPose().position.y -= 0.2;
578 
579  circ.setAccelerationScale(0.05);
580  circ.setVelocityScale(0.05);
581 
582  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
583 
585  EXPECT_TRUE(circ_->generate(planning_scene_, req, res));
586  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
587  checkCircResult(req, res);
588 }
589 
593 TEST_F(TrajectoryGeneratorCIRCTest, centerPointJointGoal)
594 {
595  auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") };
596  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
597 
599  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
600  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
601  checkCircResult(req, res);
602 }
603 
609 TEST_F(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose)
610 {
611  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
612 
613  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
614 
615  // Contains one pose (interim / center)
616  ASSERT_EQ(req.path_constraints.position_constraints.back().constraint_region.primitive_poses.size(), 1u);
617 
618  // Define a additional pose here
619  geometry_msgs::msg::Pose center_position;
620  center_position.position.x = 0.0;
621  center_position.position.y = 0.0;
622  center_position.position.z = 0.65;
623  req.path_constraints.position_constraints.back().constraint_region.primitive_poses.push_back(center_position);
624 
626  ASSERT_FALSE(circ_->generate(planning_scene_, req, res));
627  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
628 }
629 
636 TEST_F(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint)
637 {
638  auto circ{ tdp_->getCircJointCenterCart("circ1_center_2") };
639 
640  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
641 
642  // Define the additional joint constraint
643  moveit_msgs::msg::JointConstraint joint_constraint;
644  joint_constraint.joint_name = req.goal_constraints.front().joint_constraints.front().joint_name;
645  req.goal_constraints.front().joint_constraints.push_back(joint_constraint); //<-- Additional constraint
646 
648  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
649  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
650 }
651 
655 TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal)
656 {
657  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
658 
659  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
660 
662  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
663  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
664  checkCircResult(req, res);
665 }
666 
670 TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints)
671 {
672  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
673 
674  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
675 
676  req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame();
677 
679  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
680  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
681  checkCircResult(req, res);
682 }
683 
687 TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints)
688 {
689  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
690 
691  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
692  req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame();
693 
695  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
696  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
697  checkCircResult(req, res);
698 }
699 
703 TEST_F(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints)
704 {
705  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
706 
707  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
708 
709  // Both set
710  req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame();
711  req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame();
712 
714  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
715  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
716  checkCircResult(req, res);
717 }
718 
722 TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoal)
723 {
724  auto circ{ tdp_->getCircJointInterimCart("circ3_interim") };
725 
726  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
727 
729  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
730  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
731  checkCircResult(req, res);
732 }
733 
740 TEST_F(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero)
741 {
742  auto circ{ tdp_->getCircJointInterimCart("circ3_interim") };
743 
744  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
745 
746  // Set velocity near zero
747  req.start_state.joint_state.velocity = std::vector<double>(req.start_state.joint_state.position.size(), 1e-16);
748 
750  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
751  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
752  checkCircResult(req, res);
753 }
754 
758 TEST_F(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal)
759 {
760  auto circ{ tdp_->getCircJointInterimCart("circ3_interim") };
761  moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
762 
764  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
765  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
766  checkCircResult(req, res);
767 }
768 
769 int main(int argc, char** argv)
770 {
771  rclcpp::init(argc, argv);
772  testing::InitGoogleTest(&argc, argv);
773  return RUN_ALL_TESTS();
774 }
void SetUp() override
Create test scenario for circ trajectory generator.
std::unique_ptr< TrajectoryGeneratorCIRC > circ_
std::unique_ptr< pilz_industrial_motion_planner_testutils::TestdataLoader > tdp_
void getCircCenter(const planning_interface::MotionPlanRequest &req, const planning_interface::MotionPlanResponse &res, Eigen::Vector3d &circ_center)
moveit::core::RobotModelConstPtr robot_model_
void checkCircResult(const planning_interface::MotionPlanRequest &req, const planning_interface::MotionPlanResponse &res)
planning_scene::PlanningSceneConstPtr planning_scene_
std::unique_ptr< robot_model_loader::RobotModelLoader > rm_loader_
Set of cartesian limits, has values for velocity, acceleration and deceleration of both the translati...
void setMaxRotationalVelocity(double max_rot_vel)
Set the maximum rotational velocity.
void setMaxTranslationalVelocity(double max_trans_vel)
Set the maximal translational velocity.
void setMaxTranslationalAcceleration(double max_trans_acc)
Set the maximum translational acceleration.
void setMaxTranslationalDeceleration(double max_trans_dec)
Set the maximum translational deceleration.
static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr &node, const std::string &param_namespace, const std::vector< const moveit::core::JointModel * > &joint_models)
Aggregates(combines) the joint limits from joint model and node parameters. The rules for the combina...
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
This class combines CartesianLimit and JointLimits into on single class.
This class implements a trajectory generator of arcs in Cartesian space. The arc is specified by a st...
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
w
Definition: pick.py:67
TEST_F(GetSolverTipFrameIntegrationTest, TestHasSolverManipulator)
Check if hasSolver() can be called successfully for the manipulator group.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
bool isGoalReached(const trajectory_msgs::msg::JointTrajectory &trajectory, const std::vector< moveit_msgs::msg::JointConstraint > &goal, const double joint_position_tolerance, const double joint_velocity_tolerance=1.0e-6)
check if the goal given in joint space is reached Only the last point in the trajectory is veryfied.
Definition: test_utils.cpp:118
::testing::AssertionResult checkCartesianTranslationalPath(const robot_trajectory::RobotTrajectoryConstPtr &trajectory, const std::string &link_name, const double acc_tol=DEFAULT_ACCELERATION_EQUALITY_TOLERANCE)
Check that the translational path of a given trajectory has a trapezoid velocity profile.
bool checkJointTrajectory(const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
check joint trajectory of consistency, position, velocity and acceleration limits
Definition: test_utils.cpp:460
void checkRobotModel(const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name, const std::string &link_name)
::testing::AssertionResult checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr &trajectory, const std::string &link_name, const double rot_axis_tol=DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE, const double acc_tol=DEFAULT_ACCELERATION_EQUALITY_TOLERANCE)
Check that the rotational path of a given trajectory is a quaternion slerp.
robot_trajectory::RobotTrajectoryPtr trajectory_
void getMessage(moveit_msgs::msg::MotionPlanResponse &msg) const
moveit_msgs::msg::MoveItErrorCodes error_code_
int main(int argc, char **argv)