37#include <gtest/gtest.h>
43#include <tf2_eigen/tf2_eigen.hpp>
44#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
52#include <rclcpp/rclcpp.hpp>
57static const std::string PARAM_NAMESPACE_LIMITS =
"robot_description_planning";
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);
73 rm_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(
node_);
75 ASSERT_TRUE(
bool(
robot_model_)) <<
"Failed to load robot model";
79 ASSERT_TRUE(
node_->has_parameter(
"testdata_file_name"));
81 ASSERT_TRUE(
node_->has_parameter(
"planning_group"));
83 ASSERT_TRUE(
node_->has_parameter(
"target_link"));
85 ASSERT_TRUE(
node_->has_parameter(
"cartesian_position_tolerance"));
87 ASSERT_TRUE(
node_->has_parameter(
"angular_acc_tolerance"));
89 ASSERT_TRUE(
node_->has_parameter(
"rot_axis_norm_tolerance"));
91 ASSERT_TRUE(
node_->has_parameter(
"acceleration_tolerance"));
93 ASSERT_TRUE(
node_->has_parameter(
"other_tolerance"));
101 ASSERT_NE(
nullptr,
tdp_) <<
"Failed to load test data by provider.";
112 cartesian_limits::Params cartesian_limit;
113 cartesian_limit.max_trans_vel = 1.0 * M_PI;
114 cartesian_limit.max_trans_acc = 1.0 * M_PI;
115 cartesian_limit.max_trans_dec = 1.0 * M_PI;
116 cartesian_limit.max_rot_vel = 1.0 * M_PI;
123 ASSERT_NE(
nullptr,
circ_) <<
"failed to create CIRC trajectory generator";
129 moveit_msgs::msg::MotionPlanResponse res_msg;
137 EXPECT_EQ(req.path_constraints.position_constraints.size(), 1u);
138 EXPECT_EQ(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.size(), 1u);
141 Eigen::Vector3d circ_center;
144 for (std::size_t i = 0; i < res.
trajectory->getWayPointCount(); ++i)
148 (res.
trajectory->getFirstWayPointPtr()->getFrameTransform(
target_link_).translation() - circ_center).norm(),
157 for (
size_t idx = 0; idx < res.
trajectory->getLastWayPointPtr()->getVariableCount(); ++idx)
172 if (req.path_constraints.name ==
"center")
174 tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
177 else if (req.path_constraints.name ==
"interim")
179 Eigen::Vector3d interim;
180 tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
182 Eigen::Vector3d start = res.
trajectory->getFirstWayPointPtr()->getFrameTransform(
target_link_).translation();
183 Eigen::Vector3d goal = res.
trajectory->getLastWayPointPtr()->getFrameTransform(
target_link_).translation();
185 const Eigen::Vector3d t = interim - start;
186 const Eigen::Vector3d u = goal - start;
187 const Eigen::Vector3d v = goal - interim;
189 const Eigen::Vector3d w = t.cross(u);
191 ASSERT_GT(w.norm(), 1e-8) <<
"Circle center not well defined for given start, interim and goal.";
193 circ_center = start + (u * t.dot(t) * u.dot(v) - t * u.dot(u) * t.dot(v)) * 0.5 / pow(w.norm(), 2);
201 std::unique_ptr<robot_model_loader::RobotModelLoader>
rm_loader_;
204 std::unique_ptr<TrajectoryGeneratorCIRC>
circ_;
206 std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader>
tdp_;
223 auto cnp_ex = std::make_shared<CircleNoPlane>(
"");
224 EXPECT_EQ(cnp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
228 auto cts_ex = std::make_shared<CircleToSmall>(
"");
229 EXPECT_EQ(cts_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
233 auto cpdr_ex = std::make_shared<CenterPointDifferentRadius>(
"");
234 EXPECT_EQ(cpdr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
238 auto ctcf_ex = std::make_shared<CircTrajectoryConversionFailure>(
"");
239 EXPECT_EQ(ctcf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
243 auto upcn_ex = std::make_shared<UnknownPathConstraintName>(
"");
244 EXPECT_EQ(upcn_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
248 auto npc_ex = std::make_shared<NoPositionConstraints>(
"");
249 EXPECT_EQ(npc_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
253 auto npp_ex = std::make_shared<NoPrimitivePose>(
"");
254 EXPECT_EQ(npp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
258 auto ulnoap_ex = std::make_shared<UnknownLinkNameOfAuxiliaryPoint>(
"");
259 EXPECT_EQ(ulnoap_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME);
263 auto nocm_ex = std::make_shared<NumberOfConstraintsMismatch>(
"");
264 EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
268 auto cjmiss_ex = std::make_shared<CircJointMissingInStartState>(
"");
269 EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
273 auto cifgi_ex = std::make_shared<CircInverseForGoalIncalculable>(
"");
274 EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
284 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
287 EXPECT_GT(req.start_state.joint_state.name.size(), 1u);
288 req.start_state.joint_state.name.resize(1);
289 req.start_state.joint_state.position.resize(1);
292 circ_->generate(planning_scene_, req, res);
293 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
301 moveit_msgs::msg::MotionPlanRequest req{ tdp_->getCircJointCenterCart(
"circ1_center_2").toRequest() };
304 req.start_state.joint_state.velocity.push_back(1.0);
306 circ_->generate(planning_scene_, req, res);
307 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
308 req.start_state.joint_state.velocity.clear();
313 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
316 circ_->generate(planning_scene_, circ.toRequest(), res);
317 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
325 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
327 circ.setVelocityScale(1.0);
329 circ_->generate(planning_scene_, circ.toRequest(), res);
330 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED);
338 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
340 circ.setAccelerationScale(1.0);
342 circ_->generate(planning_scene_, circ.toRequest(), res);
343 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED);
353 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
354 circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
355 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 1e-8;
356 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1e-8;
357 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.z += 1e-8;
358 circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
359 circ.getGoalConfiguration().getPose().position.x -= 1e-8;
360 circ.getGoalConfiguration().getPose().position.y -= 1e-8;
361 circ.getGoalConfiguration().getPose().position.z -= 1e-8;
364 circ_->generate(planning_scene_, circ.toRequest(), res);
365 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
376 auto circ{ tdp_->getCircCartInterimCart(
"circ3_interim") };
377 circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
378 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 1e-8;
379 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1e-8;
380 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.z += 1e-8;
381 circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
382 circ.getGoalConfiguration().getPose().position.x -= 1e-8;
383 circ.getGoalConfiguration().getPose().position.y -= 1e-8;
384 circ.getGoalConfiguration().getPose().position.z -= 1e-8;
387 circ_->generate(planning_scene_, circ.toRequest(), res);
388 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
396 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
400 req.path_constraints.position_constraints.clear();
403 circ_->generate(planning_scene_, req, res);
404 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
412 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
416 req.path_constraints.name =
"";
419 circ_->generate(planning_scene_, req, res);
420 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
429 auto circ{ tdp_->getCircJointInterimCart(
"circ3_interim") };
433 req.path_constraints.position_constraints.front().link_name =
"INVALID";
436 circ_->generate(planning_scene_, req, res);
437 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME);
445 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
446 circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
447 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1;
450 circ_->generate(planning_scene_, circ.toRequest(), res);
451 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
461 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
462 circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
463 circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
466 circ.getStartConfiguration().getPose().position.x -= 0.1;
467 circ.getGoalConfiguration().getPose().position.x += 0.1;
470 circ_->generate(planning_scene_, circ.toRequest(), res);
471 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
482 auto circ{ tdp_->getCircCartInterimCart(
"circ3_interim") };
483 circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
484 circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
487 circ.getStartConfiguration().getPose().position.x -= 0.1;
488 circ.getGoalConfiguration().getPose().position.x += 0.1;
491 circ_->generate(planning_scene_, circ.toRequest(), res);
492 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
506 auto circ{ tdp_->getCircCartInterimCart(
"circ3_interim") };
509 circ_->generate(planning_scene_, circ.toRequest(), res);
510 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
525 auto circ{ tdp_->getCircCartInterimCart(
"circ3_interim") };
529 circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
530 circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
532 circ.getStartConfiguration().getPose().position.x -= 0.2;
533 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 0.2;
534 circ.getGoalConfiguration().getPose().position.y -= 0.2;
536 circ.setAccelerationScale(0.05);
537 circ.setVelocityScale(0.05);
539 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
542 circ_->generate(planning_scene_, req, res);
543 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
544 checkCircResult(req, res);
558 auto circ{ tdp_->getCircCartInterimCart(
"circ3_interim") };
562 circ.getAuxiliaryConfiguration().getConfiguration().setPose(circ.getStartConfiguration().getPose());
563 circ.getGoalConfiguration().setPose(circ.getStartConfiguration().getPose());
565 circ.getStartConfiguration().getPose().position.x -= 0.2;
566 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.x += 0.14142136;
567 circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y -= 0.14142136;
568 circ.getGoalConfiguration().getPose().position.y -= 0.2;
570 circ.setAccelerationScale(0.05);
571 circ.setVelocityScale(0.05);
573 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
576 circ_->generate(planning_scene_, req, res);
577 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
578 checkCircResult(req, res);
586 auto circ{ tdp_->getCircJointCenterCart(
"circ1_center_2") };
587 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
590 circ_->generate(planning_scene_, req, res);
591 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
592 checkCircResult(req, res);
602 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
604 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
607 ASSERT_EQ(req.path_constraints.position_constraints.back().constraint_region.primitive_poses.size(), 1u);
610 geometry_msgs::msg::Pose center_position;
611 center_position.position.x = 0.0;
612 center_position.position.y = 0.0;
613 center_position.position.z = 0.65;
614 req.path_constraints.position_constraints.back().constraint_region.primitive_poses.push_back(center_position);
617 circ_->generate(planning_scene_, req, res);
618 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
629 auto circ{ tdp_->getCircJointCenterCart(
"circ1_center_2") };
631 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
634 moveit_msgs::msg::JointConstraint joint_constraint;
635 joint_constraint.joint_name = req.goal_constraints.front().joint_constraints.front().joint_name;
636 req.goal_constraints.front().joint_constraints.push_back(joint_constraint);
639 circ_->generate(planning_scene_, req, res);
640 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
648 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
650 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
653 circ_->generate(planning_scene_, req, res);
654 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
655 checkCircResult(req, res);
663 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
665 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
667 req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame();
670 circ_->generate(planning_scene_, req, res);
671 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
672 checkCircResult(req, res);
680 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
682 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
683 req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame();
686 circ_->generate(planning_scene_, req, res);
687 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
688 checkCircResult(req, res);
696 auto circ{ tdp_->getCircCartCenterCart(
"circ1_center_2") };
698 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
701 req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame();
702 req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame();
705 circ_->generate(planning_scene_, req, res);
706 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
707 checkCircResult(req, res);
715 auto circ{ tdp_->getCircJointInterimCart(
"circ3_interim") };
717 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
720 circ_->generate(planning_scene_, req, res);
721 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
722 checkCircResult(req, res);
733 auto circ{ tdp_->getCircJointInterimCart(
"circ3_interim") };
735 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
738 req.start_state.joint_state.velocity = std::vector<double>(req.start_state.joint_state.position.size(), 1e-16);
741 circ_->generate(planning_scene_, req, res);
742 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
743 checkCircResult(req, res);
751 auto circ{ tdp_->getCircJointInterimCart(
"circ3_interim") };
752 moveit_msgs::msg::MotionPlanRequest req = circ.toRequest();
755 circ_->generate(planning_scene_, req, res);
756 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
757 checkCircResult(req, res);
762 rclcpp::init(argc, argv);
763 testing::InitGoogleTest(&argc, argv);
764 return RUN_ALL_TESTS();
double rot_axis_norm_tolerance_
void SetUp() override
Create test scenario for circ trajectory generator.
double cartesian_position_tolerance_
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)
double acceleration_tolerance_
LimitsContainer planner_limits_
moveit::core::RobotModelConstPtr robot_model_
double angular_acc_tolerance_
void checkCircResult(const planning_interface::MotionPlanRequest &req, const planning_interface::MotionPlanResponse &res)
std::string planning_group_
planning_scene::PlanningSceneConstPtr planning_scene_
std::unique_ptr< robot_model_loader::RobotModelLoader > rm_loader_
std::string test_data_file_name_
rclcpp::Node::SharedPtr node_
static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr &node, const std::string ¶m_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.
void setCartesianLimits(cartesian_limits::Params &cartesian_limit)
Set cartesian limits.
void setJointLimits(JointLimitsContainer &joint_limits)
Set joint limits.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
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 verified.
::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
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.
Response to a planning query.
moveit::core::MoveItErrorCode error_code
robot_trajectory::RobotTrajectoryPtr trajectory
void getMessage(moveit_msgs::msg::MotionPlanResponse &msg) const
Construct a ROS message from struct data.
int main(int argc, char **argv)