diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py index 733b78d8e7c..c8ed06c6d09 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py @@ -112,6 +112,14 @@ def create_parser(): "--moveit_config_pkg", help="The robot moveit_config package. Defaults to _moveit_config", ) + parser.add_argument( + "--eef_direction", + type=float, + nargs=3, + metavar=("X", "Y", "Z"), + default=[0, 0, 1], + help="The end effector's direction vector defined in its own frame, which is used to generate necessary parameters to a IKFast solver of one of the following types: Direction3D, Ray4D, TranslationDirection5D, Translation*AxisAngle4D, and Translation*AxisAngle*Norm4D. When not specified, a unit-z vector, i.e. 0 0 1, is adopted as default", + ) return parser @@ -136,6 +144,9 @@ def print_args(args): print(f" srdf_filename: {args.srdf_filename}") print(f" robot_name_in_srdf: {args.robot_name_in_srdf}") print(f" moveit_config_pkg: {args.moveit_config_pkg}") + print( + f" eef_direction: {args.eef_direction[0]:g} {args.eef_direction[1]:g} {args.eef_direction[2]:g}" + ) print("") @@ -262,6 +273,7 @@ def update_ikfast_package(args): _BASE_LINK_=args.base_link_name, _PACKAGE_NAME_=args.ikfast_plugin_pkg, _NAMESPACE_=args.namespace, + _EEF_DIRECTION_=f"{args.eef_direction[0]:g}, {args.eef_direction[1]:g}, {args.eef_direction[2]:g}", ) # Copy ikfast header file @@ -392,12 +404,16 @@ def update_ikfast_package(args): + "\n" + "ikfast_output_path=" + args.ikfast_output_path + + "\n" + + "eef_direction=" + + f'"{args.eef_direction[0]:g} {args.eef_direction[1]:g} {args.eef_direction[2]:g}"' + "\n\n" + "rosrun moveit_kinematics create_ikfast_moveit_plugin.py\\\n" + " --search_mode=$search_mode\\\n" + " --srdf_filename=$srdf_filename\\\n" + " --robot_name_in_srdf=$robot_name_in_srdf\\\n" + " --moveit_config_pkg=$moveit_config_pkg\\\n" + + " --eef_direction $eef_direction\\\n" + " $robot_name\\\n" + " $planning_group_name\\\n" + " $ikfast_plugin_pkg\\\n" diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index d1f03ef405b..51c5623d1d4 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -560,7 +560,9 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector(GetIkType()); + + switch (ik_type) { case IKP_Transform6D: case IKP_Translation3D: @@ -589,7 +591,10 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); return solutions.GetNumSolutions(); @@ -610,35 +615,53 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector 0 ? &vfree[0] : nullptr, solutions); - return solutions.GetNumSolutions(); - case IKP_TranslationYAxisAngle4D: - case IKP_TranslationYAxisAngleXNorm4D: - // For **TranslationYAxisAngleXNorm4D** - end effector origin reaches desired 3D translation, manipulator - // direction needs to be orthogonal to x axis and be rotated at a certain angle starting from the y axis (defined - // in the manipulator base link’s coordinate system) - pose_frame.M.GetRPY(roll, pitch, yaw); - ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); - return solutions.GetNumSolutions(); - case IKP_TranslationZAxisAngle4D: + // For *TranslationXAxisAngle4D*, *TranslationYAxisAngle4D*, *TranslationZAxisAngle4D* - end effector origin + // reaches desired 3D translation; manipulator direction makes a specific angle with x/y/z-axis (defined in the + // manipulator base link's coordinate system) + case IKP_TranslationXAxisAngleZNorm4D: + case IKP_TranslationYAxisAngleXNorm4D: case IKP_TranslationZAxisAngleYNorm4D: - // For **TranslationZAxisAngleYNorm4D** - end effector origin reaches desired 3D translation, manipulator - // direction needs to be orthogonal to y axis and be rotated at a certain angle starting from the z axis (defined - // in the manipulator base link’s coordinate system) - pose_frame.M.GetRPY(roll, pitch, yaw); - ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); - return solutions.GetNumSolutions(); + // For **TranslationXAxisAngleZNorm4D**, **TranslationYAxisAngleXNorm4D**, **TranslationZAxisAngleYNorm4D** - + // end effector origin reaches desired 3D translation; + // manipulator direction needs to be orthogonal to z/x/y-axis and be rotated at a certain angle + // starting from the x/y/z-axis (defined in the manipulator base link’s coordinate system) + { + double angle = 0; + direction = pose_frame.M * KDL::Vector(_EEF_DIRECTION_); + // Making sure the resulting "direction" has a unit length, as users might pass in an unnormalized input through + // the input argument "--eef_direction" + direction.Normalize(); + + switch (ik_type) // inner switch case + { + case IKP_TranslationXAxisAngle4D: + angle = std::acos(direction.x()); + break; + case IKP_TranslationYAxisAngle4D: + angle = std::acos(direction.y()); + break; + case IKP_TranslationZAxisAngle4D: + angle = std::acos(direction.z()); + break; + case IKP_TranslationXAxisAngleZNorm4D: + angle = std::atan2(direction.y(), direction.x()); + break; + case IKP_TranslationYAxisAngleXNorm4D: + angle = std::atan2(direction.z(), direction.y()); + break; + case IKP_TranslationZAxisAngleYNorm4D: + angle = std::atan2(direction.x(), direction.z()); + break; + default: + ROS_ERROR_STREAM_NAMED(name_, "An impossible case was reached with ik_type = " << ik_type); + return 0; + } + + ComputeIk(trans, &angle, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + } default: ROS_ERROR_NAMED(name_, "Unknown IkParameterizationType! " diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index e99ce099203..24c115db249 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/moveit_kinematics/test/CMakeLists.txt @@ -33,6 +33,24 @@ if(CATKIN_ENABLE_TESTING) add_rostest(panda-ikfast.test ${DEPS}) endif() + set(IK_TYPES translationxaxisangle4d + translationyaxisangle4d + translationzaxisangle4d + translationxaxisangleznorm4d + translationyaxisanglexnorm4d + translationzaxisangleynorm4d) + foreach(IK_TYPE IN LISTS IK_TYPES) + find_package(test_4dof_${IK_TYPE}_ikfast_plugin QUIET) + if (test_4dof_${IK_TYPE}_ikfast_plugin_FOUND) + # Setting a default value to IKFAST_PLUGIN_PATH for passing the catkin_lint test + set(IKFAST_PLUGIN_PATH "test_4dof") + exec_program(rospack ARGS find "test_4dof_${IK_TYPE}_ikfast_plugin" OUTPUT_VARIABLE IKFAST_PLUGIN_PATH RETURN_VALUE ROSPACK_RESULT) + if (NOT ROSPACK_RESULT) + add_rostest("${IKFAST_PLUGIN_PATH}/test_4dof-ikfast.test" ${DEPS}) + endif() + endif() + endforeach() + # Benchmarking program for cached_ik_kinematics add_executable(benchmark_ik benchmark_ik.cpp) target_link_libraries(benchmark_ik diff --git a/moveit_kinematics/test/test_4dof/packages.tgz b/moveit_kinematics/test/test_4dof/packages.tgz new file mode 100644 index 00000000000..5b89c295d4b Binary files /dev/null and b/moveit_kinematics/test/test_4dof/packages.tgz differ diff --git a/moveit_kinematics/test/test_4dof/test_4dof-ikfast.test b/moveit_kinematics/test/test_4dof/test_4dof-ikfast.test new file mode 100644 index 00000000000..a48776e1507 --- /dev/null +++ b/moveit_kinematics/test/test_4dof/test_4dof-ikfast.test @@ -0,0 +1,45 @@ + + + + + + + + + + + + [_JOINT_NAMES_] + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_kinematics/test/test_4dof/test_4dof.sh b/moveit_kinematics/test/test_4dof/test_4dof.sh new file mode 100755 index 00000000000..4c0cb99c1d8 --- /dev/null +++ b/moveit_kinematics/test/test_4dof/test_4dof.sh @@ -0,0 +1,149 @@ +#!/bin/bash + +set -e # fail script on error + +# Note: this script assumes that it is located at SCRIPT_FOLDER="[moveit_source_root]/moveit_kinematics/test/test_4dof", +# and all the model/testing packages generated by it will be placed at ${SCRIPT_FOLDER}/../../../test_4dof/src + +TMP_DIR=$(mktemp -d --tmpdir test_4dof.XXXXXX) +SCRIPT_FOLDER="$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" +LOG_FILE="${TMP_DIR}/test_4dof.log" + +OUTPUT_SRC_FOLDER="${SCRIPT_FOLDER}/../../../test_4dof/src" + +if [ -d "${OUTPUT_SRC_FOLDER}" ] +then + OLD_PACKAGES=$(ls -1 "${OUTPUT_SRC_FOLDER}") + if [ -n "${OLD_PACKAGES}" ] + then + echo "${OLD_PACKAGES}" | xargs -d '\n' catkin clean + fi +fi + +rm -rf ""$(dirname "${OUTPUT_SRC_FOLDER}")"" +mkdir -p "${OUTPUT_SRC_FOLDER}" +tar -zxf "${SCRIPT_FOLDER}/packages.tgz" -C "${OUTPUT_SRC_FOLDER}" + + +# Robot/IK configuration +PLANNING_GROUP="arm" +BASE_LINK="base" +EEF_LINK="link_3" +JOINT_NAMES="joint_base, joint_0, joint_1, joint_2" +IK_TYPES=("translationxaxisangle4d" + "translationyaxisangle4d" + "translationzaxisangle4d" + "translationxaxisangleznorm4d" + "translationyaxisanglexnorm4d" + "translationzaxisangleynorm4d") +ROBOT_NAMES=("test_4dof_xaxis" "test_4dof_yaxis" "test_4dof_zaxis" "test_4dof_xaxis" "test_4dof_yaxis" "test_4dof_zaxis") +EEF_DIRECTIONS=("1.0 0.0 0.0" "0.0 1.0 0.0" "0.0 0.0 1.0" "1.0 0.0 0.0" "0.0 1.0 0.0" "0.0 0.0 1.0") + +echo -e "\n\nNote: detailed messages to stdout are redirected to ${LOG_FILE}\n\n" + +echo "Building docker image" +cat <> "${LOG_FILE}" +FROM personalrobotics/ros-openrave +# Update ROS keys (https://discourse.ros.org/t/new-gpg-keys-deployed-for-packages-ros-org/9454) +RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && \ +apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 && \ +apt-get update && \ +apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \ +apt-get clean && rm -rf /var/lib/apt/lists/* +# enforce a specific version of sympy, which is known to work with OpenRave +#RUN python -v -m pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1 +RUN pip install https://github.com/sympy/sympy/archive/refs/tags/sympy-0.7.1.tar.gz +EOF + +CUR_DIR="${PWD}" +cd "${OUTPUT_SRC_FOLDER}" + +for INDEX in ${!IK_TYPES[@]} +do + IK_TYPE=${IK_TYPES[${INDEX}]} + ROBOT_NAME=${ROBOT_NAMES[${INDEX}]} + EEF_DIRECTION=${EEF_DIRECTIONS[${INDEX}]} + + + DOCKER_INPUT_BINDING="${OUTPUT_SRC_FOLDER}/${ROBOT_NAME}_description" + + if [ ! -f "${DOCKER_INPUT_BINDING}/${ROBOT_NAME}.dae" ] + then + # Running a container to convert the given urdf file to one in the dae format + CMD=(rosrun collada_urdf urdf_to_collada "/input/${ROBOT_NAME}.urdf" "/input/${ROBOT_NAME}.dae") + + docker run --rm --user $(id -u):$(id -g) -v ${TMP_DIR}:/workspace -v "${DOCKER_INPUT_BINDING}":/input \ + --workdir /workspace -e HOME=/workspace fixed-openrave:latest "${CMD[@]}" >> "${LOG_FILE}" + + fi + + # Producing a wrapper.xml describing the robot configuration + cat < "${DOCKER_INPUT_BINDING}/${IK_TYPE}_wrapper.xml" + + + ${BASE_LINK} + ${EEF_LINK} + ${EEF_DIRECTION} + + +EOF + + # Running a container to generate a IKFast solver cpp with the given ik type + CMD=(openrave0.9.py --database inversekinematics --robot "/input/${IK_TYPE}_wrapper.xml" --iktype "${IK_TYPE}" --iktests=1000) + echo "Running ${CMD[@]}" + + docker run --rm --user $(id -u):$(id -g) -v "${TMP_DIR}":/workspace -v "${DOCKER_INPUT_BINDING}":/input \ + --workdir /workspace -e HOME=/workspace fixed-openrave:latest "${CMD[@]}" >> "${LOG_FILE}" + + # the solver cpp, if having been generated successfully, will be located in $TMP_DIR/.openrave/*/ + CPP_FILE=$(ls -1 ${TMP_DIR}/.openrave/*/*.cpp 2> /dev/null) + if [ -z "${CPP_FILE}" ] ; then + echo "Failed to create an ikfast solver for iktype = ${IK_TYPE}" + continue + fi + + + # Note that the robot name given to create_ikfast_moveit_plugin.py is deliberately changed to $test_4dof_${IK_TYPE} + # for the purpose of creating a unique namespace for each plugin referencing the same robot definition but with a different ik solver; + # the real robot name is still available through the input argument --robot_name_in_srdf + + PACKAGE_NAME="test_4dof_${IK_TYPE}_ikfast_plugin" + CMD=$(cat <> "${LOG_FILE}" + + # Removing the current openrave data, so that it will not interfere with the next run of solver generation + rm -rf "${TMP_DIR}/.openrave" + + # Generating a test case file "test_4dof-ikfast.test" for this iktype + CMD=$(cat < "./${PACKAGE_NAME}/test_4dof-ikfast.test" +EOF + ) + eval "${CMD}" + +done + +cd "${CUR_DIR}" +# Building all generated packages, including model/config ones that are extracted from the prepared tar file "packages.tgz" +ls -1 "${OUTPUT_SRC_FOLDER}" | xargs -d '\n' catkin build --no-status --no-summary --no-deps diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 7f66340e71f..72719a43c2f 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -92,6 +92,8 @@ class SharedData int num_ik_tests_; int num_ik_multiple_tests_; int num_nearest_ik_tests_; + bool plugin_fk_support_; + bool position_only_check_; SharedData(SharedData const&) = delete; // this is a singleton SharedData() @@ -133,6 +135,12 @@ class SharedData ASSERT_TRUE(robot_model_->hasJointModelGroup(group_name_)); ASSERT_TRUE(robot_model_->hasLinkModel(root_link_)); ASSERT_TRUE(robot_model_->hasLinkModel(tip_link_)); + + if (!getParam("plugin_fk_support", plugin_fk_support_)) + plugin_fk_support_ = true; + + if (!getParam("position_only_check", position_only_check_)) + position_only_check_ = false; } public: @@ -173,6 +181,8 @@ class KinematicsTest : public ::testing::Test num_ik_tests_ = data.num_ik_tests_; num_ik_multiple_tests_ = data.num_ik_multiple_tests_; num_nearest_ik_tests_ = data.num_nearest_ik_tests_; + plugin_fk_support_ = data.plugin_fk_support_; + position_only_check_ = data.position_only_check_; } void SetUp() override @@ -252,20 +262,24 @@ class KinematicsTest : public ::testing::Test val2[i].position, abs_error), GTEST_NONFATAL_FAILURE_); - ss.str(""); - ss << "[" << i << "].orientation"; - GTEST_ASSERT_(isNear((expr1 + ss.str()).c_str(), (expr2 + ss.str()).c_str(), abs_error_expr, val1[i].orientation, - val2[i].orientation, abs_error), - GTEST_NONFATAL_FAILURE_); + if (!position_only_check_) + { + ss.str(""); + ss << "[" << i << "].orientation"; + GTEST_ASSERT_(isNear((expr1 + ss.str()).c_str(), (expr2 + ss.str()).c_str(), abs_error_expr, + val1[i].orientation, val2[i].orientation, abs_error), + GTEST_NONFATAL_FAILURE_); + } } return testing::AssertionSuccess(); } - void searchIKCallback(const std::vector& joint_state, moveit_msgs::MoveItErrorCodes& error_code) + void searchIKCallback(const std::vector& joint_state, moveit_msgs::MoveItErrorCodes& error_code, + moveit::core::RobotState& robot_state) { std::vector link_names = { tip_link_ }; std::vector poses; - if (!kinematics_solver_->getPositionFK(link_names, joint_state, poses)) + if (!getPositionFK(link_names, joint_state, poses, robot_state)) { error_code.val = error_code.PLANNING_FAILED; return; @@ -278,6 +292,38 @@ class KinematicsTest : public ::testing::Test error_code.val = error_code.PLANNING_FAILED; } + bool getPositionFK(const std::vector& link_names, const std::vector& joint_state, + std::vector& poses, moveit::core::RobotState& robot_state) + { + // There are some cases, e.g. testing a IKFast plugin targeting a robot of dof < 6, where + // kinematics_solver_->getPositionFK() will always return false, and that will render the entire test process + // useless as every test is doomed to fail due to calls to getPositionFK(). + // + // When plugin_fk_support_ is set to false, the FK pass will be done by updating robot_state with the given joints + // and calling moveit::core::RobotState::getGlobalLinkTransform(); therefore the lack of support in + // kinematics_solver_->getPositionFK() is circumvented, and tests for IK functionality can still be run to perform + // meaningful checks + + if (plugin_fk_support_) + return kinematics_solver_->getPositionFK(link_names, joint_state, poses); + else + { + std::vector joint_state_backup; + robot_state.copyJointGroupPositions(jmg_, joint_state_backup); + robot_state.setJointGroupPositions(jmg_, joint_state); + robot_state.updateLinkTransforms(); + + poses.clear(); + poses.reserve(link_names.size()); + for (const std::string& link_name : link_names) + poses.emplace_back(tf2::toMsg(robot_state.getGlobalLinkTransform(link_name))); + + robot_state.setJointGroupPositions(jmg_, joint_state_backup); + robot_state.updateLinkTransforms(); + return true; + } + } + public: moveit::core::RobotModelPtr robot_model_; moveit::core::JointModelGroup* jmg_; @@ -296,6 +342,8 @@ class KinematicsTest : public ::testing::Test unsigned int num_ik_tests_; unsigned int num_ik_multiple_tests_; unsigned int num_nearest_ik_tests_; + bool plugin_fk_support_; + bool position_only_check_; }; #define EXPECT_NEAR_POSES(lhs, rhs, near) \ @@ -314,7 +362,7 @@ TEST_F(KinematicsTest, getFK) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, joints); std::vector fk_poses; - EXPECT_TRUE(kinematics_solver_->getPositionFK(tip_frames, joints, fk_poses)); + EXPECT_TRUE(getPositionFK(tip_frames, joints, fk_poses, robot_state)); robot_state.updateLinkTransforms(); std::vector model_poses; @@ -357,7 +405,7 @@ TEST_F(KinematicsTest, randomWalkIK) robot_state.copyJointGroupPositions(jmg_, goal); // compute target tip_frames std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(tip_frames, goal, poses)); + ASSERT_TRUE(getPositionFK(tip_frames, goal, poses, robot_state)); // compute IK moveit_msgs::MoveItErrorCodes error_code; @@ -370,16 +418,21 @@ TEST_F(KinematicsTest, randomWalkIK) // on success: validate reached poses std::vector reached_poses; - kinematics_solver_->getPositionFK(tip_frames, solution, reached_poses); + getPositionFK(tip_frames, solution, reached_poses, robot_state); EXPECT_NEAR_POSES(poses, reached_poses, tolerance_); - // validate closeness of solution pose to goal - auto diff = Eigen::Map(solution.data(), solution.size()) - - Eigen::Map(goal.data(), goal.size()); - if (!diff.isZero(1.05 * NEAR_JOINT)) + // The following joint state check is skipped when position_only_check_ is true, + // because matching two sets of joint states would imply fully matching the two corresponding poses + if (!position_only_check_) { - ++failures; - ROS_WARN_STREAM("jump in [" << i << "]: " << diff.transpose()); + // validate closeness of solution pose to goal + auto diff = Eigen::Map(solution.data(), solution.size()) - + Eigen::Map(goal.data(), goal.size()); + if (!diff.isZero(1.05 * NEAR_JOINT)) + { + ++failures; + ROS_WARN_STREAM("jump in [" << i << "]: " << diff.transpose()); + } } // update robot state to found pose @@ -475,7 +528,7 @@ TEST_F(KinematicsTest, unitIK) // compute initial end-effector pose std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(tip_frames, seed, poses)); + ASSERT_TRUE(getPositionFK(tip_frames, seed, poses, robot_state)); Eigen::Isometry3d initial, goal; tf2::fromMsg(poses[0], initial); @@ -488,7 +541,7 @@ TEST_F(KinematicsTest, unitIK) // validate reached poses std::vector reached_poses; - kinematics_solver_->getPositionFK(tip_frames, sol, reached_poses); + getPositionFK(tip_frames, sol, reached_poses, robot_state); EXPECT_NEAR_POSES({ goal }, reached_poses, tolerance_); // validate ground truth @@ -549,7 +602,7 @@ TEST_F(KinematicsTest, searchIK) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); kinematics_solver_->searchPositionIK(poses[0], seed, timeout_, solution, error_code); if (error_code.val == error_code.SUCCESS) @@ -558,7 +611,7 @@ TEST_F(KinematicsTest, searchIK) continue; std::vector reached_poses; - kinematics_solver_->getPositionFK(fk_names, solution, reached_poses); + getPositionFK(fk_names, solution, reached_poses, robot_state); EXPECT_NEAR_POSES(poses, reached_poses, tolerance_); } @@ -583,7 +636,7 @@ TEST_F(KinematicsTest, searchIKWithCallback) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); if (poses[0].position.z <= 0.0f) { --i; // draw a new random state @@ -592,8 +645,10 @@ TEST_F(KinematicsTest, searchIKWithCallback) kinematics_solver_->searchPositionIK( poses[0], fk_values, timeout_, solution, - [this](const geometry_msgs::Pose& /*unused*/, const std::vector& joints, - moveit_msgs::MoveItErrorCodes& error_code) { searchIKCallback(joints, error_code); }, + [this, &robot_state](const geometry_msgs::Pose& /*unused*/, const std::vector& joints, + moveit_msgs::MoveItErrorCodes& error_code) { + searchIKCallback(joints, error_code, robot_state); + }, error_code); if (error_code.val == error_code.SUCCESS) success++; @@ -601,7 +656,7 @@ TEST_F(KinematicsTest, searchIKWithCallback) continue; std::vector reached_poses; - kinematics_solver_->getPositionFK(fk_names, solution, reached_poses); + getPositionFK(fk_names, solution, reached_poses, robot_state); EXPECT_NEAR_POSES(poses, reached_poses, tolerance_); } @@ -623,16 +678,27 @@ TEST_F(KinematicsTest, getIK) fk_values.resize(kinematics_solver_->getJointNames().size(), 0.0); robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); - std::vector poses; + std::vector poses, poses_from_ik; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); kinematics_solver_->getPositionIK(poses[0], fk_values, solution, error_code); - // starting from the correct solution, should yield the same pose EXPECT_EQ(error_code.val, error_code.SUCCESS); - Eigen::Map sol(solution.data(), solution.size()); - Eigen::Map truth(fk_values.data(), fk_values.size()); - EXPECT_TRUE(sol.isApprox(truth, tolerance_)) << sol.transpose() << std::endl << truth.transpose() << std::endl; + if (position_only_check_) + { + // The joint state check is skipped when position_only_check_ is true, + // because matching two sets of joint states would imply fully matching the two corresponding poses. + // Instead we only check if the derived position from the solution is close enough to the original one. + ASSERT_TRUE(getPositionFK(fk_names, solution, poses_from_ik, robot_state)); + EXPECT_NEAR_POSES(poses, poses_from_ik, tolerance_); + } + else + { + // starting from the correct solution, should yield the same pose + Eigen::Map sol(solution.data(), solution.size()); + Eigen::Map truth(fk_values.data(), fk_values.size()); + EXPECT_TRUE(sol.isApprox(truth, tolerance_)) << sol.transpose() << std::endl << truth.transpose() << std::endl; + } } } @@ -655,7 +721,7 @@ TEST_F(KinematicsTest, getIKMultipleSolutions) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); solutions.clear(); kinematics_solver_->getPositionIK(poses, fk_values, solutions, result, options); @@ -668,7 +734,7 @@ TEST_F(KinematicsTest, getIKMultipleSolutions) std::vector reached_poses; for (const auto& s : solutions) { - kinematics_solver_->getPositionFK(fk_names, s, reached_poses); + getPositionFK(fk_names, s, reached_poses, robot_state); EXPECT_NEAR_POSES(poses, reached_poses, tolerance_); } } @@ -695,7 +761,7 @@ TEST_F(KinematicsTest, getNearestIKSolution) robot_state.setToRandomPositions(jmg_, this->rng_); robot_state.copyJointGroupPositions(jmg_, fk_values); std::vector poses; - ASSERT_TRUE(kinematics_solver_->getPositionFK(fk_names, fk_values, poses)); + ASSERT_TRUE(getPositionFK(fk_names, fk_values, poses, robot_state)); // sample seed vector robot_state.setToRandomPositions(jmg_, this->rng_);