moveit2
The MoveIt Motion Planning Framework for ROS 2.
unittest_trajectory_generator_ptp.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 
39 #include <pilz_industrial_motion_planner/joint_limits_aggregator.h>
40 #include <pilz_industrial_motion_planner/trajectory_generator_ptp.h>
41 #include "test_utils.h"
42 
43 #include <moveit/kinematic_constraints/utils.h>
44 #include <moveit/robot_model/robot_model.h>
45 #include <moveit/robot_model_loader/robot_model_loader.h>
46 #include <pluginlib/class_loader.hpp>
47 
48 #include <rclcpp/rclcpp.hpp>
49 
50 // parameters from parameter server
51 const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
52 const std::string PARAM_TARGET_LINK_NAME("target_link");
53 const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance");
54 const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance");
55 const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance");
56 const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance");
57 
58 using namespace pilz_industrial_motion_planner;
59 
60 class TrajectoryGeneratorPTPTest : public testing::Test
61 {
62 protected:
67  void SetUp() override
68  {
69  rclcpp::NodeOptions node_options;
70  node_options.automatically_declare_parameters_from_overrides(true);
71  node_ = rclcpp::Node::make_shared("unittest_trajectory_generator_ptp", node_options);
72 
73  // load robot model
74  rm_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(node_);
75  robot_model_ = rm_loader_->getModel();
76  ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
77  planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
78 
79  // get parameters from parameter server
80  ASSERT_TRUE(node_->has_parameter("planning_group"));
81  node_->get_parameter<std::string>("planning_group", planning_group_);
82  ASSERT_TRUE(node_->has_parameter("target_link"));
83  node_->get_parameter<std::string>("target_link", target_link_);
84  ASSERT_TRUE(node_->has_parameter("joint_position_tolerance"));
85  node_->get_parameter<double>("joint_position_tolerance", joint_position_tolerance_);
86  ASSERT_TRUE(node_->has_parameter("joint_velocity_tolerance"));
87  node_->get_parameter<double>("joint_velocity_tolerance", joint_velocity_tolerance_);
88  ASSERT_TRUE(node_->has_parameter("joint_acceleration_tolerance"));
89  node_->get_parameter<double>("joint_acceleration_tolerance", joint_acceleration_tolerance_);
90  ASSERT_TRUE(node_->has_parameter("pose_norm_tolerance"));
91  node_->get_parameter<double>("pose_norm_tolerance", pose_norm_tolerance_);
92 
93  testutils::checkRobotModel(robot_model_, planning_group_, target_link_);
94 
95  // create the limits container
96  JointLimitsContainer joint_limits;
97  for (const auto& jmg : robot_model_->getJointModelGroups())
98  {
99  std::vector<std::string> joint_names = jmg->getActiveJointModelNames();
100  pilz_industrial_motion_planner::JointLimit joint_limit;
101  joint_limit.max_position = 3.124;
102  joint_limit.min_position = -3.124;
103  joint_limit.has_velocity_limits = true;
104  joint_limit.max_velocity = 1;
105  joint_limit.has_acceleration_limits = true;
106  joint_limit.max_acceleration = 0.5;
107  joint_limit.has_deceleration_limits = true;
108  joint_limit.max_deceleration = -1;
109  for (const auto& joint_name : joint_names)
110  {
111  joint_limits.addLimit(joint_name, joint_limit);
112  }
113  }
114 
115  // create the trajectory generator
116  planner_limits_.setJointLimits(joint_limits);
117  ptp_ = std::make_unique<TrajectoryGeneratorPTP>(robot_model_, planner_limits_, planning_group_);
118  ASSERT_NE(nullptr, ptp_);
119  }
120 
121  void TearDown() override
122  {
123  robot_model_.reset();
124  }
125 
133  bool checkTrajectory(const trajectory_msgs::msg::JointTrajectory& trajectory,
134  const planning_interface::MotionPlanRequest& req, const JointLimitsContainer& joint_limits)
135  {
136  return (testutils::isTrajectoryConsistent(trajectory) &&
137  testutils::isGoalReached(trajectory, req.goal_constraints.front().joint_constraints,
138  joint_position_tolerance_, joint_velocity_tolerance_) &&
139  testutils::isPositionBounded(trajectory, joint_limits) &&
140  testutils::isVelocityBounded(trajectory, joint_limits) &&
141  testutils::isAccelerationBounded(trajectory, joint_limits));
142  }
143 
144 protected:
145  // ros stuff
146  rclcpp::Node::SharedPtr node_;
147  moveit::core::RobotModelConstPtr robot_model_;
148  std::unique_ptr<robot_model_loader::RobotModelLoader> rm_loader_;
149  planning_scene::PlanningSceneConstPtr planning_scene_;
150 
151  // trajectory generator
152  std::unique_ptr<TrajectoryGenerator> ptp_;
153 
154  // test parameters from parameter server
155  LimitsContainer planner_limits_;
156  std::string planning_group_, target_link_;
157  double joint_position_tolerance_, joint_velocity_tolerance_, joint_acceleration_tolerance_, pose_norm_tolerance_;
158 };
159 
164 TEST_F(TrajectoryGeneratorPTPTest, TestExceptionErrorCodeMapping)
165 {
166  {
167  auto pvpsf_ex = std::make_shared<PtpVelocityProfileSyncFailed>("");
168  EXPECT_EQ(pvpsf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE);
169  }
170 
171  {
172  auto pnisfgp_ex = std::make_shared<PtpNoIkSolutionForGoalPose>("");
173  EXPECT_EQ(pnisfgp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
174  }
175 }
176 
180 TEST_F(TrajectoryGeneratorPTPTest, noLimits)
181 {
182  LimitsContainer planner_limits;
183  EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_),
184  TrajectoryGeneratorInvalidLimitsException);
185 }
186 
197 TEST_F(TrajectoryGeneratorPTPTest, emptyRequest)
198 {
199  planning_interface::MotionPlanResponse res;
200  planning_interface::MotionPlanRequest req;
201 
202  robot_trajectory::RobotTrajectoryPtr trajectory(
203  new robot_trajectory::RobotTrajectory(this->robot_model_, planning_group_));
204  moveit::core::RobotState state(this->robot_model_);
205  trajectory->addPrefixWayPoint(state, 0);
206  res.trajectory = trajectory;
207 
208  EXPECT_FALSE(res.trajectory->empty());
209 
210  ptp_->generate(planning_scene_, req, res);
211 
212  EXPECT_FALSE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
213  EXPECT_TRUE(res.trajectory->empty());
214 }
215 
219 TEST_F(TrajectoryGeneratorPTPTest, missingVelocityLimits)
220 {
221  LimitsContainer planner_limits;
222 
223  JointLimitsContainer joint_limits;
224  auto joint_models = robot_model_->getActiveJointModels();
225  pilz_industrial_motion_planner::JointLimit joint_limit;
226  joint_limit.has_velocity_limits = false;
227  joint_limit.has_acceleration_limits = true;
228  joint_limit.max_deceleration = -1;
229  joint_limit.has_deceleration_limits = true;
230  for (const auto& joint_model : joint_models)
231  {
232  ASSERT_TRUE(joint_limits.addLimit(joint_model->getName(), joint_limit))
233  << "Failed to add the limits for joint " << joint_model->getName();
234  }
235 
236  planner_limits.setJointLimits(joint_limits);
237  EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_),
238  TrajectoryGeneratorInvalidLimitsException);
239 }
240 
244 TEST_F(TrajectoryGeneratorPTPTest, missingDecelerationimits)
245 {
246  LimitsContainer planner_limits;
247 
248  JointLimitsContainer joint_limits;
249  const auto& joint_models = robot_model_->getActiveJointModels();
250  pilz_industrial_motion_planner::JointLimit joint_limit;
251  joint_limit.has_velocity_limits = true;
252  joint_limit.has_acceleration_limits = true;
253  joint_limit.has_deceleration_limits = false;
254  for (const auto& joint_model : joint_models)
255  {
256  ASSERT_TRUE(joint_limits.addLimit(joint_model->getName(), joint_limit))
257  << "Failed to add the limits for joint " << joint_model->getName();
258  }
259 
260  planner_limits.setJointLimits(joint_limits);
261  EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_),
262  TrajectoryGeneratorInvalidLimitsException);
263 }
264 
276 TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit)
277 {
278  /**********/
279  /* Step 1 */
280  /**********/
281  const auto& joint_models = robot_model_->getActiveJointModels();
282  ASSERT_TRUE(joint_models.size());
283 
284  // joint limit with insufficient limits (no acc/dec limits)
285  pilz_industrial_motion_planner::JointLimit insufficient_limit;
286  insufficient_limit.has_position_limits = true;
287  insufficient_limit.max_position = 2.5;
288  insufficient_limit.min_position = -2.5;
289  insufficient_limit.has_velocity_limits = true;
290  insufficient_limit.max_velocity = 1.256;
291  insufficient_limit.has_acceleration_limits = false;
292  insufficient_limit.has_deceleration_limits = false;
293  JointLimitsContainer insufficient_joint_limits;
294  for (const auto& joint_model : joint_models)
295  {
296  ASSERT_TRUE(insufficient_joint_limits.addLimit(joint_model->getName(), insufficient_limit))
297  << "Failed to add the limits for joint " << joint_model->getName();
298  }
299  LimitsContainer insufficient_planner_limits;
300  insufficient_planner_limits.setJointLimits(insufficient_joint_limits);
301 
302  EXPECT_THROW(
303  {
304  auto ptp_error =
305  std::make_unique<TrajectoryGeneratorPTP>(robot_model_, insufficient_planner_limits, planning_group_);
306  },
307  TrajectoryGeneratorInvalidLimitsException);
308 
309  /**********/
310  /* Step 2 */
311  /**********/
312  // joint limit with sufficient limits
313  pilz_industrial_motion_planner::JointLimit sufficient_limit;
314  sufficient_limit.has_position_limits = true;
315  sufficient_limit.max_position = 2.356;
316  sufficient_limit.min_position = -2.356;
317  sufficient_limit.has_velocity_limits = true;
318  sufficient_limit.max_velocity = 1;
319  sufficient_limit.has_acceleration_limits = true;
320  sufficient_limit.max_acceleration = 0.5;
321  sufficient_limit.has_deceleration_limits = true;
322  sufficient_limit.max_deceleration = -1;
323  JointLimitsContainer sufficient_joint_limits;
324  // fill joint limits container, such that it contains one sufficient limit and
325  // all others are insufficient
326  for (const auto& jmg : robot_model_->getJointModelGroups())
327  {
328  const auto& joint_names{ jmg->getActiveJointModelNames() };
329  ASSERT_FALSE(joint_names.empty());
330  ASSERT_TRUE(sufficient_joint_limits.addLimit(joint_names.front(), sufficient_limit))
331  << "Failed to add the limits for joint " << joint_names.front();
332 
333  for (auto it = std::next(joint_names.begin()); it != joint_names.end(); ++it)
334  {
335  ASSERT_TRUE(sufficient_joint_limits.addLimit((*it), insufficient_limit))
336  << "Failed to add the limits for joint " << (*it);
337  }
338  }
339  LimitsContainer sufficient_planner_limits;
340  sufficient_planner_limits.setJointLimits(sufficient_joint_limits);
341 
342  EXPECT_NO_THROW({
343  auto ptp_no_error =
344  std::make_unique<TrajectoryGeneratorPTP>(robot_model_, sufficient_planner_limits, planning_group_);
345  });
346 }
347 
351 TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoal)
352 {
353  //***************************************
354  //*** prepare the motion plan request ***
355  //***************************************
356  planning_interface::MotionPlanResponse res;
357  planning_interface::MotionPlanRequest req;
358  testutils::createDummyRequest(robot_model_, planning_group_, req);
359 
360  // cartesian goal pose
361  geometry_msgs::msg::PoseStamped pose;
362  pose.pose.position.x = 0.1;
363  pose.pose.position.y = 0.2;
364  pose.pose.position.z = 0.65;
365  pose.pose.orientation.w = 1.0;
366  pose.pose.orientation.x = 0.0;
367  pose.pose.orientation.y = 0.0;
368  pose.pose.orientation.z = 0.0;
369  std::vector<double> tolerance_pose(3, 0.01);
370  std::vector<double> tolerance_angle(3, 0.01);
371  moveit_msgs::msg::Constraints pose_goal =
372  kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle);
373  req.goal_constraints.push_back(pose_goal);
374 
375  //****************************************
376  //*** test robot model without gripper ***
377  //****************************************
378  ptp_->generate(planning_scene_, req, res);
379  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
380 
381  moveit_msgs::msg::MotionPlanResponse res_msg;
382  res.getMessage(res_msg);
383  if (!res_msg.trajectory.joint_trajectory.points.empty())
384  {
385  EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));
386  }
387  else
388  {
389  FAIL() << "Received empty trajectory.";
390  }
391 
392  // check goal pose
393  EXPECT_TRUE(testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_));
394 }
395 
400 TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints)
401 {
402  //***************************************
403  //*** prepare the motion plan request ***
404  //***************************************
405  planning_interface::MotionPlanResponse res;
406  planning_interface::MotionPlanRequest req;
407  testutils::createDummyRequest(robot_model_, planning_group_, req);
408 
409  // cartesian goal pose
410  geometry_msgs::msg::PoseStamped pose;
411  pose.pose.position.x = 0.1;
412  pose.pose.position.y = 0.2;
413  pose.pose.position.z = 0.65;
414  pose.pose.orientation.w = 1.0;
415  pose.pose.orientation.x = 0.0;
416  pose.pose.orientation.y = 0.0;
417  pose.pose.orientation.z = 0.0;
418  std::vector<double> tolerance_pose(3, 0.01);
419  std::vector<double> tolerance_angle(3, 0.01);
420  moveit_msgs::msg::Constraints pose_goal =
421  kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle);
422  req.goal_constraints.push_back(pose_goal);
423 
424  planning_interface::MotionPlanRequest req_no_position_constaint_link_name = req;
425  req_no_position_constaint_link_name.goal_constraints.front().position_constraints.front().link_name = "";
426  ptp_->generate(planning_scene_, req_no_position_constaint_link_name, res);
427  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
428 
429  planning_interface::MotionPlanRequest req_no_orientation_constaint_link_name = req;
430  req_no_orientation_constaint_link_name.goal_constraints.front().orientation_constraints.front().link_name = "";
431  ptp_->generate(planning_scene_, req_no_orientation_constaint_link_name, res);
432  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
433 }
434 
438 TEST_F(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal)
439 {
440  planning_interface::MotionPlanResponse res;
441  planning_interface::MotionPlanRequest req;
442  testutils::createDummyRequest(robot_model_, planning_group_, req);
443 
444  geometry_msgs::msg::PoseStamped pose;
445  pose.pose.position.x = 0.1;
446  pose.pose.position.y = 0.2;
447  pose.pose.position.z = 2.5;
448  pose.pose.orientation.w = 1.0;
449  pose.pose.orientation.x = 0.0;
450  pose.pose.orientation.y = 0.0;
451  pose.pose.orientation.z = 0.0;
452  std::vector<double> tolerance_pose(3, 0.01);
453  std::vector<double> tolerance_angle(3, 0.01);
454  moveit_msgs::msg::Constraints pose_goal =
455  kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle);
456  req.goal_constraints.push_back(pose_goal);
457 
458  ptp_->generate(planning_scene_, req, res);
459  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
460  EXPECT_EQ(res.trajectory, nullptr);
461 }
462 
468 TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached)
469 {
470  planning_interface::MotionPlanResponse res;
471  planning_interface::MotionPlanRequest req;
472  testutils::createDummyRequest(robot_model_, planning_group_, req);
473  ASSERT_TRUE(robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size())
474  << "No link exists in the planning group.";
475 
476  moveit_msgs::msg::Constraints gc;
477  moveit_msgs::msg::JointConstraint jc;
478  jc.joint_name = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().front();
479  jc.position = 0.0;
480  gc.joint_constraints.push_back(jc);
481  req.goal_constraints.push_back(gc);
482 
483  // TODO lin and circ has different settings
484  ptp_->generate(planning_scene_, req, res);
485  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
486 
487  moveit_msgs::msg::MotionPlanResponse res_msg;
488  res.getMessage(res_msg);
489  EXPECT_EQ(1u, res_msg.trajectory.joint_trajectory.points.size());
490 }
491 
496 TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor)
497 {
498  // create ptp generator with different limits
499  pilz_industrial_motion_planner::JointLimit joint_limit;
500  JointLimitsContainer joint_limits;
501 
502  // set the joint limits
503  joint_limit.has_position_limits = true;
504  joint_limit.max_position = 2.967;
505  joint_limit.min_position = -2.967;
506  joint_limit.has_velocity_limits = true;
507  joint_limit.max_velocity = 2;
508  joint_limit.has_acceleration_limits = true;
509  joint_limit.max_acceleration = 1.5;
510  joint_limit.has_deceleration_limits = true;
511  joint_limit.max_deceleration = -3;
512  joint_limits.addLimit("prbt_joint_1", joint_limit);
513  joint_limit.max_position = 2.530;
514  joint_limit.min_position = -2.530;
515  joint_limits.addLimit("prbt_joint_2", joint_limit);
516  joint_limit.max_position = 2.356;
517  joint_limit.min_position = -2.356;
518  joint_limits.addLimit("prbt_joint_3", joint_limit);
519  joint_limit.max_position = 2.967;
520  joint_limit.min_position = -2.967;
521  joint_limits.addLimit("prbt_joint_4", joint_limit);
522  joint_limit.max_position = 2.967;
523  joint_limit.min_position = -2.967;
524  joint_limits.addLimit("prbt_joint_5", joint_limit);
525  joint_limit.max_position = 3.132;
526  joint_limit.min_position = -3.132;
527  joint_limits.addLimit("prbt_joint_6", joint_limit);
528  // add gripper limit such that generator does not complain about missing limit
529  joint_limits.addLimit("prbt_gripper_finger_left_joint", joint_limit);
530 
531  pilz_industrial_motion_planner::LimitsContainer planner_limits;
532  planner_limits.setJointLimits(joint_limits);
533 
534  // create the generator with new limits
535  ptp_ = std::make_unique<TrajectoryGeneratorPTP>(robot_model_, planner_limits, planning_group_);
536 
537  planning_interface::MotionPlanResponse res;
538  planning_interface::MotionPlanRequest req;
539  testutils::createDummyRequest(robot_model_, planning_group_, req);
540  req.start_state.joint_state.position[2] = 0.1;
541  moveit_msgs::msg::Constraints gc;
542  moveit_msgs::msg::JointConstraint jc;
543  jc.joint_name = "prbt_joint_1";
544  jc.position = 1.5;
545  gc.joint_constraints.push_back(jc);
546  jc.joint_name = "prbt_joint_3";
547  jc.position = 2.1;
548  gc.joint_constraints.push_back(jc);
549  jc.joint_name = "prbt_joint_6";
550  jc.position = 3.0;
551  gc.joint_constraints.push_back(jc);
552  req.goal_constraints.push_back(gc);
553  req.max_velocity_scaling_factor = 0.5;
554  req.max_acceleration_scaling_factor = 1.0 / 3.0;
555 
556  ptp_->generate(planning_scene_, req, res);
557  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
558 
559  moveit_msgs::msg::MotionPlanResponse res_msg;
560  res.getMessage(res_msg);
561  EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));
562 
563  // trajectory duration
564  EXPECT_NEAR(4.5, res.trajectory->getWayPointDurationFromStart(res.trajectory->getWayPointCount()),
565  joint_acceleration_tolerance_);
566 
567  // way point at 1s
568  int index;
569  index = testutils::getWayPointIndex(res.trajectory, 1.0);
570  // joint_1
571  EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
572  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
573  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
574  // joint_3
575  EXPECT_NEAR(1.0 / 6.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
576  joint_position_tolerance_);
577  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
578  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
579  joint_acceleration_tolerance_);
580  // joint_6
581  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
582  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
583  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
584  // other joints
585  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[4], joint_position_tolerance_);
586  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[4], joint_velocity_tolerance_);
587  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_);
588 
589  // way point at 2s
590  index = testutils::getWayPointIndex(res.trajectory, 2.0);
591  // joint_1
592  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
593  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
594  // joint_3
595  EXPECT_NEAR(2.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
596  joint_position_tolerance_);
597  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
598  // joint_6
599  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
600  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
601  // other joints
602  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
603  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
604  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
605 
606  // way point at 3s
607  index = testutils::getWayPointIndex(res.trajectory, 3.0);
608  // joint_1
609  EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
610  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
611  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
612  // joint_3
613  EXPECT_NEAR(4.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
614  joint_position_tolerance_);
615  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
616  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_);
617  // joint_6
618  EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
619  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
620  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
621  // other joints
622  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
623  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
624  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_);
625 
626  // way point at 4s
627  index = testutils::getWayPointIndex(res.trajectory, 4.0);
628  // joint_1
629  EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
630  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
631  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
632  // joint_3
633  EXPECT_NEAR(5.75 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
634  joint_position_tolerance_);
635  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
636  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
637  joint_acceleration_tolerance_);
638  // joint_6
639  EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
640  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
641  EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
642 
643  // way point at 4.5s
644  index = testutils::getWayPointIndex(res.trajectory, 4.5);
645  // joint_1
646  EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
647  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
648  // joint_3
649  EXPECT_NEAR(2.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_);
650  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
651  // joint_6
652  EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
653  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
654 }
655 
660 TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity)
661 {
662  planning_interface::MotionPlanResponse res;
663  planning_interface::MotionPlanRequest req;
664  testutils::createDummyRequest(robot_model_, planning_group_, req);
665  req.start_state.joint_state.position[2] = 0.1;
666 
667  // Set velocity to all 1e-16
668  req.start_state.joint_state.velocity = std::vector<double>(req.start_state.joint_state.position.size(), 1e-16);
669 
670  moveit_msgs::msg::Constraints gc;
671  moveit_msgs::msg::JointConstraint jc;
672  jc.joint_name = "prbt_joint_1";
673  jc.position = 1.5;
674  gc.joint_constraints.push_back(jc);
675  jc.joint_name = "prbt_joint_3";
676  jc.position = 2.1;
677  gc.joint_constraints.push_back(jc);
678  jc.joint_name = "prbt_joint_6";
679  jc.position = 3.0;
680  gc.joint_constraints.push_back(jc);
681  req.goal_constraints.push_back(gc);
682 
683  ptp_->generate(planning_scene_, req, res);
684  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
685 
686  moveit_msgs::msg::MotionPlanResponse res_msg;
687  res.getMessage(res_msg);
688  EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));
689 
690  // trajectory duration
691  EXPECT_NEAR(4.5, res.trajectory->getWayPointDurationFromStart(res.trajectory->getWayPointCount()),
692  joint_acceleration_tolerance_);
693 
694  // way point at 1s
695  int index;
696  index = testutils::getWayPointIndex(res.trajectory, 1.0);
697  // joint_1
698  EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
699  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
700  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
701  // joint_3
702  EXPECT_NEAR(1.0 / 6.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
703  joint_position_tolerance_);
704  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
705  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
706  joint_acceleration_tolerance_);
707  // joint_6
708  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
709  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
710  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
711  // other joints
712  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[4], joint_position_tolerance_);
713  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[4], joint_velocity_tolerance_);
714  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_);
715 
716  // way point at 2s
717  index = testutils::getWayPointIndex(res.trajectory, 2.0);
718  // joint_1
719  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
720  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
721  // joint_3
722  EXPECT_NEAR(2.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
723  joint_position_tolerance_);
724  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
725  // joint_6
726  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
727  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
728  // other joints
729  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
730  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
731  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
732 
733  // way point at 3s
734  index = testutils::getWayPointIndex(res.trajectory, 3.0);
735  // joint_1
736  EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
737  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
738  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
739  // joint_3
740  EXPECT_NEAR(4.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
741  joint_position_tolerance_);
742  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
743  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_);
744  // joint_6
745  EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
746  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
747  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
748  // other joints
749  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
750  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
751  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_);
752 
753  // way point at 4s
754  index = testutils::getWayPointIndex(res.trajectory, 4.0);
755  // joint_1
756  EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
757  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
758  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
759  // joint_3
760  EXPECT_NEAR(5.75 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
761  joint_position_tolerance_);
762  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
763  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
764  joint_acceleration_tolerance_);
765  // joint_6
766  EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
767  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
768  EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
769 
770  // way point at 4.5s
771  index = testutils::getWayPointIndex(res.trajectory, 4.5);
772  // joint_1
773  EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
774  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
775  // joint_3
776  EXPECT_NEAR(2.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_);
777  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
778  // joint_6
779  EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
780  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
781 
782  // Check that velocity at the end is all zero
783  EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().velocities.cbegin(),
784  res_msg.trajectory.joint_trajectory.points.back().velocities.cend(),
785  [this](double v) { return std::fabs(v) < this->joint_velocity_tolerance_; }));
786 
787  // Check that acceleration at the end is all zero
788  EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().accelerations.cbegin(),
789  res_msg.trajectory.joint_trajectory.points.back().accelerations.cend(),
790  [this](double v) { return std::fabs(v) < this->joint_acceleration_tolerance_; }));
791 }
792 
797 TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel)
798 {
799  planning_interface::MotionPlanResponse res;
800  planning_interface::MotionPlanRequest req;
801  testutils::createDummyRequest(robot_model_, planning_group_, req);
802  req.start_state.joint_state.position[4] = 0.3;
803  req.start_state.joint_state.position[2] = 0.11;
804 
805  moveit_msgs::msg::Constraints gc;
806  moveit_msgs::msg::JointConstraint jc;
807 
808  jc.joint_name = "prbt_joint_1";
809  jc.position = 1.5;
810  gc.joint_constraints.push_back(jc);
811  jc.joint_name = "prbt_joint_2";
812  jc.position = -1.5;
813  gc.joint_constraints.push_back(jc);
814  jc.joint_name = "prbt_joint_3";
815  jc.position = 2.11;
816  gc.joint_constraints.push_back(jc);
817  jc.joint_name = "prbt_joint_4";
818  jc.position = -2.0;
819  gc.joint_constraints.push_back(jc);
820  jc.joint_name = "prbt_joint_6";
821  jc.position = 3.0;
822  gc.joint_constraints.push_back(jc);
823  req.goal_constraints.push_back(gc);
824 
825  ptp_->generate(planning_scene_, req, res);
826  EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
827 
828  moveit_msgs::msg::MotionPlanResponse res_msg;
829  res.getMessage(res_msg);
830  EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));
831 
832  // trajectory duration
833  EXPECT_NEAR(4.5, res.trajectory->getWayPointDurationFromStart(res.trajectory->getWayPointCount()),
834  joint_position_tolerance_);
835 
836  // way point at 0s
837  // joint_1
838  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[0], joint_position_tolerance_);
839  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[0], joint_velocity_tolerance_);
840  // joint_2
841  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[1], joint_position_tolerance_);
842  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[1], joint_velocity_tolerance_);
843  // joint_3
844  EXPECT_NEAR(0.11, res_msg.trajectory.joint_trajectory.points[0].positions[2], joint_position_tolerance_);
845  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[2], joint_velocity_tolerance_);
846  // joint_4
847  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[3], joint_position_tolerance_);
848  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[3], joint_velocity_tolerance_);
849  // joint_6
850  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[5], joint_position_tolerance_);
851  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[5], joint_velocity_tolerance_);
852 
853  // way point at 1s
854  int index;
855  index = testutils::getWayPointIndex(res.trajectory, 1.0);
856  // joint_1
857  EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
858  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
859  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
860  // joint_2
861  EXPECT_NEAR(-0.125, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
862  EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
863  EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
864  // joint_3
865  EXPECT_NEAR(1.0 / 6.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2],
866  joint_position_tolerance_);
867  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
868  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
869  joint_acceleration_tolerance_);
870  // joint_4
871  EXPECT_NEAR(-1.0 / 6.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
872  EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
873  EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3],
874  joint_acceleration_tolerance_);
875  // joint_6
876  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
877  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
878  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
879 
880  // way point at 2s
881  index = testutils::getWayPointIndex(res.trajectory, 2.0);
882  // joint_1
883  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
884  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
885  // joint_2
886  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
887  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
888  // joint_3
889  EXPECT_NEAR(2.0 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2],
890  joint_position_tolerance_);
891  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
892  // joint_4
893  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
894  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
895  // joint_6
896  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
897  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
898 
899  // way point at 3s
900  index = testutils::getWayPointIndex(res.trajectory, 3.0);
901  // joint_1
902  EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
903  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
904  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
905  // joint_2
906  EXPECT_NEAR(-1, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
907  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
908  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
909  // joint_3
910  EXPECT_NEAR(4.0 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2],
911  joint_position_tolerance_);
912  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
913  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_);
914  // joint_4
915  EXPECT_NEAR(-4.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
916  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
917  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_);
918  // joint_6
919  EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
920  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
921  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
922 
923  // way point at 4s
924  index = testutils::getWayPointIndex(res.trajectory, 4.0);
925  // joint_1
926  EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
927  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
928  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
929  // joint_2
930  EXPECT_NEAR(-2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
931  EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
932  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
933  // joint_3
934  EXPECT_NEAR(5.75 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2],
935  joint_position_tolerance_);
936  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
937  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
938  joint_acceleration_tolerance_);
939  // joint_4
940  EXPECT_NEAR(-5.75 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
941  EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
942  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3],
943  joint_acceleration_tolerance_);
944  // joint_6
945  EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
946  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
947  EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
948 
949  // way point at 4.5s
950  index = testutils::getWayPointIndex(res.trajectory, 4.5);
951  // joint_1
952  EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
953  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
954  // joint_2
955  EXPECT_NEAR(-1.5, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
956  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
957  // joint_3
958  EXPECT_NEAR(2.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_);
959  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
960  // joint_4
961  EXPECT_NEAR(-2.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
962  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
963  // joint_6
964  EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
965  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
966 
967  // Check last point
968  EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
969 
970  // Check that velocity at the end is all zero
971  EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().velocities.cbegin(),
972  res_msg.trajectory.joint_trajectory.points.back().velocities.cend(),
973  [this](double v) { return std::fabs(v) < this->joint_velocity_tolerance_; }));
974 
975  // Check that acceleration at the end is all zero
976  EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().accelerations.cbegin(),
977  res_msg.trajectory.joint_trajectory.points.back().accelerations.cend(),
978  [this](double v) { return std::fabs(v) < this->joint_acceleration_tolerance_; }));
979 }
980 
981 int main(int argc, char** argv)
982 {
983  rclcpp::init(argc, argv);
984  testing::InitGoogleTest(&argc, argv);
985  return RUN_ALL_TESTS();
986 }
TrajectoryGeneratorPTPTest::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: unittest_trajectory_generator_ptp.cpp:147
TrajectoryGeneratorPTPTest::rm_loader_
std::unique_ptr< robot_model_loader::RobotModelLoader > rm_loader_
Definition: unittest_trajectory_generator_ptp.cpp:148
TrajectoryGeneratorPTPTest::checkTrajectory
bool checkTrajectory(const trajectory_msgs::msg::JointTrajectory &trajectory, const planning_interface::MotionPlanRequest &req, const JointLimitsContainer &joint_limits)
check the resulted joint trajectory
Definition: unittest_trajectory_generator_ptp.cpp:133
TrajectoryGeneratorPTPTest::planning_scene_
planning_scene::PlanningSceneConstPtr planning_scene_
Definition: unittest_trajectory_generator_ptp.cpp:149
TrajectoryGeneratorPTPTest::ptp_
std::unique_ptr< TrajectoryGenerator > ptp_
Definition: unittest_trajectory_generator_ptp.cpp:152
TrajectoryGeneratorPTPTest::SetUp
void SetUp() override
Create test fixture for ptp trajectory generator.
Definition: unittest_trajectory_generator_ptp.cpp:67
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
pilz_industrial_motion_planner::JointLimitsContainer
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
Definition: joint_limits_container.h:52
pilz_industrial_motion_planner::JointLimitsContainer::addLimit
bool addLimit(const std::string &joint_name, JointLimit joint_limit)
Add a limit.
Definition: joint_limits_container.cpp:52
pilz_industrial_motion_planner::LimitsContainer
This class combines CartesianLimit and JointLimits into on single class.
Definition: limits_container.h:49
pilz_industrial_motion_planner::LimitsContainer::setJointLimits
void setJointLimits(JointLimitsContainer &joint_limits)
Set joint limits.
Definition: limits_container.cpp:58
pilz_industrial_motion_planner::TrajectoryGeneratorPTP
This class implements a point-to-point trajectory generator based on VelocityProfileATrap.
Definition: trajectory_generator_ptp.h:56
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:60
joint_limits_aggregator.h
utils.h
robot_model.h
joint_limits
Definition: joint_limits.hpp:28
kinematic_constraints::constructGoalConstraints
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
Definition: utils.cpp:152
pilz_industrial_motion_planner
Definition: capability_names.h:40
pilz_industrial_motion_planner::TEST_F
TEST_F(GetSolverTipFrameIntegrationTest, TestHasSolverManipulator)
Check if hasSolver() can be called successfully for the manipulator group.
Definition: integrationtest_get_solver_tip_frame.cpp:76
planning_interface::MotionPlanRequest
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
Definition: planning_request.h:45
testutils::isGoalReached
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.
Definition: test_utils.cpp:118
testutils::isVelocityBounded
bool isVelocityBounded(const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
is Velocity Bounded
Definition: test_utils.cpp:353
testutils::createDummyRequest
void createDummyRequest(const moveit::core::RobotModelConstPtr &robot_model, const std::string &planning_group, planning_interface::MotionPlanRequest &req)
create a dummy motion plan request with zero start state No goal constraint is given.
Definition: test_utils.cpp:504
testutils::getWayPointIndex
int getWayPointIndex(const robot_trajectory::RobotTrajectoryPtr &trajectory, const double time_from_start)
get the waypoint index from time from start
Definition: test_utils.h:229
testutils::isAccelerationBounded
bool isAccelerationBounded(const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
is Acceleration Bounded
Definition: test_utils.cpp:393
testutils::isTrajectoryConsistent
bool isTrajectoryConsistent(const trajectory_msgs::msg::JointTrajectory &trajectory)
check if the sizes of the joint position/veloicty/acceleration are correct
Definition: test_utils.cpp:376
testutils::isPositionBounded
bool isPositionBounded(const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
is Position Bounded
Definition: test_utils.cpp:435
testutils::checkRobotModel
void checkRobotModel(const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name, const std::string &link_name)
Definition: test_utils.cpp:1260
robot_model_loader.h
joint_limits::JointLimits::max_velocity
double max_velocity
Definition: joint_limits.hpp:49
joint_limits::JointLimits::has_velocity_limits
bool has_velocity_limits
Definition: joint_limits.hpp:55
joint_limits::JointLimits::has_acceleration_limits
bool has_acceleration_limits
Definition: joint_limits.hpp:56
joint_limits::JointLimits::max_position
double max_position
Definition: joint_limits.hpp:48
joint_limits::JointLimits::max_acceleration
double max_acceleration
Definition: joint_limits.hpp:50
joint_limits::JointLimits::min_position
double min_position
Definition: joint_limits.hpp:47
joint_limits::JointLimits::has_position_limits
bool has_position_limits
Definition: joint_limits.hpp:54
pilz_industrial_motion_planner::joint_limits_interface::JointLimits
Extends joint_limits_interface::JointLimits with a deceleration parameter.
Definition: joint_limits_extension.h:53
pilz_industrial_motion_planner::joint_limits_interface::JointLimits::has_deceleration_limits
bool has_deceleration_limits
Definition: joint_limits_extension.h:61
pilz_industrial_motion_planner::joint_limits_interface::JointLimits::max_deceleration
double max_deceleration
maximum deceleration MUST(!) be negative
Definition: joint_limits_extension.h:59
planning_interface::MotionPlanResponse
Response to a planning query.
Definition: planning_response.h:49
planning_interface::MotionPlanResponse::error_code
moveit::core::MoveItErrorCode error_code
Definition: planning_response.h:63
planning_interface::MotionPlanResponse::trajectory
robot_trajectory::RobotTrajectoryPtr trajectory
Definition: planning_response.h:59
planning_interface::MotionPlanResponse::getMessage
void getMessage(moveit_msgs::msg::MotionPlanResponse &msg) const
Construct a ROS message from struct data.
Definition: planning_response.cpp:40
test_utils.h
trajectory_generator_ptp.h
PARAM_PLANNING_GROUP_NAME
const std::string PARAM_PLANNING_GROUP_NAME("planning_group")
JOINT_POSITION_TOLERANCE
const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance")
main
int main(int argc, char **argv)
Definition: unittest_trajectory_generator_ptp.cpp:981
PARAM_TARGET_LINK_NAME
const std::string PARAM_TARGET_LINK_NAME("target_link")
JOINT_ACCELERATION_TOLERANCE
const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance")
POSE_TRANSFORM_MATRIX_NORM_TOLERANCE
const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance")
JOINT_VELOCITY_TOLERANCE
const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance")
Generated by doxygen 1.9.1