diff --git a/.clang-format b/.clang-format index 8b3d0c00dab..ee5cc2929cd 100644 --- a/.clang-format +++ b/.clang-format @@ -8,7 +8,6 @@ AllowAllParametersOfDeclarationOnNextLine: false AllowShortFunctionsOnASingleLine: true AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false -AllowShortLoopsOnASingleLine: false AlwaysBreakBeforeMultilineStrings: false AlwaysBreakTemplateDeclarations: true BinPackArguments: false @@ -54,15 +53,15 @@ BreakBeforeBraces: Custom # Control of individual brace wrapping cases BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' + AfterClass: 'true', + AfterControlStatement: 'true', + AfterEnum : 'true', + AfterFunction : 'true', + AfterNamespace : 'true', + AfterStruct : 'true', + AfterUnion : 'true', + BeforeCatch : 'true', + BeforeElse : 'true', + IndentBraces : 'false', } ... diff --git a/tesseract_kinematics/kdl/CMakeLists.txt b/tesseract_kinematics/kdl/CMakeLists.txt index a757dadb458..6d89bcebfa1 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..e56e05b0325 --- /dev/null +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h @@ -0,0 +1,117 @@ +/** + * @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 +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: + 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..2c563db9156 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h @@ -100,6 +100,8 @@ struct KDLChainData std::string tip_link_name; /**< @brief Link name of last kink in the kinematic object */ std::map segment_index; /**< @brief A map from chain link name to kdl chain segment number */ std::vector> chains; /**< The chains used to create the object */ + KDL::JntArray q_min; /**< @brief Lower joint limits */ + KDL::JntArray q_max; /**< @brief Upper joint limits */ }; /** 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..47cdfa92eb8 --- /dev/null +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp @@ -0,0 +1,162 @@ +/** + * @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"); + + // 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, kdl_data_.q_min, kdl_data_.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) +{ + 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, kdl_data_.q_min, kdl_data_.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..88081fb1417 100644 --- a/tesseract_kinematics/kdl/src/kdl_utils.cpp +++ b/tesseract_kinematics/kdl/src/kdl_utils.cpp @@ -108,6 +108,8 @@ bool parseSceneGraph(KDLChainData& results, results.joint_names.clear(); results.joint_names.resize(results.robot_chain.getNrOfJoints()); + results.q_min.resize(results.robot_chain.getNrOfJoints()); + results.q_max.resize(results.robot_chain.getNrOfJoints()); results.segment_index.clear(); results.segment_index[results.base_link_name] = 0; @@ -128,19 +130,30 @@ bool parseSceneGraph(KDLChainData& results, results.joint_names[j] = jnt.getName(); + auto joint = scene_graph.getJoint(results.joint_names[j]); + 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 + results.q_min(j) = lower; + results.q_max(j) = upper; + ++j; } return true; } - -bool parseSceneGraph(KDLChainData& results, - const tesseract_scene_graph::SceneGraph& scene_graph, - const std::string& base_name, - const std::string& tip_name) -{ - std::vector> chains; - chains.emplace_back(base_name, tip_name); - return parseSceneGraph(results, scene_graph, chains); -} } // 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)