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  std::shared_ptr<PtpVelocityProfileSyncFailed> pvpsf_ex{ new PtpVelocityProfileSyncFailed("") };
168  EXPECT_EQ(pvpsf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE);
169  }
170 
171  {
172  std::shared_ptr<PtpNoIkSolutionForGoalPose> pnisfgp_ex{ new 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  EXPECT_FALSE(ptp_->generate(planning_scene_, req, res));
211 
212  EXPECT_TRUE(res.trajectory_->empty());
213 }
214 
218 TEST_F(TrajectoryGeneratorPTPTest, missingVelocityLimits)
219 {
220  LimitsContainer planner_limits;
221 
222  JointLimitsContainer joint_limits;
223  auto joint_models = robot_model_->getActiveJointModels();
224  pilz_industrial_motion_planner::JointLimit joint_limit;
225  joint_limit.has_velocity_limits = false;
226  joint_limit.has_acceleration_limits = true;
227  joint_limit.max_deceleration = -1;
228  joint_limit.has_deceleration_limits = true;
229  for (const auto& joint_model : joint_models)
230  {
231  ASSERT_TRUE(joint_limits.addLimit(joint_model->getName(), joint_limit))
232  << "Failed to add the limits for joint " << joint_model->getName();
233  }
234 
235  planner_limits.setJointLimits(joint_limits);
236  EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_),
237  TrajectoryGeneratorInvalidLimitsException);
238 }
239 
243 TEST_F(TrajectoryGeneratorPTPTest, missingDecelerationimits)
244 {
245  LimitsContainer planner_limits;
246 
247  JointLimitsContainer joint_limits;
248  const auto& joint_models = robot_model_->getActiveJointModels();
249  pilz_industrial_motion_planner::JointLimit joint_limit;
250  joint_limit.has_velocity_limits = true;
251  joint_limit.has_acceleration_limits = true;
252  joint_limit.has_deceleration_limits = false;
253  for (const auto& joint_model : joint_models)
254  {
255  ASSERT_TRUE(joint_limits.addLimit(joint_model->getName(), joint_limit))
256  << "Failed to add the limits for joint " << joint_model->getName();
257  }
258 
259  planner_limits.setJointLimits(joint_limits);
260  EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_),
261  TrajectoryGeneratorInvalidLimitsException);
262 }
263 
275 TEST_F(TrajectoryGeneratorPTPTest, testInsufficientLimit)
276 {
277  /**********/
278  /* Step 1 */
279  /**********/
280  const auto& joint_models = robot_model_->getActiveJointModels();
281  ASSERT_TRUE(joint_models.size());
282 
283  // joint limit with insufficient limits (no acc/dec limits)
284  pilz_industrial_motion_planner::JointLimit insufficient_limit;
285  insufficient_limit.has_position_limits = true;
286  insufficient_limit.max_position = 2.5;
287  insufficient_limit.min_position = -2.5;
288  insufficient_limit.has_velocity_limits = true;
289  insufficient_limit.max_velocity = 1.256;
290  insufficient_limit.has_acceleration_limits = false;
291  insufficient_limit.has_deceleration_limits = false;
292  JointLimitsContainer insufficient_joint_limits;
293  for (const auto& joint_model : joint_models)
294  {
295  ASSERT_TRUE(insufficient_joint_limits.addLimit(joint_model->getName(), insufficient_limit))
296  << "Failed to add the limits for joint " << joint_model->getName();
297  }
298  LimitsContainer insufficient_planner_limits;
299  insufficient_planner_limits.setJointLimits(insufficient_joint_limits);
300 
301  EXPECT_THROW(
302  {
303  std::unique_ptr<TrajectoryGeneratorPTP> ptp_error(
304  new TrajectoryGeneratorPTP(robot_model_, insufficient_planner_limits, planning_group_));
305  },
306  TrajectoryGeneratorInvalidLimitsException);
307 
308  /**********/
309  /* Step 2 */
310  /**********/
311  // joint limit with sufficient limits
312  pilz_industrial_motion_planner::JointLimit sufficient_limit;
313  sufficient_limit.has_position_limits = true;
314  sufficient_limit.max_position = 2.356;
315  sufficient_limit.min_position = -2.356;
316  sufficient_limit.has_velocity_limits = true;
317  sufficient_limit.max_velocity = 1;
318  sufficient_limit.has_acceleration_limits = true;
319  sufficient_limit.max_acceleration = 0.5;
320  sufficient_limit.has_deceleration_limits = true;
321  sufficient_limit.max_deceleration = -1;
322  JointLimitsContainer sufficient_joint_limits;
323  // fill joint limits container, such that it contains one sufficient limit and
324  // all others are insufficient
325  for (const auto& jmg : robot_model_->getJointModelGroups())
326  {
327  const auto& joint_names{ jmg->getActiveJointModelNames() };
328  ASSERT_FALSE(joint_names.empty());
329  ASSERT_TRUE(sufficient_joint_limits.addLimit(joint_names.front(), sufficient_limit))
330  << "Failed to add the limits for joint " << joint_names.front();
331 
332  for (auto it = std::next(joint_names.begin()); it != joint_names.end(); ++it)
333  {
334  ASSERT_TRUE(sufficient_joint_limits.addLimit((*it), insufficient_limit))
335  << "Failed to add the limits for joint " << (*it);
336  }
337  }
338  LimitsContainer sufficient_planner_limits;
339  sufficient_planner_limits.setJointLimits(sufficient_joint_limits);
340 
341  EXPECT_NO_THROW({
342  std::unique_ptr<TrajectoryGeneratorPTP> ptp_no_error(
343  new TrajectoryGeneratorPTP(robot_model_, sufficient_planner_limits, planning_group_));
344  });
345 }
346 
350 TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoal)
351 {
352  //***************************************
353  //*** prepare the motion plan request ***
354  //***************************************
355  planning_interface::MotionPlanResponse res;
356  planning_interface::MotionPlanRequest req;
357  testutils::createDummyRequest(robot_model_, planning_group_, req);
358 
359  // cartesian goal pose
360  geometry_msgs::msg::PoseStamped pose;
361  pose.pose.position.x = 0.1;
362  pose.pose.position.y = 0.2;
363  pose.pose.position.z = 0.65;
364  pose.pose.orientation.w = 1.0;
365  pose.pose.orientation.x = 0.0;
366  pose.pose.orientation.y = 0.0;
367  pose.pose.orientation.z = 0.0;
368  std::vector<double> tolerance_pose(3, 0.01);
369  std::vector<double> tolerance_angle(3, 0.01);
370  moveit_msgs::msg::Constraints pose_goal =
371  kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle);
372  req.goal_constraints.push_back(pose_goal);
373 
374  //****************************************
375  //*** test robot model without gripper ***
376  //****************************************
377  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
378  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
379 
380  moveit_msgs::msg::MotionPlanResponse res_msg;
381  res.getMessage(res_msg);
382  if (!res_msg.trajectory.joint_trajectory.points.empty())
383  {
384  EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));
385  }
386  else
387  {
388  FAIL() << "Received empty trajectory.";
389  }
390 
391  // check goal pose
392  EXPECT_TRUE(testutils::isGoalReached(robot_model_, res_msg.trajectory.joint_trajectory, req, pose_norm_tolerance_));
393 }
394 
399 TEST_F(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints)
400 {
401  //***************************************
402  //*** prepare the motion plan request ***
403  //***************************************
404  planning_interface::MotionPlanResponse res;
405  planning_interface::MotionPlanRequest req;
406  testutils::createDummyRequest(robot_model_, planning_group_, req);
407 
408  // cartesian goal pose
409  geometry_msgs::msg::PoseStamped pose;
410  pose.pose.position.x = 0.1;
411  pose.pose.position.y = 0.2;
412  pose.pose.position.z = 0.65;
413  pose.pose.orientation.w = 1.0;
414  pose.pose.orientation.x = 0.0;
415  pose.pose.orientation.y = 0.0;
416  pose.pose.orientation.z = 0.0;
417  std::vector<double> tolerance_pose(3, 0.01);
418  std::vector<double> tolerance_angle(3, 0.01);
419  moveit_msgs::msg::Constraints pose_goal =
420  kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle);
421  req.goal_constraints.push_back(pose_goal);
422 
423  planning_interface::MotionPlanRequest req_no_position_constaint_link_name = req;
424  req_no_position_constaint_link_name.goal_constraints.front().position_constraints.front().link_name = "";
425  ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_position_constaint_link_name, res));
426  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
427 
428  planning_interface::MotionPlanRequest req_no_orientation_constaint_link_name = req;
429  req_no_orientation_constaint_link_name.goal_constraints.front().orientation_constraints.front().link_name = "";
430  ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_orientation_constaint_link_name, res));
431  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
432 }
433 
437 TEST_F(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal)
438 {
439  planning_interface::MotionPlanResponse res;
440  planning_interface::MotionPlanRequest req;
441  testutils::createDummyRequest(robot_model_, planning_group_, req);
442 
443  geometry_msgs::msg::PoseStamped pose;
444  pose.pose.position.x = 0.1;
445  pose.pose.position.y = 0.2;
446  pose.pose.position.z = 2.5;
447  pose.pose.orientation.w = 1.0;
448  pose.pose.orientation.x = 0.0;
449  pose.pose.orientation.y = 0.0;
450  pose.pose.orientation.z = 0.0;
451  std::vector<double> tolerance_pose(3, 0.01);
452  std::vector<double> tolerance_angle(3, 0.01);
453  moveit_msgs::msg::Constraints pose_goal =
454  kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle);
455  req.goal_constraints.push_back(pose_goal);
456 
457  ASSERT_FALSE(ptp_->generate(planning_scene_, req, res));
458  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
459  EXPECT_EQ(res.trajectory_, nullptr);
460 }
461 
467 TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached)
468 {
469  planning_interface::MotionPlanResponse res;
470  planning_interface::MotionPlanRequest req;
471  testutils::createDummyRequest(robot_model_, planning_group_, req);
472  ASSERT_TRUE(robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size())
473  << "No link exists in the planning group.";
474 
475  moveit_msgs::msg::Constraints gc;
476  moveit_msgs::msg::JointConstraint jc;
477  jc.joint_name = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().front();
478  jc.position = 0.0;
479  gc.joint_constraints.push_back(jc);
480  req.goal_constraints.push_back(gc);
481 
482  // TODO lin and circ has different settings
483  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
484  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
485 
486  moveit_msgs::msg::MotionPlanResponse res_msg;
487  res.getMessage(res_msg);
488  EXPECT_EQ(1u, res_msg.trajectory.joint_trajectory.points.size());
489 }
490 
495 TEST_F(TrajectoryGeneratorPTPTest, testScalingFactor)
496 {
497  // create ptp generator with different limits
498  pilz_industrial_motion_planner::JointLimit joint_limit;
499  JointLimitsContainer joint_limits;
500 
501  // set the joint limits
502  joint_limit.has_position_limits = true;
503  joint_limit.max_position = 2.967;
504  joint_limit.min_position = -2.967;
505  joint_limit.has_velocity_limits = true;
506  joint_limit.max_velocity = 2;
507  joint_limit.has_acceleration_limits = true;
508  joint_limit.max_acceleration = 1.5;
509  joint_limit.has_deceleration_limits = true;
510  joint_limit.max_deceleration = -3;
511  joint_limits.addLimit("prbt_joint_1", joint_limit);
512  joint_limit.max_position = 2.530;
513  joint_limit.min_position = -2.530;
514  joint_limits.addLimit("prbt_joint_2", joint_limit);
515  joint_limit.max_position = 2.356;
516  joint_limit.min_position = -2.356;
517  joint_limits.addLimit("prbt_joint_3", joint_limit);
518  joint_limit.max_position = 2.967;
519  joint_limit.min_position = -2.967;
520  joint_limits.addLimit("prbt_joint_4", joint_limit);
521  joint_limit.max_position = 2.967;
522  joint_limit.min_position = -2.967;
523  joint_limits.addLimit("prbt_joint_5", joint_limit);
524  joint_limit.max_position = 3.132;
525  joint_limit.min_position = -3.132;
526  joint_limits.addLimit("prbt_joint_6", joint_limit);
527  // add gripper limit such that generator does not complain about missing limit
528  joint_limits.addLimit("prbt_gripper_finger_left_joint", joint_limit);
529 
530  pilz_industrial_motion_planner::LimitsContainer planner_limits;
531  planner_limits.setJointLimits(joint_limits);
532 
533  // create the generator with new limits
534  ptp_ = std::make_unique<TrajectoryGeneratorPTP>(robot_model_, planner_limits, planning_group_);
535 
536  planning_interface::MotionPlanResponse res;
537  planning_interface::MotionPlanRequest req;
538  testutils::createDummyRequest(robot_model_, planning_group_, req);
539  req.start_state.joint_state.position[2] = 0.1;
540  moveit_msgs::msg::Constraints gc;
541  moveit_msgs::msg::JointConstraint jc;
542  jc.joint_name = "prbt_joint_1";
543  jc.position = 1.5;
544  gc.joint_constraints.push_back(jc);
545  jc.joint_name = "prbt_joint_3";
546  jc.position = 2.1;
547  gc.joint_constraints.push_back(jc);
548  jc.joint_name = "prbt_joint_6";
549  jc.position = 3.0;
550  gc.joint_constraints.push_back(jc);
551  req.goal_constraints.push_back(gc);
552  req.max_velocity_scaling_factor = 0.5;
553  req.max_acceleration_scaling_factor = 1.0 / 3.0;
554 
555  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
556  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
557 
558  moveit_msgs::msg::MotionPlanResponse res_msg;
559  res.getMessage(res_msg);
560  EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));
561 
562  // trajectory duration
563  EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()),
564  joint_acceleration_tolerance_);
565 
566  // way point at 1s
567  int index;
568  index = testutils::getWayPointIndex(res.trajectory_, 1.0);
569  // joint_1
570  EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
571  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
572  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
573  // joint_3
574  EXPECT_NEAR(1.0 / 6.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
575  joint_position_tolerance_);
576  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
577  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
578  joint_acceleration_tolerance_);
579  // joint_6
580  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
581  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
582  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
583  // other joints
584  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[4], joint_position_tolerance_);
585  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[4], joint_velocity_tolerance_);
586  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_);
587 
588  // way point at 2s
589  index = testutils::getWayPointIndex(res.trajectory_, 2.0);
590  // joint_1
591  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
592  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
593  // joint_3
594  EXPECT_NEAR(2.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
595  joint_position_tolerance_);
596  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
597  // joint_6
598  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
599  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
600  // other joints
601  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
602  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
603  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
604 
605  // way point at 3s
606  index = testutils::getWayPointIndex(res.trajectory_, 3.0);
607  // joint_1
608  EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
609  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
610  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
611  // joint_3
612  EXPECT_NEAR(4.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
613  joint_position_tolerance_);
614  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
615  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_);
616  // joint_6
617  EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
618  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
619  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
620  // other joints
621  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
622  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
623  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_);
624 
625  // way point at 4s
626  index = testutils::getWayPointIndex(res.trajectory_, 4.0);
627  // joint_1
628  EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
629  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
630  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
631  // joint_3
632  EXPECT_NEAR(5.75 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
633  joint_position_tolerance_);
634  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
635  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
636  joint_acceleration_tolerance_);
637  // joint_6
638  EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
639  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
640  EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
641 
642  // way point at 4.5s
643  index = testutils::getWayPointIndex(res.trajectory_, 4.5);
644  // joint_1
645  EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
646  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
647  // joint_3
648  EXPECT_NEAR(2.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_);
649  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
650  // joint_6
651  EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
652  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
653 }
654 
659 TEST_F(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity)
660 {
661  planning_interface::MotionPlanResponse res;
662  planning_interface::MotionPlanRequest req;
663  testutils::createDummyRequest(robot_model_, planning_group_, req);
664  req.start_state.joint_state.position[2] = 0.1;
665 
666  // Set velocity to all 1e-16
667  req.start_state.joint_state.velocity = std::vector<double>(req.start_state.joint_state.position.size(), 1e-16);
668 
669  moveit_msgs::msg::Constraints gc;
670  moveit_msgs::msg::JointConstraint jc;
671  jc.joint_name = "prbt_joint_1";
672  jc.position = 1.5;
673  gc.joint_constraints.push_back(jc);
674  jc.joint_name = "prbt_joint_3";
675  jc.position = 2.1;
676  gc.joint_constraints.push_back(jc);
677  jc.joint_name = "prbt_joint_6";
678  jc.position = 3.0;
679  gc.joint_constraints.push_back(jc);
680  req.goal_constraints.push_back(gc);
681 
682  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
683  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
684 
685  moveit_msgs::msg::MotionPlanResponse res_msg;
686  res.getMessage(res_msg);
687  EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));
688 
689  // trajectory duration
690  EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()),
691  joint_acceleration_tolerance_);
692 
693  // way point at 1s
694  int index;
695  index = testutils::getWayPointIndex(res.trajectory_, 1.0);
696  // joint_1
697  EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
698  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
699  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
700  // joint_3
701  EXPECT_NEAR(1.0 / 6.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
702  joint_position_tolerance_);
703  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
704  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
705  joint_acceleration_tolerance_);
706  // joint_6
707  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
708  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
709  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
710  // other joints
711  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[4], joint_position_tolerance_);
712  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[4], joint_velocity_tolerance_);
713  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[4], joint_acceleration_tolerance_);
714 
715  // way point at 2s
716  index = testutils::getWayPointIndex(res.trajectory_, 2.0);
717  // joint_1
718  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
719  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
720  // joint_3
721  EXPECT_NEAR(2.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
722  joint_position_tolerance_);
723  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
724  // joint_6
725  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
726  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
727  // other joints
728  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
729  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
730  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
731 
732  // way point at 3s
733  index = testutils::getWayPointIndex(res.trajectory_, 3.0);
734  // joint_1
735  EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
736  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
737  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
738  // joint_3
739  EXPECT_NEAR(4.0 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
740  joint_position_tolerance_);
741  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
742  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_);
743  // joint_6
744  EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
745  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
746  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
747  // other joints
748  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
749  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
750  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_);
751 
752  // way point at 4s
753  index = testutils::getWayPointIndex(res.trajectory_, 4.0);
754  // joint_1
755  EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
756  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
757  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
758  // joint_3
759  EXPECT_NEAR(5.75 / 3.0 + 0.1, res_msg.trajectory.joint_trajectory.points[index].positions[2],
760  joint_position_tolerance_);
761  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
762  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
763  joint_acceleration_tolerance_);
764  // joint_6
765  EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
766  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
767  EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
768 
769  // way point at 4.5s
770  index = testutils::getWayPointIndex(res.trajectory_, 4.5);
771  // joint_1
772  EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
773  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
774  // joint_3
775  EXPECT_NEAR(2.1, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_);
776  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
777  // joint_6
778  EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
779  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
780 
781  // Check that velocity at the end is all zero
782  EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().velocities.cbegin(),
783  res_msg.trajectory.joint_trajectory.points.back().velocities.cend(),
784  [this](double v) { return std::fabs(v) < this->joint_velocity_tolerance_; }));
785 
786  // Check that acceleration at the end is all zero
787  EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().accelerations.cbegin(),
788  res_msg.trajectory.joint_trajectory.points.back().accelerations.cend(),
789  [this](double v) { return std::fabs(v) < this->joint_acceleration_tolerance_; }));
790 }
791 
796 TEST_F(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel)
797 {
798  planning_interface::MotionPlanResponse res;
799  planning_interface::MotionPlanRequest req;
800  testutils::createDummyRequest(robot_model_, planning_group_, req);
801  req.start_state.joint_state.position[4] = 0.3;
802  req.start_state.joint_state.position[2] = 0.11;
803 
804  moveit_msgs::msg::Constraints gc;
805  moveit_msgs::msg::JointConstraint jc;
806 
807  jc.joint_name = "prbt_joint_1";
808  jc.position = 1.5;
809  gc.joint_constraints.push_back(jc);
810  jc.joint_name = "prbt_joint_2";
811  jc.position = -1.5;
812  gc.joint_constraints.push_back(jc);
813  jc.joint_name = "prbt_joint_3";
814  jc.position = 2.11;
815  gc.joint_constraints.push_back(jc);
816  jc.joint_name = "prbt_joint_4";
817  jc.position = -2.0;
818  gc.joint_constraints.push_back(jc);
819  jc.joint_name = "prbt_joint_6";
820  jc.position = 3.0;
821  gc.joint_constraints.push_back(jc);
822  req.goal_constraints.push_back(gc);
823 
824  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
825  EXPECT_EQ(res.error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
826 
827  moveit_msgs::msg::MotionPlanResponse res_msg;
828  res.getMessage(res_msg);
829  EXPECT_TRUE(checkTrajectory(res_msg.trajectory.joint_trajectory, req, planner_limits_.getJointLimitContainer()));
830 
831  // trajectory duration
832  EXPECT_NEAR(4.5, res.trajectory_->getWayPointDurationFromStart(res.trajectory_->getWayPointCount()),
833  joint_position_tolerance_);
834 
835  // way point at 0s
836  // joint_1
837  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[0], joint_position_tolerance_);
838  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[0], joint_velocity_tolerance_);
839  // joint_2
840  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[1], joint_position_tolerance_);
841  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[1], joint_velocity_tolerance_);
842  // joint_3
843  EXPECT_NEAR(0.11, res_msg.trajectory.joint_trajectory.points[0].positions[2], joint_position_tolerance_);
844  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[2], joint_velocity_tolerance_);
845  // joint_4
846  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[3], joint_position_tolerance_);
847  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[3], joint_velocity_tolerance_);
848  // joint_6
849  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].positions[5], joint_position_tolerance_);
850  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[0].velocities[5], joint_velocity_tolerance_);
851 
852  // way point at 1s
853  int index;
854  index = testutils::getWayPointIndex(res.trajectory_, 1.0);
855  // joint_1
856  EXPECT_NEAR(0.125, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
857  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
858  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
859  // joint_2
860  EXPECT_NEAR(-0.125, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
861  EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
862  EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
863  // joint_3
864  EXPECT_NEAR(1.0 / 6.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2],
865  joint_position_tolerance_);
866  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
867  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
868  joint_acceleration_tolerance_);
869  // joint_4
870  EXPECT_NEAR(-1.0 / 6.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
871  EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
872  EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3],
873  joint_acceleration_tolerance_);
874  // joint_6
875  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
876  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
877  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
878 
879  // way point at 2s
880  index = testutils::getWayPointIndex(res.trajectory_, 2.0);
881  // joint_1
882  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
883  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
884  // joint_2
885  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
886  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
887  // joint_3
888  EXPECT_NEAR(2.0 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2],
889  joint_position_tolerance_);
890  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
891  // joint_4
892  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
893  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
894  // joint_6
895  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
896  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
897 
898  // way point at 3s
899  index = testutils::getWayPointIndex(res.trajectory_, 3.0);
900  // joint_1
901  EXPECT_NEAR(1, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
902  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
903  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
904  // joint_2
905  EXPECT_NEAR(-1, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
906  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
907  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
908  // joint_3
909  EXPECT_NEAR(4.0 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2],
910  joint_position_tolerance_);
911  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
912  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2], joint_acceleration_tolerance_);
913  // joint_4
914  EXPECT_NEAR(-4.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
915  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
916  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3], joint_acceleration_tolerance_);
917  // joint_6
918  EXPECT_NEAR(2.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
919  EXPECT_NEAR(1.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
920  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
921 
922  // way point at 4s
923  index = testutils::getWayPointIndex(res.trajectory_, 4.0);
924  // joint_1
925  EXPECT_NEAR(2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
926  EXPECT_NEAR(0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
927  EXPECT_NEAR(-0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[0], joint_acceleration_tolerance_);
928  // joint_2
929  EXPECT_NEAR(-2.875 / 2.0, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
930  EXPECT_NEAR(-0.25, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
931  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].accelerations[1], joint_acceleration_tolerance_);
932  // joint_3
933  EXPECT_NEAR(5.75 / 3.0 + 0.11, res_msg.trajectory.joint_trajectory.points[index].positions[2],
934  joint_position_tolerance_);
935  EXPECT_NEAR(1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
936  EXPECT_NEAR(-2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[2],
937  joint_acceleration_tolerance_);
938  // joint_4
939  EXPECT_NEAR(-5.75 / 3.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
940  EXPECT_NEAR(-1.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
941  EXPECT_NEAR(2.0 / 3.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[3],
942  joint_acceleration_tolerance_);
943  // joint_6
944  EXPECT_NEAR(2.875, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
945  EXPECT_NEAR(0.5, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
946  EXPECT_NEAR(-1.0, res_msg.trajectory.joint_trajectory.points[index].accelerations[5], joint_acceleration_tolerance_);
947 
948  // way point at 4.5s
949  index = testutils::getWayPointIndex(res.trajectory_, 4.5);
950  // joint_1
951  EXPECT_NEAR(1.5, res_msg.trajectory.joint_trajectory.points[index].positions[0], joint_position_tolerance_);
952  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[0], joint_velocity_tolerance_);
953  // joint_2
954  EXPECT_NEAR(-1.5, res_msg.trajectory.joint_trajectory.points[index].positions[1], joint_position_tolerance_);
955  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[1], joint_velocity_tolerance_);
956  // joint_3
957  EXPECT_NEAR(2.11, res_msg.trajectory.joint_trajectory.points[index].positions[2], joint_position_tolerance_);
958  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[2], joint_velocity_tolerance_);
959  // joint_4
960  EXPECT_NEAR(-2.0, res_msg.trajectory.joint_trajectory.points[index].positions[3], joint_position_tolerance_);
961  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[3], joint_velocity_tolerance_);
962  // joint_6
963  EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
964  EXPECT_NEAR(0.0, res_msg.trajectory.joint_trajectory.points[index].velocities[5], joint_velocity_tolerance_);
965 
966  // Check last point
967  EXPECT_NEAR(3.0, res_msg.trajectory.joint_trajectory.points[index].positions[5], joint_position_tolerance_);
968 
969  // Check that velocity at the end is all zero
970  EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().velocities.cbegin(),
971  res_msg.trajectory.joint_trajectory.points.back().velocities.cend(),
972  [this](double v) { return std::fabs(v) < this->joint_velocity_tolerance_; }));
973 
974  // Check that acceleration at the end is all zero
975  EXPECT_TRUE(std::all_of(res_msg.trajectory.joint_trajectory.points.back().accelerations.cbegin(),
976  res_msg.trajectory.joint_trajectory.points.back().accelerations.cend(),
977  [this](double v) { return std::fabs(v) < this->joint_acceleration_tolerance_; }));
978 }
979 
980 int main(int argc, char** argv)
981 {
982  rclcpp::init(argc, argv);
983  testing::InitGoogleTest(&argc, argv);
984  return RUN_ALL_TESTS();
985 }
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:45
pilz_industrial_motion_planner::LimitsContainer
This class combines CartesianLimit and JointLimits into on single class.
Definition: limits_container.h:48
pilz_industrial_motion_planner::LimitsContainer::setJointLimits
void setJointLimits(JointLimitsContainer &joint_limits)
Set joint limits.
Definition: limits_container.cpp:47
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:61
joint_limits_aggregator.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:144
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 veryfied.
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:1258
robot_model.h
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
Definition: planning_response.h:47
planning_interface::MotionPlanResponse::trajectory_
robot_trajectory::RobotTrajectoryPtr trajectory_
Definition: planning_response.h:54
planning_interface::MotionPlanResponse::getMessage
void getMessage(moveit_msgs::msg::MotionPlanResponse &msg) const
Definition: planning_response.cpp:40
planning_interface::MotionPlanResponse::error_code_
moveit_msgs::msg::MoveItErrorCodes error_code_
Definition: planning_response.h:56
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:980
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