35#include <gtest/gtest.h>
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>
62#define _USE_MATH_DEFINES
64static constexpr double EPSILON{ 1.0e-6 };
65static constexpr double IK_SEED_OFFSET{ 0.1 };
66static constexpr double L0{ 0.2604 };
67static constexpr double L1{ 0.3500 };
68static constexpr double L2{ 0.3070 };
69static constexpr double L3{ 0.0840 };
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);
95 rm_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(
node_);
97 ASSERT_TRUE(
bool(
robot_model_)) <<
"Failed to load robot model";
101 ASSERT_TRUE(
node_->has_parameter(
"planning_group"));
103 ASSERT_TRUE(
node_->has_parameter(
"group_tip_link"));
105 ASSERT_TRUE(
node_->has_parameter(
"tcp_link"));
107 ASSERT_TRUE(
node_->has_parameter(
"ik_fast_link"));
109 ASSERT_TRUE(
node_->has_parameter(
"random_test_number"));
135 bool tfNear(
const Eigen::Isometry3d& pose1,
const Eigen::Isometry3d& pose2,
double epsilon);
141 std::unique_ptr<robot_model_loader::RobotModelLoader>
rm_loader_;
157 for (std::size_t i = 0; i < 3; ++i)
159 for (std::size_t j = 0; j < 4; ++j)
161 if (fabs(pose1(i, j) - pose2(i, j)) > fabs(epsilon))
190 Eigen::Isometry3d tip_pose;
191 std::map<std::string, double> test_state = zero_state_;
193 EXPECT_NEAR(tip_pose(0, 3), 0,
EPSILON);
194 EXPECT_NEAR(tip_pose(1, 3), 0,
EPSILON);
195 EXPECT_NEAR(tip_pose(2, 3), L0 + L1 + L2 + L3,
EPSILON);
197 test_state[joint_names_.at(1)] = M_PI_2;
199 EXPECT_NEAR(tip_pose(0, 3), L1 + L2 + L3,
EPSILON);
200 EXPECT_NEAR(tip_pose(1, 3), 0,
EPSILON);
201 EXPECT_NEAR(tip_pose(2, 3), L0,
EPSILON);
203 test_state[joint_names_.at(1)] = -M_PI_2;
204 test_state[joint_names_.at(2)] = M_PI_2;
206 EXPECT_NEAR(tip_pose(0, 3), -L1,
EPSILON);
207 EXPECT_NEAR(tip_pose(1, 3), 0,
EPSILON);
208 EXPECT_NEAR(tip_pose(2, 3), L0 - L2 - L3,
EPSILON);
211 std::string link_name =
"wrong_link_name";
226 throw(
"No IK solver configured for group '" + planning_group_ +
"'");
231 while (random_test_number_ > 0)
236 geometry_msgs::msg::Pose pose_expect = tf2::toMsg(rstate.
getFrameTransform(ik_fast_link_));
239 std::vector<geometry_msgs::msg::Pose> ik_poses;
240 ik_poses.push_back(pose_expect);
241 std::vector<double> ik_seed, ik_expect, ik_actual;
255 std::vector<std::vector<double>> ik_solutions;
257 moveit_msgs::msg::MoveItErrorCodes err_code;
261 EXPECT_TRUE(solver->getPositionIK(ik_poses, ik_seed, ik_solutions, ik_result, options));
264 EXPECT_TRUE(solver->getPositionIK(pose_expect, ik_seed, ik_actual, err_code));
266 ASSERT_EQ(ik_expect.size(), ik_actual.size());
268 for (std::size_t i = 0; i < ik_expect.size(); ++i)
270 EXPECT_NEAR(ik_actual.at(i), ik_expect.at(i), 4 * IK_SEED_OFFSET);
273 --random_test_number_;
287 while (random_test_number_ > 0)
295 std::map<std::string, double> ik_seed, ik_expect;
296 for (
const auto& joint_name : joint_names_)
313 std::map<std::string, double> ik_actual;
315 EXPECT_TRUE(rstate.
setFromIK(robot_model_->getJointModelGroup(planning_group_), pose_expect, tcp_link_));
317 for (
const auto& joint_name : joint_names_)
323 for (
const auto& joint_pair : ik_actual)
325 EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET);
333 EXPECT_TRUE(tfNear(pose_expect, pose_actual,
EPSILON));
335 --random_test_number_;
348 const std::string frame_id = robot_model_->getModelFrame();
351 while (random_test_number_ > 0)
359 std::map<std::string, double> ik_seed, ik_expect;
360 for (
const auto& joint_name : robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames())
374 std::map<std::string, double> ik_actual;
376 frame_id, ik_seed, ik_actual,
false));
379 for (
const auto& joint_pair : ik_actual)
381 EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET);
384 --random_test_number_;
393 const std::string frame_id = robot_model_->getModelFrame();
394 Eigen::Isometry3d pose_expect;
396 std::map<std::string, double> ik_seed;
399 std::map<std::string, double> ik_actual;
401 pose_expect, frame_id, ik_seed, ik_actual,
false));
409 const std::string frame_id = robot_model_->getModelFrame();
410 Eigen::Isometry3d pose_expect;
412 std::map<std::string, double> ik_seed;
415 std::map<std::string, double> ik_actual;
417 frame_id, ik_seed, ik_actual,
false));
427 Eigen::Isometry3d pose_expect;
429 std::map<std::string, double> ik_seed;
432 std::map<std::string, double> ik_actual;
434 "InvalidFrameId", ik_seed, ik_actual,
false));
519 const std::string frame_id = robot_model_->getModelFrame();
523 std::map<std::string, double> ik_seed;
526 ik_seed[joint_name] = 0;
530 std::vector<double> ik_goal = { 0, 2.3, -2.3, 0, 0, 0 };
537 std::map<std::string, double> ik_actual;
539 frame_id, ik_seed, ik_actual,
false));
543 frame_id, ik_seed, ik_actual,
true));
558 const std::map<std::string, double> position_last, velocity_last, position_current;
559 double duration_last{ 0.0 };
562 double duration_current = 10e-7;
580 const std::string test_joint_name{
"joint" };
582 std::map<std::string, double> position_last{ { test_joint_name, 2.0 } };
583 std::map<std::string, double> position_current{ { test_joint_name, 10.0 } };
584 std::map<std::string, double> velocity_last;
585 double duration_current{ 1.0 };
586 double duration_last{ 0.0 };
593 ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current) - 1.0;
595 joint_limits.addLimit(test_joint_name, test_joint_limits);
613 const std::string test_joint_name{
"joint" };
615 double duration_current = 1.0;
616 double duration_last = 1.0;
618 std::map<std::string, double> position_last{ { test_joint_name, 2.0 } };
619 std::map<std::string, double> position_current{ { test_joint_name, 20.0 } };
620 double velocity_current =
621 ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current);
622 std::map<std::string, double> velocity_last{ { test_joint_name, 9.0 } };
628 test_joint_limits.
max_velocity = velocity_current + 1.0;
631 double acceleration_current =
632 (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2;
638 joint_limits.addLimit(test_joint_name, test_joint_limits);
656 const std::string test_joint_name{
"joint" };
658 double duration_current = 1.0;
659 double duration_last = 1.0;
661 std::map<std::string, double> position_last{ { test_joint_name, 20.0 } };
662 std::map<std::string, double> position_current{ { test_joint_name, 2.0 } };
663 double velocity_current =
664 ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current);
665 std::map<std::string, double> velocity_last{ { test_joint_name, 19.0 } };
671 test_joint_limits.
max_velocity = fabs(velocity_current) + 1.0;
674 double acceleration_current =
675 (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2;
681 joint_limits.addLimit(test_joint_name, test_joint_limits);
705 KDL::Path_RoundedComposite* path =
706 new KDL::Path_RoundedComposite(0.2, 0.01,
new KDL::RotationalInterpolation_SingleAxis());
707 path->Add(KDL::Frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(-1, 0, 0)));
710 KDL::VelocityProfile* vel_prof =
new KDL::VelocityProfile_Trap(0.5, 0.1);
711 vel_prof->SetProfile(0, path->PathLength());
712 KDL::Trajectory_Segment kdl_trajectory(path, vel_prof);
715 std::string group_name{
"invalid_group_name" };
716 std::map<std::string, double> initial_joint_position;
717 double sampling_time{ 0.1 };
718 trajectory_msgs::msg::JointTrajectory joint_trajectory;
719 moveit_msgs::msg::MoveItErrorCodes error_code;
720 bool check_self_collision{
false };
723 planning_scene_,
joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time,
724 joint_trajectory, error_code, check_self_collision));
726 std::map<std::string, double> initial_joint_velocity;
732 cart_traj.
points.push_back(cart_traj_point);
735 planning_scene_,
joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity,
736 joint_trajectory, error_code, check_self_collision));
752 robot_trajectory::RobotTrajectoryPtr first_trajectory =
753 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
754 robot_trajectory::RobotTrajectoryPtr second_trajectory =
755 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
756 double epsilon{ 0.0 };
757 double sampling_time{ 0.0 };
760 first_trajectory->insertWayPoint(0, rstate, 0.1);
761 second_trajectory->insertWayPoint(0, rstate, 0.1);
764 epsilon, sampling_time));
780 robot_trajectory::RobotTrajectoryPtr first_trajectory =
781 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
782 robot_trajectory::RobotTrajectoryPtr second_trajectory =
783 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
784 double epsilon{ 0.0001 };
785 double sampling_time{ 0.0 };
786 double expected_sampling_time{ 0.1 };
789 first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
790 first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
792 second_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
793 second_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
794 second_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
797 epsilon, sampling_time));
798 EXPECT_EQ(expected_sampling_time, sampling_time);
814 robot_trajectory::RobotTrajectoryPtr first_trajectory =
815 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
816 robot_trajectory::RobotTrajectoryPtr second_trajectory =
817 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
818 double epsilon{ 0.0001 };
819 double sampling_time{ 0.0 };
820 double expected_sampling_time{ 0.1 };
823 first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
824 first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
825 first_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
827 first_trajectory->insertWayPoint(2, rstate, expected_sampling_time + 1.0);
828 first_trajectory->insertWayPoint(3, rstate, expected_sampling_time);
830 second_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
831 second_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
832 second_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
833 second_trajectory->insertWayPoint(3, rstate, expected_sampling_time);
836 epsilon, sampling_time));
837 EXPECT_EQ(expected_sampling_time, sampling_time);
856 double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
859 default_joint_position[0] = default_joint_position[0] + 70.0;
862 double epsilon{ 0.0001 };
883 double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
887 double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
890 default_joint_velocity[1] = default_joint_velocity[1] + 10.0;
893 double epsilon{ 0.0001 };
914 double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
919 double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
923 double default_joint_acceleration[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
926 default_joint_acceleration[1] = default_joint_acceleration[1] + 10.0;
929 double epsilon{ 0.0001 };
949 double default_joint_velocity[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
952 double epsilon{ 0.0001 };
972 double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
976 double default_joint_acceleration[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
979 double epsilon{ 0.0001 };
985 rclcpp::init(argc, argv);
986 testing::InitGoogleTest(&argc, argv);
987 return RUN_ALL_TESTS();
std::unique_ptr< robot_model_loader::RobotModelLoader > rm_loader_
void SetUp() override
Create test scenario for trajectory functions.
std::string group_tip_link_
rclcpp::Node::SharedPtr node_
std::string planning_group_
random_numbers::RandomNumberGenerator rng_
std::string ik_fast_link_
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, double epsilon)
check if two transformations are close
std::vector< std::string > joint_names_
Parametrized class for tests with and without gripper.
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...
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
Representation of a robot's state. This includes position, velocity, acceleration and effort.
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...
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...
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...
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.
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...
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 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, 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 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)
bool has_acceleration_limits
A set of options for the kinematics solver.
std::vector< CartesianTrajectoryPoint > points
Extends joint_limits_interface::JointLimits with a deceleration parameter.
bool has_deceleration_limits
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")