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
64 static constexpr
double EPSILON{ 1.0e-6 };
65 static constexpr
double IK_SEED_OFFSET{ 0.1 };
66 static constexpr
double L0{ 0.2604 };
67 static constexpr
double L1{ 0.3500 };
68 static constexpr
double L2{ 0.3070 };
69 static 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,
const double& epsilon);
141 std::unique_ptr<robot_model_loader::RobotModelLoader>
rm_loader_;
156 const double& epsilon)
158 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))
189 Eigen::Isometry3d tip_pose;
190 std::map<std::string, double> test_state = zero_state_;
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);
196 test_state[joint_names_.at(1)] = M_PI_2;
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);
202 test_state[joint_names_.at(1)] = -M_PI_2;
203 test_state[joint_names_.at(2)] = M_PI_2;
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);
210 std::string link_name =
"wrong_link_name";
225 throw(
"No IK solver configured for group '" + planning_group_ +
"'");
230 while (random_test_number_ > 0)
235 geometry_msgs::msg::Pose pose_expect = tf2::toMsg(rstate.
getFrameTransform(ik_fast_link_));
238 std::vector<geometry_msgs::msg::Pose> ik_poses;
239 ik_poses.push_back(pose_expect);
240 std::vector<double> ik_seed, ik_expect, ik_actual;
250 std::vector<std::vector<double>> ik_solutions;
252 moveit_msgs::msg::MoveItErrorCodes err_code;
256 EXPECT_TRUE(solver->getPositionIK(ik_poses, ik_seed, ik_solutions, ik_result,
options));
259 EXPECT_TRUE(solver->getPositionIK(pose_expect, ik_seed, ik_actual, err_code));
261 ASSERT_EQ(ik_expect.size(), ik_actual.size());
263 for (std::size_t i = 0; i < ik_expect.size(); ++i)
265 EXPECT_NEAR(ik_actual.at(i), ik_expect.at(i), 4 * IK_SEED_OFFSET);
268 --random_test_number_;
282 while (random_test_number_ > 0)
290 std::map<std::string, double> ik_seed, ik_expect;
291 for (
const auto& joint_name : joint_names_)
304 std::map<std::string, double> ik_actual;
306 EXPECT_TRUE(rstate.
setFromIK(robot_model_->getJointModelGroup(planning_group_), pose_expect, tcp_link_));
308 for (
const auto& joint_name : joint_names_)
314 for (
const auto& joint_pair : ik_actual)
316 EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET);
324 EXPECT_TRUE(tfNear(pose_expect, pose_actual,
EPSILON));
326 --random_test_number_;
339 const std::string
frame_id = robot_model_->getModelFrame();
342 while (random_test_number_ > 0)
350 std::map<std::string, double> ik_seed, ik_expect;
351 for (
const auto& joint_name : robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames())
365 std::map<std::string, double> ik_actual;
367 frame_id, ik_seed, ik_actual,
false));
370 for (
const auto& joint_pair : ik_actual)
372 EXPECT_NEAR(joint_pair.second, ik_expect.at(joint_pair.first), 4 * IK_SEED_OFFSET);
375 --random_test_number_;
384 const std::string
frame_id = robot_model_->getModelFrame();
385 Eigen::Isometry3d pose_expect;
387 std::map<std::string, double> ik_seed;
390 std::map<std::string, double> ik_actual;
392 pose_expect,
frame_id, ik_seed, ik_actual,
false));
400 const std::string
frame_id = robot_model_->getModelFrame();
401 Eigen::Isometry3d pose_expect;
403 std::map<std::string, double> ik_seed;
406 std::map<std::string, double> ik_actual;
408 frame_id, ik_seed, ik_actual,
false));
418 Eigen::Isometry3d pose_expect;
420 std::map<std::string, double> ik_seed;
423 std::map<std::string, double> ik_actual;
425 "InvalidFrameId", ik_seed, ik_actual,
false));
510 const std::string
frame_id = robot_model_->getModelFrame();
514 std::map<std::string, double> ik_seed;
517 ik_seed[joint_name] = 0;
521 std::vector<double> ik_goal = { 0, 2.3, -2.3, 0, 0, 0 };
528 std::map<std::string, double> ik_actual;
530 frame_id, ik_seed, ik_actual,
false));
534 frame_id, ik_seed, ik_actual,
true));
549 const std::map<std::string, double> position_last, velocity_last, position_current;
550 double duration_last{ 0.0 };
553 double duration_current = 10e-7;
571 const std::string test_joint_name{
"joint" };
573 std::map<std::string, double> position_last{ { test_joint_name, 2.0 } };
574 std::map<std::string, double> position_current{ { test_joint_name, 10.0 } };
575 std::map<std::string, double> velocity_last;
576 double duration_current{ 1.0 };
577 double duration_last{ 0.0 };
584 ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current) - 1.0;
586 joint_limits.addLimit(test_joint_name, test_joint_limits);
604 const std::string test_joint_name{
"joint" };
606 double duration_current = 1.0;
607 double duration_last = 1.0;
609 std::map<std::string, double> position_last{ { test_joint_name, 2.0 } };
610 std::map<std::string, double> position_current{ { test_joint_name, 20.0 } };
611 double velocity_current =
612 ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current);
613 std::map<std::string, double> velocity_last{ { test_joint_name, 9.0 } };
619 test_joint_limits.
max_velocity = velocity_current + 1.0;
622 double acceleration_current =
623 (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2;
629 joint_limits.addLimit(test_joint_name, test_joint_limits);
647 const std::string test_joint_name{
"joint" };
649 double duration_current = 1.0;
650 double duration_last = 1.0;
652 std::map<std::string, double> position_last{ { test_joint_name, 20.0 } };
653 std::map<std::string, double> position_current{ { test_joint_name, 2.0 } };
654 double velocity_current =
655 ((position_current.at(test_joint_name) - position_last.at(test_joint_name)) / duration_current);
656 std::map<std::string, double> velocity_last{ { test_joint_name, 19.0 } };
662 test_joint_limits.
max_velocity = fabs(velocity_current) + 1.0;
665 double acceleration_current =
666 (velocity_current - velocity_last.at(test_joint_name)) / (duration_last + duration_current) * 2;
672 joint_limits.addLimit(test_joint_name, test_joint_limits);
696 KDL::Path_RoundedComposite* path =
697 new KDL::Path_RoundedComposite(0.2, 0.01,
new KDL::RotationalInterpolation_SingleAxis());
698 path->Add(KDL::Frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(-1, 0, 0)));
701 KDL::VelocityProfile* vel_prof =
new KDL::VelocityProfile_Trap(0.5, 0.1);
702 vel_prof->SetProfile(0, path->PathLength());
703 KDL::Trajectory_Segment kdl_trajectory(path, vel_prof);
706 std::string group_name{
"invalid_group_name" };
707 std::map<std::string, double> initial_joint_position;
708 double sampling_time{ 0.1 };
709 trajectory_msgs::msg::JointTrajectory joint_trajectory;
710 moveit_msgs::msg::MoveItErrorCodes error_code;
711 bool check_self_collision{
false };
714 planning_scene_,
joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time,
715 joint_trajectory, error_code, check_self_collision));
717 std::map<std::string, double> initial_joint_velocity;
723 cart_traj.
points.push_back(cart_traj_point);
726 planning_scene_,
joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity,
727 joint_trajectory, error_code, check_self_collision));
743 robot_trajectory::RobotTrajectoryPtr first_trajectory =
744 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
745 robot_trajectory::RobotTrajectoryPtr second_trajectory =
746 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
747 double epsilon{ 0.0 };
748 double sampling_time{ 0.0 };
751 first_trajectory->insertWayPoint(0, rstate, 0.1);
752 second_trajectory->insertWayPoint(0, rstate, 0.1);
755 epsilon, sampling_time));
771 robot_trajectory::RobotTrajectoryPtr first_trajectory =
772 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
773 robot_trajectory::RobotTrajectoryPtr second_trajectory =
774 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
775 double epsilon{ 0.0001 };
776 double sampling_time{ 0.0 };
777 double expected_sampling_time{ 0.1 };
780 first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
781 first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
783 second_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
784 second_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
785 second_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
788 epsilon, sampling_time));
789 EXPECT_EQ(expected_sampling_time, sampling_time);
805 robot_trajectory::RobotTrajectoryPtr first_trajectory =
806 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
807 robot_trajectory::RobotTrajectoryPtr second_trajectory =
808 std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
809 double epsilon{ 0.0001 };
810 double sampling_time{ 0.0 };
811 double expected_sampling_time{ 0.1 };
814 first_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
815 first_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
816 first_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
818 first_trajectory->insertWayPoint(2, rstate, expected_sampling_time + 1.0);
819 first_trajectory->insertWayPoint(3, rstate, expected_sampling_time);
821 second_trajectory->insertWayPoint(0, rstate, expected_sampling_time);
822 second_trajectory->insertWayPoint(1, rstate, expected_sampling_time);
823 second_trajectory->insertWayPoint(2, rstate, expected_sampling_time);
824 second_trajectory->insertWayPoint(3, rstate, expected_sampling_time);
827 epsilon, sampling_time));
828 EXPECT_EQ(expected_sampling_time, sampling_time);
847 double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
850 default_joint_position[0] = default_joint_position[0] + 70.0;
853 double epsilon{ 0.0001 };
874 double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
878 double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
881 default_joint_velocity[1] = default_joint_velocity[1] + 10.0;
884 double epsilon{ 0.0001 };
905 double default_joint_position[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
910 double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
914 double default_joint_acceleration[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
917 default_joint_acceleration[1] = default_joint_acceleration[1] + 10.0;
920 double epsilon{ 0.0001 };
940 double default_joint_velocity[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
943 double epsilon{ 0.0001 };
963 double default_joint_velocity[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
967 double default_joint_acceleration[6] = { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
970 double epsilon{ 0.0001 };
974 int main(
int argc,
char** argv)
976 rclcpp::init(argc, argv);
977 testing::InitGoogleTest(&argc, argv);
978 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, const double &epsilon)
check if two transformations are close
std::vector< std::string > joint_names_
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.
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 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)
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")