moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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
51const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
52const std::string PARAM_TARGET_LINK_NAME("target_link");
53const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance");
54const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance");
55const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance");
56const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance");
57
58using namespace pilz_industrial_motion_planner;
59
60class TrajectoryGeneratorPTPTest : public testing::Test
61{
62protected:
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
144protected:
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
164TEST_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
180TEST_F(TrajectoryGeneratorPTPTest, noLimits)
181{
182 LimitsContainer planner_limits;
183 EXPECT_THROW(TrajectoryGeneratorPTP(this->robot_model_, planner_limits, planning_group_),
184 TrajectoryGeneratorInvalidLimitsException);
185}
186
197TEST_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
219TEST_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
244TEST_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
276TEST_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
351TEST_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
400TEST_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
438TEST_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
468TEST_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
496TEST_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
660TEST_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
797TEST_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
981int 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.8