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