From 9256b9dc7a3b7a300e5cbd16f9e613e36ce6d684 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 21 Aug 2023 11:47:14 +0200 Subject: [PATCH] Add support for KDL::ChainIkSolverPos_NR_JL, which takes joint limits into account --- tesseract_kinematics/CMakeLists.txt | 12 +- tesseract_kinematics/kdl/CMakeLists.txt | 1 + .../tesseract_kinematics/kdl/kdl_factories.h | 9 + .../kdl/kdl_inv_kin_chain_nr_jl.h | 120 +++++++++++++ .../tesseract_kinematics/kdl/kdl_utils.h | 11 ++ .../kdl/src/kdl_factories.cpp | 33 ++++ .../kdl/src/kdl_inv_kin_chain_nr_jl.cpp | 164 ++++++++++++++++++ tesseract_kinematics/kdl/src/kdl_utils.cpp | 33 ++++ .../test/kdl_kinematics_unit.cpp | 11 ++ .../test/kinematic_plugins.yaml | 5 + .../test/kinematics_factory_unit.cpp | 39 +++++ 11 files changed, 432 insertions(+), 6 deletions(-) create mode 100644 tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h create mode 100644 tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp diff --git a/tesseract_kinematics/CMakeLists.txt b/tesseract_kinematics/CMakeLists.txt index 9eea6459686..9b930ec69c7 100644 --- a/tesseract_kinematics/CMakeLists.txt +++ b/tesseract_kinematics/CMakeLists.txt @@ -92,15 +92,15 @@ string( target_compile_definitions(${PROJECT_NAME}_core PRIVATE TESSERACT_KINEMATICS_PLUGINS="${KINEMATICS_PLUGINS_STRING}") # Testing -if((TESSERACT_ENABLE_TESTING OR TESSERACT_KINEMATICS_ENABLE_TESTING) - AND TESSERACT_BUILD_IKFAST - AND TESSERACT_BUILD_KDL - AND TESSERACT_BUILD_OPW - AND TESSERACT_BUILD_UR) +# if((TESSERACT_ENABLE_TESTING OR TESSERACT_KINEMATICS_ENABLE_TESTING) +# AND TESSERACT_BUILD_IKFAST +# AND TESSERACT_BUILD_KDL +# AND TESSERACT_BUILD_OPW +# AND TESSERACT_BUILD_UR) enable_testing() add_run_tests_target(ENABLE ${TESSERACT_ENABLE_RUN_TESTING}) add_subdirectory(test) -endif() +# endif() configure_package(NAMESPACE tesseract) diff --git a/tesseract_kinematics/kdl/CMakeLists.txt b/tesseract_kinematics/kdl/CMakeLists.txt index 68638ad393e..76c0ed5084b 100644 --- a/tesseract_kinematics/kdl/CMakeLists.txt +++ b/tesseract_kinematics/kdl/CMakeLists.txt @@ -5,6 +5,7 @@ add_library( src/kdl_fwd_kin_chain.cpp src/kdl_inv_kin_chain_lma.cpp src/kdl_inv_kin_chain_nr.cpp + src/kdl_inv_kin_chain_nr_jl.cpp src/kdl_utils.cpp) target_link_libraries( ${PROJECT_NAME}_kdl diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h index 515fd1f0b50..0ef213c6bfc 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h @@ -57,6 +57,15 @@ class KDLInvKinChainNRFactory : public InvKinFactory const YAML::Node& config) const override final; }; +class KDLInvKinChainNR_JLFactory : public InvKinFactory +{ + InverseKinematics::UPtr create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& scene_state, + const KinematicsPluginFactory& plugin_factory, + const YAML::Node& config) const override final; +}; + TESSERACT_PLUGIN_ANCHOR_DECL(KDLFactoriesAnchor) } // namespace tesseract_kinematics diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h new file mode 100644 index 00000000000..b7cb02326bf --- /dev/null +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h @@ -0,0 +1,120 @@ +/** + * @file kdl_fwd_kin_chain_nr_jl.h + * @brief Tesseract KDL inverse kinematics chain Newton-Raphson implementation. + * + * @author Levi Armstrong, Roelof Oomen + * @date July 26, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_JL_H +#define TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_JL_H +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace tesseract_kinematics +{ +static const std::string KDL_INV_KIN_CHAIN_NR_JL_SOLVER_NAME = "KDLInvKinChainNR_JL"; + +/** + * @brief KDL Inverse kinematic chain implementation. + */ +class KDLInvKinChainNR_JL : public InverseKinematics +{ +public: + // LCOV_EXCL_START + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // LCOV_EXCL_STOP + + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using UPtr = std::unique_ptr; + using ConstUPtr = std::unique_ptr; + + ~KDLInvKinChainNR_JL() override = default; + KDLInvKinChainNR_JL(const KDLInvKinChainNR_JL& other); + KDLInvKinChainNR_JL& operator=(const KDLInvKinChainNR_JL& other); + KDLInvKinChainNR_JL(KDLInvKinChainNR_JL&&) = delete; + KDLInvKinChainNR_JL& operator=(KDLInvKinChainNR_JL&&) = delete; + + /** + * @brief Construct KDL Forward Kinematics + * Creates KDL::Chain from tesseract scene graph + * @param scene_graph The Tesseract Scene Graph + * @param base_link The name of the base link for the kinematic chain + * @param tip_link The name of the tip link for the kinematic chain + * @param solver_name The solver name of the kinematic chain + */ + KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph, + const std::string& base_link, + const std::string& tip_link, + std::string solver_name = KDL_INV_KIN_CHAIN_NR_JL_SOLVER_NAME); + + /** + * @brief Construct Inverse Kinematics as chain + * Creates a inverse kinematic chain object from sequential chains + * @param scene_graph The Tesseract Scene Graph + * @param chains A vector of kinematics chains that get concatenated + * @param solver_name The solver name of the kinematic chain + */ + KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph, + const std::vector >& chains, + std::string solver_name = KDL_INV_KIN_CHAIN_NR_JL_SOLVER_NAME); + + IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses, + const Eigen::Ref& seed) const override final; + + std::vector getJointNames() const override final; + Eigen::Index numJoints() const override final; + std::string getBaseLinkName() const override final; + std::string getWorkingFrame() const override final; + std::vector getTipLinkNames() const override final; + std::string getSolverName() const override final; + InverseKinematics::UPtr clone() const override final; + +private: + KDL::JntArray q_min_; /**< @brief Minimum joint positions */ + KDL::JntArray q_max_; /**< @brief Maximum joint positions */ + KDLChainData kdl_data_; /**< @brief KDL data parsed from Scene Graph */ + std::unique_ptr fk_solver_; /**< @brief KDL Forward Kinematic Solver */ + std::unique_ptr ik_vel_solver_; /**< @brief KDL Inverse kinematic velocity solver */ + std::unique_ptr ik_solver_; /**< @brief KDL Inverse kinematic solver */ + std::string solver_name_{ KDL_INV_KIN_CHAIN_NR_JL_SOLVER_NAME }; /**< @brief Name of this solver */ + mutable std::mutex mutex_; /**< @brief KDL is not thread safe due to mutable variables in Joint Class */ + + /** @brief calcFwdKin helper function */ + IKSolutions calcInvKinHelper(const Eigen::Isometry3d& pose, + const Eigen::Ref& seed, + int segment_num = -1) const; +}; + +} // namespace tesseract_kinematics +#endif // TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_JL_H diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h index 3b161b1a9ab..1c49b70c06f 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h @@ -126,5 +126,16 @@ bool parseSceneGraph(KDLChainData& results, const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_name, const std::string& tip_name); + +/** + * @brief Parse joint limits from the KDL chain data + * @param lb Lower bounds (limits) of the joints + * @param ub Upper bounds (limits) of the joints + * @param scene_graph The Scene Graph + */ +void parseChainData(KDL::JntArray& q_min, + KDL::JntArray& q_max, + const KDLChainData& kdl_data, + const tesseract_scene_graph::SceneGraph& scene_graph); } // namespace tesseract_kinematics #endif // TESSERACT_KINEMATICS_KDL_UTILS_H diff --git a/tesseract_kinematics/kdl/src/kdl_factories.cpp b/tesseract_kinematics/kdl/src/kdl_factories.cpp index e29f89c3409..fa2db4be07d 100644 --- a/tesseract_kinematics/kdl/src/kdl_factories.cpp +++ b/tesseract_kinematics/kdl/src/kdl_factories.cpp @@ -28,6 +28,7 @@ #include #include #include +#include namespace tesseract_kinematics { @@ -121,6 +122,36 @@ InverseKinematics::UPtr KDLInvKinChainNRFactory::create(const std::string& solve return std::make_unique(scene_graph, base_link, tip_link, solver_name); } +InverseKinematics::UPtr KDLInvKinChainNR_JLFactory::create(const std::string& solver_name, + const tesseract_scene_graph::SceneGraph& scene_graph, + const tesseract_scene_graph::SceneState& /*scene_state*/, + const KinematicsPluginFactory& /*plugin_factory*/, + const YAML::Node& config) const +{ + std::string base_link; + std::string tip_link; + + try + { + if (YAML::Node n = config["base_link"]) + base_link = n.as(); + else + throw std::runtime_error("KDLInvKinChainNR_JLFactory, missing 'base_link' entry"); + + if (YAML::Node n = config["tip_link"]) + tip_link = n.as(); + else + throw std::runtime_error("KDLInvKinChainNR_JLFactory, missing 'tip_link' entry"); + } + catch (const std::exception& e) + { + CONSOLE_BRIDGE_logError("KDLInvKinChainNR_JLFactory: Failed to parse yaml config data! Details: %s", e.what()); + return nullptr; + } + + return std::make_unique(scene_graph, base_link, tip_link, solver_name); +} + TESSERACT_PLUGIN_ANCHOR_IMPL(KDLFactoriesAnchor) } // namespace tesseract_kinematics @@ -131,3 +162,5 @@ TESSERACT_ADD_FWD_KIN_PLUGIN(tesseract_kinematics::KDLFwdKinChainFactory, KDLFwd TESSERACT_ADD_INV_KIN_PLUGIN(tesseract_kinematics::KDLInvKinChainLMAFactory, KDLInvKinChainLMAFactory); // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) TESSERACT_ADD_INV_KIN_PLUGIN(tesseract_kinematics::KDLInvKinChainNRFactory, KDLInvKinChainNRFactory); +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +TESSERACT_ADD_INV_KIN_PLUGIN(tesseract_kinematics::KDLInvKinChainNR_JLFactory, KDLInvKinChainNR_JLFactory); diff --git a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp new file mode 100644 index 00000000000..bfe08ec7921 --- /dev/null +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp @@ -0,0 +1,164 @@ +/** + * @file kdl_fwd_kin_chain_nr_jl.cpp + * @brief Tesseract KDL inverse kinematics chain Newton-Raphson implementation. + * + * @author Levi Armstrong, Roelof Oomen + * @date July 26, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace tesseract_kinematics +{ +using Eigen::MatrixXd; +using Eigen::VectorXd; + +KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph, + const std::vector>& chains, + std::string solver_name) + : solver_name_(std::move(solver_name)) +{ + if (!scene_graph.getLink(scene_graph.getRoot())) + throw std::runtime_error("The scene graph has an invalid root."); + + if (!parseSceneGraph(kdl_data_, scene_graph, chains)) + throw std::runtime_error("Failed to parse KDL data from Scene Graph"); + + parseChainData(q_min_, q_max_, kdl_data_, scene_graph); + + // Create KDL FK and IK Solver + fk_solver_ = std::make_unique(kdl_data_.robot_chain); + ik_vel_solver_ = std::make_unique(kdl_data_.robot_chain); + ik_solver_ = std::make_unique(kdl_data_.robot_chain, q_min_, q_max_, *fk_solver_, *ik_vel_solver_); +} + +KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph, + const std::string& base_link, + const std::string& tip_link, + std::string solver_name) + : KDLInvKinChainNR_JL(scene_graph, { std::make_pair(base_link, tip_link) }, std::move(solver_name)) +{ +} + +InverseKinematics::UPtr KDLInvKinChainNR_JL::clone() const { return std::make_unique(*this); } + +KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const KDLInvKinChainNR_JL& other) { *this = other; } + +KDLInvKinChainNR_JL& KDLInvKinChainNR_JL::operator=(const KDLInvKinChainNR_JL& other) +{ + q_min_ = other.q_min_; + q_max_ = other.q_max_; + kdl_data_ = other.kdl_data_; + fk_solver_ = std::make_unique(kdl_data_.robot_chain); + ik_vel_solver_ = std::make_unique(kdl_data_.robot_chain); + ik_solver_ = std::make_unique(kdl_data_.robot_chain, q_min_, q_max_, *fk_solver_, *ik_vel_solver_); + solver_name_ = other.solver_name_; + + return *this; +} + +IKSolutions KDLInvKinChainNR_JL::calcInvKinHelper(const Eigen::Isometry3d& pose, + const Eigen::Ref& seed, + int /*segment_num*/) const +{ + assert(std::abs(1.0 - pose.matrix().determinant()) < 1e-6); // NOLINT + KDL::JntArray kdl_seed, kdl_solution; + EigenToKDL(seed, kdl_seed); + kdl_solution.resize(static_cast(seed.size())); + Eigen::VectorXd solution(seed.size()); + + // run IK solver + // TODO: Need to update to handle seg number. Need to create an IK solver for each seg. + KDL::Frame kdl_pose; + EigenToKDL(pose, kdl_pose); + int status{ -1 }; + { + std::lock_guard guard(mutex_); + status = ik_solver_->CartToJnt(kdl_seed, kdl_pose, kdl_solution); + } + + if (status < 0) + { + // LCOV_EXCL_START + if (status == KDL::ChainIkSolverPos_NR_JL::E_DEGRADED) + { + CONSOLE_BRIDGE_logDebug("KDL NR Failed to calculate IK, solution converged to & seed) const +{ + assert(tip_link_poses.find(kdl_data_.tip_link_name) != tip_link_poses.end()); + return calcInvKinHelper(tip_link_poses.at(kdl_data_.tip_link_name), seed); +} + +std::vector KDLInvKinChainNR_JL::getJointNames() const { return kdl_data_.joint_names; } + +Eigen::Index KDLInvKinChainNR_JL::numJoints() const { return kdl_data_.robot_chain.getNrOfJoints(); } + +std::string KDLInvKinChainNR_JL::getBaseLinkName() const { return kdl_data_.base_link_name; } + +std::string KDLInvKinChainNR_JL::getWorkingFrame() const { return kdl_data_.base_link_name; } + +std::vector KDLInvKinChainNR_JL::getTipLinkNames() const { return { kdl_data_.tip_link_name }; } + +std::string KDLInvKinChainNR_JL::getSolverName() const { return solver_name_; } + +} // namespace tesseract_kinematics diff --git a/tesseract_kinematics/kdl/src/kdl_utils.cpp b/tesseract_kinematics/kdl/src/kdl_utils.cpp index 2e7147c300b..bd4ce412c68 100644 --- a/tesseract_kinematics/kdl/src/kdl_utils.cpp +++ b/tesseract_kinematics/kdl/src/kdl_utils.cpp @@ -143,4 +143,37 @@ bool parseSceneGraph(KDLChainData& results, chains.emplace_back(base_name, tip_name); return parseSceneGraph(results, scene_graph, chains); } + +void parseChainData(KDL::JntArray& q_min, + KDL::JntArray& q_max, + const KDLChainData& kdl_data, + const tesseract_scene_graph::SceneGraph& scene_graph) +{ + q_min.resize(kdl_data.robot_chain.getNrOfJoints()); + q_max.resize(kdl_data.robot_chain.getNrOfJoints()); + + for (uint joint_num = 0; joint_num < kdl_data.robot_chain.getNrOfJoints(); ++joint_num) + { + auto joint = scene_graph.getJoint(kdl_data.joint_names[joint_num]); + double lower = std::numeric_limits::lowest(); + double upper = std::numeric_limits::max(); + // Does the joint have limits? + if (joint->type != tesseract_scene_graph::JointType::CONTINUOUS) + { + if (joint->safety) + { + lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit); + upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit); + } + else + { + lower = joint->limits->lower; + upper = joint->limits->upper; + } + } + // Assign limits + q_min(joint_num) = lower; + q_max(joint_num) = upper; + } +} } // namespace tesseract_kinematics diff --git a/tesseract_kinematics/test/kdl_kinematics_unit.cpp b/tesseract_kinematics/test/kdl_kinematics_unit.cpp index 19313e7775e..0e43e7c2b9f 100644 --- a/tesseract_kinematics/test/kdl_kinematics_unit.cpp +++ b/tesseract_kinematics/test/kdl_kinematics_unit.cpp @@ -9,6 +9,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_kinematics::test_suite; @@ -32,6 +33,16 @@ TEST(TesseractKinematicsUnit, KDLKinChainNRInverseKinematicUnit) // NOLINT runInvKinIIWATest(factory, "KDLInvKinChainNRFactory", "KDLFwdKinChainFactory"); } +TEST(TesseractKinematicsUnit, KDLKinChainNR_JLInverseKinematicUnit) // NOLINT +{ + auto scene_graph = getSceneGraphIIWA(); + + tesseract_kinematics::KDLInvKinChainNR_JL derived_kin(*scene_graph, "base_link", "tool0"); + + tesseract_kinematics::KinematicsPluginFactory factory; + runInvKinIIWATest(factory, "KDLInvKinChainNR_JLFactory", "KDLFwdKinChainFactory"); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_kinematics/test/kinematic_plugins.yaml b/tesseract_kinematics/test/kinematic_plugins.yaml index bd3b659a9f7..2316f797e0f 100644 --- a/tesseract_kinematics/test/kinematic_plugins.yaml +++ b/tesseract_kinematics/test/kinematic_plugins.yaml @@ -26,6 +26,11 @@ kinematic_plugins: config: base_link: base_link tip_link: tool0 + KDLInvKinChainNR_JL: + class: KDLInvKinChainNR_JLFactory + config: + base_link: base_link + tip_link: tool0 abb_manipulator: default: OPWInvKin plugins: diff --git a/tesseract_kinematics/test/kinematics_factory_unit.cpp b/tesseract_kinematics/test/kinematics_factory_unit.cpp index 40176b94251..98ae2f56d67 100644 --- a/tesseract_kinematics/test/kinematics_factory_unit.cpp +++ b/tesseract_kinematics/test/kinematics_factory_unit.cpp @@ -1131,6 +1131,11 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT tip_link: tool0 KDLInvKinChainNR: class: KDLInvKinChainNRFactory + config: + base_link: base_link + tip_link: tool0 + KDLInvKinChainNR_JL: + class: KDLInvKinChainNR_JLFactory config: base_link: base_link tip_link: tool0)"; @@ -1156,6 +1161,13 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT EXPECT_EQ(inv_kin->getSolverName(), "KDLInvKinChainNR"); } + { + KinematicsPluginFactory factory(YAML::Load(yaml_string)); + auto inv_kin = factory.createInvKin("manipulator", "KDLInvKinChainNR_JL", *scene_graph, scene_state); + EXPECT_TRUE(inv_kin != nullptr); + EXPECT_EQ(inv_kin->getSolverName(), "KDLInvKinChainNR_JL"); + } + { // KDLFwdKinChain missing config YAML::Node config = YAML::Load(yaml_string); auto plugin = config["kinematic_plugins"]["fwd_kin_plugins"]["manipulator"]["plugins"]["KDLFwdKinChain"]; @@ -1183,6 +1195,15 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } + { // KDLInvKinChainNR_JL missing config + YAML::Node config = YAML::Load(yaml_string); + auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR_JL"]; + plugin.remove("config"); + + KinematicsPluginFactory factory(config); + auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR_JL", *scene_graph, scene_state); + EXPECT_TRUE(kin == nullptr); + } { // KDLFwdKinChain missing base_link YAML::Node config = YAML::Load(yaml_string); auto plugin = config["kinematic_plugins"]["fwd_kin_plugins"]["manipulator"]["plugins"]["KDLFwdKinChain"]; @@ -1210,6 +1231,15 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } + { // KDLInvKinChainNR_JL missing base_link + YAML::Node config = YAML::Load(yaml_string); + auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR_JL"]; + plugin["config"].remove("base_link"); + + KinematicsPluginFactory factory(config); + auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR_JL", *scene_graph, scene_state); + EXPECT_TRUE(kin == nullptr); + } { // KDLFwdKinChain missing tip_link YAML::Node config = YAML::Load(yaml_string); auto plugin = config["kinematic_plugins"]["fwd_kin_plugins"]["manipulator"]["plugins"]["KDLFwdKinChain"]; @@ -1237,6 +1267,15 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR", *scene_graph, scene_state); EXPECT_TRUE(kin == nullptr); } + { // KDLInvKinChainNR_JL missing tip_link + YAML::Node config = YAML::Load(yaml_string); + auto plugin = config["kinematic_plugins"]["inv_kin_plugins"]["manipulator"]["plugins"]["KDLInvKinChainNR_JL"]; + plugin["config"].remove("tip_link"); + + KinematicsPluginFactory factory(config); + auto kin = factory.createInvKin("manipulator", "KDLInvKinChainNR_JL", *scene_graph, scene_state); + EXPECT_TRUE(kin == nullptr); + } } int main(int argc, char** argv)