moveit2
The MoveIt Motion Planning Framework for ROS 2.
unittest_trajectory_functions.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 <gtest/gtest.h>
36 
37 #include <map>
38 #include <math.h>
39 #include <string>
40 #include <vector>
41 
42 #include <Eigen/Geometry>
43 #include <kdl/frames.hpp>
44 #include <kdl/path_roundedcomposite.hpp>
45 #include <kdl/rotational_interpolation_sa.hpp>
46 #include <kdl/trajectory.hpp>
47 #include <kdl/trajectory_segment.hpp>
48 #include <kdl/velocityprofile_trap.hpp>
53 #include <tf2_eigen/tf2_eigen.hpp>
54 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
55 
60 #include "test_utils.h"
61 
62 #define _USE_MATH_DEFINES
63 
64 static constexpr double EPSILON{ 1.0e-6 };
65 static constexpr double IK_SEED_OFFSET{ 0.1 };
66 static constexpr double L0{ 0.2604 }; // Height of foot
67 static constexpr double L1{ 0.3500 }; // Height of first connector
68 static constexpr double L2{ 0.3070 }; // Height of second connector
69 static constexpr double L3{ 0.0840 }; // Distance last joint to flange
70 
71 // parameters from parameter server
72 const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
73 const std::string GROUP_TIP_LINK_NAME("group_tip_link");
74 const std::string ROBOT_TCP_LINK_NAME("tcp_link");
75 const std::string IK_FAST_LINK_NAME("ik_fast_link");
76 const std::string RANDOM_TEST_NUMBER("random_test_number");
77 
81 class TrajectoryFunctionsTestBase : public testing::Test
82 {
83 protected:
88  void SetUp() override
89  {
90  rclcpp::NodeOptions node_options;
91  node_options.automatically_declare_parameters_from_overrides(true);
92  node_ = rclcpp::Node::make_shared("unittest_trajectory_functions", node_options);
93 
94  // load robot model
95  rm_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(node_);
96  robot_model_ = rm_loader_->getModel();
97  ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
98  planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
99 
100  // get parameters
101  ASSERT_TRUE(node_->has_parameter("planning_group"));
102  node_->get_parameter<std::string>("planning_group", planning_group_);
103  ASSERT_TRUE(node_->has_parameter("group_tip_link"));
104  node_->get_parameter<std::string>("group_tip_link", group_tip_link_);
105  ASSERT_TRUE(node_->has_parameter("tcp_link"));
106  node_->get_parameter<std::string>("tcp_link", tcp_link_);
107  ASSERT_TRUE(node_->has_parameter("ik_fast_link"));
108  node_->get_parameter<std::string>("ik_fast_link", ik_fast_link_);
109  ASSERT_TRUE(node_->has_parameter("random_test_number"));
110  node_->get_parameter<int>("random_test_number", random_test_number_);
111 
112  // check robot model
114 
115  // initialize the zero state configurationg and test joint state
116  joint_names_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames();
117  for (const auto& joint_name : joint_names_)
118  {
119  zero_state_[joint_name] = 0.0;
120  }
121  }
122 
123  void TearDown() override
124  {
125  robot_model_.reset();
126  }
127 
135  bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double& epsilon);
136 
137 protected:
138  // ros stuff
139  rclcpp::Node::SharedPtr node_;
140  moveit::core::RobotModelConstPtr robot_model_;
141  std::unique_ptr<robot_model_loader::RobotModelLoader> rm_loader_;
142  planning_scene::PlanningSceneConstPtr planning_scene_;
143 
144  // test parameters from parameter server
147  std::vector<std::string> joint_names_;
148  std::map<std::string, double> zero_state_;
149 
150  // random seed
151  uint32_t random_seed_{ 100 };
152  random_numbers::RandomNumberGenerator rng_{ random_seed_ };
153 };
154 
155 bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2,
156  const double& epsilon)
157 {
158  for (std::size_t i = 0; i < 3; ++i)
159  for (std::size_t j = 0; j < 4; ++j)
160  {
161  if (fabs(pose1(i, j) - pose2(i, j)) > fabs(epsilon))
162  return false;
163  }
164  return true;
165 }
166 
171 {
172 };
173 
174 // TODO(henningkayser): re-enable gripper tests
175 // /**
176 // * @brief Parametrized class for tests, that only run with a gripper
177 // */
178 // class TrajectoryFunctionsTestOnlyGripper : public TrajectoryFunctionsTestBase
179 // {
180 // };
181 
188 {
189  Eigen::Isometry3d tip_pose;
190  std::map<std::string, double> test_state = zero_state_;
191  EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(planning_scene_, group_tip_link_, test_state, tip_pose));
192  EXPECT_NEAR(tip_pose(0, 3), 0, EPSILON);
193  EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON);
194  EXPECT_NEAR(tip_pose(2, 3), L0 + L1 + L2 + L3, EPSILON);
195 
196  test_state[joint_names_.at(1)] = M_PI_2;
197  EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(planning_scene_, group_tip_link_, test_state, tip_pose));
198  EXPECT_NEAR(tip_pose(0, 3), L1 + L2 + L3, EPSILON);
199  EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON);
200  EXPECT_NEAR(tip_pose(2, 3), L0, EPSILON);
201 
202  test_state[joint_names_.at(1)] = -M_PI_2;
203  test_state[joint_names_.at(2)] = M_PI_2;
204  EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(planning_scene_, group_tip_link_, test_state, tip_pose));
205  EXPECT_NEAR(tip_pose(0, 3), -L1, EPSILON);
206  EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON);
207  EXPECT_NEAR(tip_pose(2, 3), L0 - L2 - L3, EPSILON);
208 
209  // wrong link name
210  std::string link_name = "wrong_link_name";
211  EXPECT_FALSE(pilz_industrial_motion_planner::computeLinkFK(planning_scene_, link_name, test_state, tip_pose));
212 }
213 
218 {
219  // Load solver
220  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
221  const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
222 
223  // robot state
224  moveit::core::RobotState rstate(robot_model_);
225 
226  while (random_test_number_ > 0)
227  {
228  // sample random robot state
229  rstate.setToRandomPositions(jmg, rng_);
230  rstate.update();
231  geometry_msgs::msg::Pose pose_expect = tf2::toMsg(rstate.getFrameTransform(ik_fast_link_));
232 
233  // prepare inverse kinematics
234  std::vector<geometry_msgs::msg::Pose> ik_poses;
235  ik_poses.push_back(pose_expect);
236  std::vector<double> ik_seed, ik_expect, ik_actual;
237  for (const auto& joint_name : jmg->getActiveJointModelNames())
238  {
239  ik_expect.push_back(rstate.getVariablePosition(joint_name));
240  if (rstate.getVariablePosition(joint_name) > 0)
241  ik_seed.push_back(rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET);
242  else
243  ik_seed.push_back(rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET);
244  }
245 
246  std::vector<std::vector<double>> ik_solutions;
248  moveit_msgs::msg::MoveItErrorCodes err_code;
250 
251  // compute all ik solutions
252  EXPECT_TRUE(solver->getPositionIK(ik_poses, ik_seed, ik_solutions, ik_result, options));
253 
254  // compute one ik solution
255  EXPECT_TRUE(solver->getPositionIK(pose_expect, ik_seed, ik_actual, err_code));
256 
257  ASSERT_EQ(ik_expect.size(), ik_actual.size());
258 
259  for (std::size_t i = 0; i < ik_expect.size(); ++i)
260  {
261  EXPECT_NEAR(ik_actual.at(i), ik_expect.at(i), 4 * IK_SEED_OFFSET);
262  }
263 
264  --random_test_number_;
265  }
266 }
267 
273 {
274  // robot state
275  moveit::core::RobotState rstate(robot_model_);
276  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
277 
278  while (random_test_number_ > 0)
279  {
280  // sample random robot state
281  rstate.setToRandomPositions(jmg, rng_);
282 
283  Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_);
284 
285  // copy the random state and set ik seed
286  std::map<std::string, double> ik_seed, ik_expect;
287  for (const auto& joint_name : joint_names_)
288  {
289  ik_expect[joint_name] = rstate.getVariablePosition(joint_name);
290  if (rstate.getVariablePosition(joint_name) > 0)
291  ik_seed[joint_name] = rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET;
292  else
293  ik_seed[joint_name] = rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET;
294  }
295 
296  rstate.setVariablePositions(ik_seed);
297  rstate.update();
298 
299  // compute the ik
300  std::map<std::string, double> ik_actual;
301 
302  EXPECT_TRUE(rstate.setFromIK(robot_model_->getJointModelGroup(planning_group_), pose_expect, tcp_link_));
303 
304  for (const auto& joint_name : joint_names_)
305  {
306  ik_actual[joint_name] = rstate.getVariablePosition(joint_name);
307  }
308 
309  // compare ik solution and expected value
310  for (const auto& joint_pair : ik_actual)
311  {
312  EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET);
313  }
314 
315  // compute the pose from ik_solution
316  rstate.setVariablePositions(ik_actual);
317  rstate.update();
318  Eigen::Isometry3d pose_actual = rstate.getFrameTransform(tcp_link_);
319 
320  EXPECT_TRUE(tfNear(pose_expect, pose_actual, EPSILON));
321 
322  --random_test_number_;
323  }
324 }
325 
331 {
332  // robot state
333  moveit::core::RobotState rstate(robot_model_);
334 
335  const std::string frame_id = robot_model_->getModelFrame();
336  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
337 
338  while (random_test_number_ > 0)
339  {
340  // sample random robot state
341  rstate.setToRandomPositions(jmg, rng_);
342 
343  Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_);
344 
345  // copy the random state and set ik seed
346  std::map<std::string, double> ik_seed, ik_expect;
347  for (const auto& joint_name : robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames())
348  {
349  ik_expect[joint_name] = rstate.getVariablePosition(joint_name);
350  if (rstate.getVariablePosition(joint_name) > 0)
351  {
352  ik_seed[joint_name] = rstate.getVariablePosition(joint_name) - IK_SEED_OFFSET;
353  }
354  else
355  {
356  ik_seed[joint_name] = rstate.getVariablePosition(joint_name) + IK_SEED_OFFSET;
357  }
358  }
359 
360  // compute the ik
361  std::map<std::string, double> ik_actual;
362  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
363  frame_id, ik_seed, ik_actual, false));
364 
365  // compare ik solution and expected value
366  for (const auto& joint_pair : ik_actual)
367  {
368  EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET);
369  }
370 
371  --random_test_number_;
372  }
373 }
374 
378 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupName)
379 {
380  const std::string frame_id = robot_model_->getModelFrame();
381  Eigen::Isometry3d pose_expect;
382 
383  std::map<std::string, double> ik_seed;
384 
385  // compute the ik
386  std::map<std::string, double> ik_actual;
387  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, "InvalidGroupName", tcp_link_,
388  pose_expect, frame_id, ik_seed, ik_actual, false));
389 }
390 
394 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName)
395 {
396  const std::string frame_id = robot_model_->getModelFrame();
397  Eigen::Isometry3d pose_expect;
398 
399  std::map<std::string, double> ik_seed;
400 
401  // compute the ik
402  std::map<std::string, double> ik_actual;
403  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, "WrongLink", pose_expect,
404  frame_id, ik_seed, ik_actual, false));
405 }
406 
412 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
413 {
414  Eigen::Isometry3d pose_expect;
415 
416  std::map<std::string, double> ik_seed;
417 
418  // compute the ik
419  std::map<std::string, double> ik_actual;
420  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
421  "InvalidFrameId", ik_seed, ik_actual, false));
422 }
423 
424 // /**
425 // * @brief Test if activated self collision for a pose that would be in self
426 // * collision without the check results in a
427 // * valid ik solution.
428 // */
429 // TEST_F(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForValidPosition)
430 // {
431 // const std::string frame_id = robot_model_->getModelFrame();
432 // const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
433 //
434 // // create seed
435 // std::vector<double> ik_seed_states = { -0.553, 0.956, 1.758, 0.146, -1.059, 1.247 };
436 // auto joint_names = jmg->getActiveJointModelNames();
437 //
438 // std::map<std::string, double> ik_seed;
439 // for (unsigned int i = 0; i < ik_seed_states.size(); ++i)
440 // {
441 // ik_seed[joint_names[i]] = ik_seed_states[i];
442 // }
443 //
444 // // create expected pose
445 // geometry_msgs::msg::Pose pose;
446 // pose.position.x = -0.454;
447 // pose.position.y = -0.15;
448 // pose.position.z = 0.431;
449 // pose.orientation.y = 0.991562;
450 // pose.orientation.w = -0.1296328;
451 // Eigen::Isometry3d pose_expect;
452 // normalizeQuaternion(pose.orientation);
453 // tf2::fromMsg(pose, pose_expect);
454 //
455 // // compute the ik without self collision check and expect the resulting pose
456 // // to be in self collision.
457 // std::map<std::string, double> ik_actual1;
458 // EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
459 // frame_id, ik_seed, ik_actual1, false));
460 //
461 // moveit::core::RobotState rstate(robot_model_);
462 // planning_scene::PlanningScene rscene(robot_model_);
463 //
464 // std::vector<double> ik_state;
465 // std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state),
466 // [](const auto& pair) { return pair.second; });
467 //
468 // rstate.setJointGroupPositions(jmg, ik_state);
469 // rstate.update();
470 //
471 // collision_detection::CollisionRequest collision_req;
472 // collision_req.group_name = jmg->getName();
473 // collision_detection::CollisionResult collision_res;
474 //
475 // rscene.checkSelfCollision(collision_req, collision_res, rstate);
476 //
477 // EXPECT_TRUE(collision_res.collision);
478 //
479 // // compute the ik with collision detection activated and expect the resulting
480 // // pose to be without self collision.
481 // std::map<std::string, double> ik_actual2;
482 // EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
483 // frame_id, ik_seed, ik_actual2, true));
484 //
485 // std::vector<double> ik_state2;
486 // std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2),
487 // [](const auto& pair) { return pair.second; });
488 // rstate.setJointGroupPositions(jmg, ik_state2);
489 // rstate.update();
490 //
491 // collision_detection::CollisionResult collision_res2;
492 // rscene.checkSelfCollision(collision_req, collision_res2, rstate);
493 //
494 // EXPECT_FALSE(collision_res2.collision);
495 // }
496 
501 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionForInvalidPose)
502 {
503  // robot state
504  moveit::core::RobotState rstate(robot_model_);
505 
506  const std::string frame_id = robot_model_->getModelFrame();
507  const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
508 
509  // create seed
510  std::map<std::string, double> ik_seed;
511  for (const auto& joint_name : jmg->getActiveJointModelNames())
512  {
513  ik_seed[joint_name] = 0;
514  }
515 
516  // create goal
517  std::vector<double> ik_goal = { 0, 2.3, -2.3, 0, 0, 0 };
518 
519  rstate.setJointGroupPositions(jmg, ik_goal);
520 
521  Eigen::Isometry3d pose_expect = rstate.getFrameTransform(tcp_link_);
522 
523  // compute the ik with disabled collision check
524  std::map<std::string, double> ik_actual;
525  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
526  frame_id, ik_seed, ik_actual, false));
527 
528  // compute the ik with enabled collision check
529  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
530  frame_id, ik_seed, ik_actual, true));
531 }
532 
543 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsWithSmallDuration)
544 {
545  const std::map<std::string, double> position_last, velocity_last, position_current;
546  double duration_last{ 0.0 };
548 
549  double duration_current = 10e-7;
550 
551  EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current,
552  duration_last, duration_current, joint_limits));
553 }
554 
565 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsVelocityViolation)
566 {
567  const std::string test_joint_name{ "joint" };
568 
569  std::map<std::string, double> position_last{ { test_joint_name, 2.0 } };
570  std::map<std::string, double> position_current{ { test_joint_name, 10.0 } };
571  std::map<std::string, double> velocity_last;
572  double duration_current{ 1.0 };
573  double duration_last{ 0.0 };
575 
577  // Calculate the max allowed velocity in such a way that it is always smaller
578  // than the current velocity.
579  test_joint_limits.max_velocity =
580  ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current) - 1.0;
581  test_joint_limits.has_velocity_limits = true;
582  joint_limits.addLimit(test_joint_name, test_joint_limits);
583 
584  EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current,
585  duration_last, duration_current, joint_limits));
586 }
587 
598 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsAccelerationViolation)
599 {
600  const std::string test_joint_name{ "joint" };
601 
602  double duration_current = 1.0;
603  double duration_last = 1.0;
604 
605  std::map<std::string, double> position_last{ { test_joint_name, 2.0 } };
606  std::map<std::string, double> position_current{ { test_joint_name, 20.0 } };
607  double velocity_current =
608  ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current);
609  std::map<std::string, double> velocity_last{ { test_joint_name, 9.0 } };
611 
613  // Calculate the max allowed velocity in such a way that it is always bigger
614  // than the current velocity.
615  test_joint_limits.max_velocity = velocity_current + 1.0;
616  test_joint_limits.has_velocity_limits = true;
617 
618  double acceleration_current =
619  (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2;
620  // Calculate the max allowed acceleration in such a way that it is always
621  // smaller than the current acceleration.
622  test_joint_limits.max_acceleration = acceleration_current - 1.0;
623  test_joint_limits.has_acceleration_limits = true;
624 
625  joint_limits.addLimit(test_joint_name, test_joint_limits);
626 
627  EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current,
628  duration_last, duration_current, joint_limits));
629 }
630 
641 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testVerifySampleJointLimitsDecelerationViolation)
642 {
643  const std::string test_joint_name{ "joint" };
644 
645  double duration_current = 1.0;
646  double duration_last = 1.0;
647 
648  std::map<std::string, double> position_last{ { test_joint_name, 20.0 } };
649  std::map<std::string, double> position_current{ { test_joint_name, 2.0 } };
650  double velocity_current =
651  ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current);
652  std::map<std::string, double> velocity_last{ { test_joint_name, 19.0 } };
654 
656  // Calculate the max allowed velocity in such a way that it is always bigger
657  // than the current velocity.
658  test_joint_limits.max_velocity = fabs(velocity_current) + 1.0;
659  test_joint_limits.has_velocity_limits = true;
660 
661  double acceleration_current =
662  (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2;
663  // Calculate the max allowed deceleration in such a way that it is always
664  // bigger than the current acceleration.
665  test_joint_limits.max_deceleration = acceleration_current + 1.0;
666  test_joint_limits.has_deceleration_limits = true;
667 
668  joint_limits.addLimit(test_joint_name, test_joint_limits);
669 
670  EXPECT_FALSE(pilz_industrial_motion_planner::verifySampleJointLimits(position_last, velocity_last, position_current,
671  duration_last, duration_current, joint_limits));
672 }
673 
688 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithInvalidCartesianTrajectory)
689 {
690  // Create random test trajectory
691  // Note: 'path' is deleted by KDL::Trajectory_Segment
692  KDL::Path_RoundedComposite* path =
693  new KDL::Path_RoundedComposite(0.2, 0.01, new KDL::RotationalInterpolation_SingleAxis());
694  path->Add(KDL::Frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(-1, 0, 0)));
695  path->Finish();
696  // Note: 'velprof' is deleted by KDL::Trajectory_Segment
697  KDL::VelocityProfile* vel_prof = new KDL::VelocityProfile_Trap(0.5, 0.1);
698  vel_prof->SetProfile(0, path->PathLength());
699  KDL::Trajectory_Segment kdl_trajectory(path, vel_prof);
700 
702  std::string group_name{ "invalid_group_name" };
703  std::map<std::string, double> initial_joint_position;
704  double sampling_time{ 0.1 };
705  trajectory_msgs::msg::JointTrajectory joint_trajectory;
706  moveit_msgs::msg::MoveItErrorCodes error_code;
707  bool check_self_collision{ false };
708 
710  planning_scene_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time,
711  joint_trajectory, error_code, check_self_collision));
712 
713  std::map<std::string, double> initial_joint_velocity;
714 
716  cart_traj.group_name = group_name;
717  cart_traj.link_name = tcp_link_;
719  cart_traj.points.push_back(cart_traj_point);
720 
722  planning_scene_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity,
723  joint_trajectory, error_code, check_self_collision));
724 }
725 
737 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeInvalidVectorSize)
738 {
739  robot_trajectory::RobotTrajectoryPtr first_trajectory =
740  std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
741  robot_trajectory::RobotTrajectoryPtr second_trajectory =
742  std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
743  double epsilon{ 0.0 };
744  double sampling_time{ 0.0 };
745 
746  moveit::core::RobotState rstate(robot_model_);
747  first_trajectory->insertWayPoint(0, rstate, 0.1);
748  second_trajectory->insertWayPoint(0, rstate, 0.1);
749 
750  EXPECT_FALSE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory,
751  epsilon, sampling_time));
752 }
753 
765 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeCorrectSamplingTime)
766 {
767  robot_trajectory::RobotTrajectoryPtr first_trajectory =
768  std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
769  robot_trajectory::RobotTrajectoryPtr second_trajectory =
770  std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
771  double epsilon{ 0.0001 };
772  double sampling_time{ 0.0 };
773  double expected_sampling_time{ 0.1 };
774 
775  moveit::core::RobotState rstate(robot_model_);
776  first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
777  first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
778 
779  second_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
780  second_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
781  second_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
782 
783  EXPECT_TRUE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory,
784  epsilon, sampling_time));
785  EXPECT_EQ(expected_sampling_time, sampling_time);
786 }
787 
799 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testDetermineAndCheckSamplingTimeViolateSamplingTime)
800 {
801  robot_trajectory::RobotTrajectoryPtr first_trajectory =
802  std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
803  robot_trajectory::RobotTrajectoryPtr second_trajectory =
804  std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
805  double epsilon{ 0.0001 };
806  double sampling_time{ 0.0 };
807  double expected_sampling_time{ 0.1 };
808 
809  moveit::core::RobotState rstate(robot_model_);
810  first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
811  first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
812  first_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
813  // Violate sampling time
814  first_trajectory->insertWayPoint(2, rstate, expected_sampling_time + 1.0);
815  first_trajectory->insertWayPoint(3, rstate, expected_sampling_time);
816 
817  second_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
818  second_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
819  second_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
820  second_trajectory->insertWayPoint(3, rstate, expected_sampling_time);
821 
822  EXPECT_FALSE(pilz_industrial_motion_planner::determineAndCheckSamplingTime(first_trajectory, second_trajectory,
823  epsilon, sampling_time));
824  EXPECT_EQ(expected_sampling_time, sampling_time);
825 }
826 
838 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualPositionUnequal)
839 {
840  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
841  moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);
842 
843  double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
844  rstate_1.setJointGroupPositions(planning_group_, default_joint_position);
845  // Ensure that the joint positions of both robot states are different
846  default_joint_position[0] = default_joint_position[0] + 70.0;
847  rstate_2.setJointGroupPositions(planning_group_, default_joint_position);
848 
849  double epsilon{ 0.0001 };
850  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon));
851 }
852 
864 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualVelocityUnequal)
865 {
866  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
867  moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);
868 
869  // Ensure that the joint positions of both robot state are equal
870  double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
871  rstate_1.setJointGroupPositions(planning_group_, default_joint_position);
872  rstate_2.setJointGroupPositions(planning_group_, default_joint_position);
873 
874  double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
875  rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity);
876  // Ensure that the joint velocites of both robot states are different
877  default_joint_velocity[1] = default_joint_velocity[1] + 10.0;
878  rstate_2.setJointGroupVelocities(planning_group_, default_joint_velocity);
879 
880  double epsilon{ 0.0001 };
881  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon));
882 }
883 
895 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateEqualAccelerationUnequal)
896 {
897  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
898  moveit::core::RobotState rstate_2 = moveit::core::RobotState(robot_model_);
899 
900  // Ensure that the joint positions of both robot state are equal
901  double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
902  rstate_1.setJointGroupPositions(planning_group_, default_joint_position);
903  rstate_2.setJointGroupPositions(planning_group_, default_joint_position);
904 
905  // Ensure that the joint velocities of both robot state are equal
906  double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
907  rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity);
908  rstate_2.setJointGroupVelocities(planning_group_, default_joint_velocity);
909 
910  double default_joint_acceleration[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
911  rstate_1.setJointGroupAccelerations(planning_group_, default_joint_acceleration);
912  // Ensure that the joint accelerations of both robot states are different
913  default_joint_acceleration[1] = default_joint_acceleration[1] + 10.0;
914  rstate_2.setJointGroupAccelerations(planning_group_, default_joint_acceleration);
915 
916  double epsilon{ 0.0001 };
917  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateEqual(rstate_1, rstate_2, planning_group_, epsilon));
918 }
919 
931 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryVelocityUnequal)
932 {
933  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
934 
935  // Ensure that the joint velocities are NOT zero
936  double default_joint_velocity[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
937  rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity);
938 
939  double epsilon{ 0.0001 };
940  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateStationary(rstate_1, planning_group_, epsilon));
941 }
942 
954 TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal)
955 {
956  moveit::core::RobotState rstate_1 = moveit::core::RobotState(robot_model_);
957 
958  // Ensure that the joint velocities are zero
959  double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
960  rstate_1.setJointGroupVelocities(planning_group_, default_joint_velocity);
961 
962  // Ensure that the joint acceleration are NOT zero
963  double default_joint_acceleration[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
964  rstate_1.setJointGroupAccelerations(planning_group_, default_joint_acceleration);
965 
966  double epsilon{ 0.0001 };
967  EXPECT_FALSE(pilz_industrial_motion_planner::isRobotStateStationary(rstate_1, planning_group_, epsilon));
968 }
969 
970 int main(int argc, char** argv)
971 {
972  rclcpp::init(argc, argv);
973  testing::InitGoogleTest(&argc, argv);
974  return RUN_ALL_TESTS();
975 }
std::unique_ptr< robot_model_loader::RobotModelLoader > rm_loader_
void SetUp() override
Create test scenario for trajectory functions.
random_numbers::RandomNumberGenerator rng_
moveit::core::RobotModelConstPtr robot_model_
std::map< std::string, double > zero_state_
planning_scene::PlanningSceneConstPtr planning_scene_
bool tfNear(const Eigen::Isometry3d &pose1, const Eigen::Isometry3d &pose2, const double &epsilon)
check if two transformations are close
Parametrized class for tests with and without gripper.
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
const std::vector< std::string > & getActiveJointModelNames() const
Get the names of the active joints in this group. These are the names of the joints returned by getJo...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
void setJointGroupAccelerations(const std::string &joint_group_name, const double *gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
Definition: robot_state.h:848
void setJointGroupVelocities(const std::string &joint_group_name, const double *gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
Definition: robot_state.h:748
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:605
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:207
void update(bool force=false)
Update all transforms.
bool setFromIK(const JointModelGroup *group, const geometry_msgs::msg::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
If the group this state corresponds to is a chain and a solver is available, then the joint values ca...
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
frame_id
Definition: pick.py:63
bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr &scene, const JointLimitsContainer &joint_limits, const KDL::Trajectory &trajectory, const std::string &group_name, const std::string &link_name, const std::map< std::string, double > &initial_joint_position, const double &sampling_time, trajectory_msgs::msg::JointTrajectory &joint_trajectory, moveit_msgs::msg::MoveItErrorCodes &error_code, bool check_self_collision=false)
Generate joint trajectory from a KDL Cartesian trajectory.
bool isRobotStateStationary(const moveit::core::RobotState &state, const std::string &group, double EPSILON)
check if the robot state have zero velocity/acceleration
bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr &first_trajectory, const robot_trajectory::RobotTrajectoryPtr &second_trajectory, double EPSILON, double &sampling_time)
Determines the sampling time and checks that both trajectroies use the same sampling time.
bool verifySampleJointLimits(const std::map< std::string, double > &position_last, const std::map< std::string, double > &velocity_last, const std::map< std::string, double > &position_current, double duration_last, double duration_current, const JointLimitsContainer &joint_limits)
verify the velocity/acceleration limits of current sample (based on backward difference computation) ...
bool computePoseIK(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const std::string &link_name, const Eigen::Isometry3d &pose, const std::string &frame_id, const std::map< std::string, double > &seed, std::map< std::string, double > &solution, bool check_self_collision=true, const double timeout=0.0)
compute the inverse kinematics of a given pose, also check robot self collision
bool isRobotStateEqual(const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const std::string &joint_group_name, double epsilon)
Check if the two robot states have the same joint position/velocity/acceleration.
bool computeLinkFK(const planning_scene::PlanningSceneConstPtr &scene, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
compute the pose of a link at give robot state
void checkRobotModel(const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name, const std::string &link_name)
A set of options for the kinematics solver.
std::vector< CartesianTrajectoryPoint > points
Extends joint_limits_interface::JointLimits with a deceleration parameter.
double max_deceleration
maximum deceleration MUST(!) be negative
const std::string PARAM_PLANNING_GROUP_NAME("planning_group")
const std::string RANDOM_TEST_NUMBER("random_test_number")
int main(int argc, char **argv)
const std::string IK_FAST_LINK_NAME("ik_fast_link")
TEST_F(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK)
Test the forward kinematics function with simple robot poses for robot tip link using robot model wit...
const std::string ROBOT_TCP_LINK_NAME("tcp_link")
const std::string GROUP_TIP_LINK_NAME("group_tip_link")