diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h index 01fa61d3c72..d5698c503e2 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h @@ -29,6 +29,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -54,6 +55,22 @@ class KDLInvKinChainLMA : public InverseKinematics using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; + /** + * @brief The Config struct + * + * This contains parameters that can be used to customize the KDL solver for your application. + * They are ultimately passed to the constuctor of the undelying ChainIkSolver. + * + * The defaults provided here are the same defaults imposed by the KDL library. + */ + struct Config + { + std::array task_weights{ 1.0, 1.0, 1.0, 0.1, 0.1, 0.1 }; + double eps{ 1E-5 }; + int max_iterations{ 500 }; + double eps_joints{ 1E-15 }; + }; + ~KDLInvKinChainLMA() override = default; KDLInvKinChainLMA(const KDLInvKinChainLMA& other); KDLInvKinChainLMA& operator=(const KDLInvKinChainLMA& other); @@ -71,6 +88,7 @@ class KDLInvKinChainLMA : public InverseKinematics KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, + Config kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME); /** @@ -82,6 +100,7 @@ class KDLInvKinChainLMA : public InverseKinematics */ KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector >& chains, + Config kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME); IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses, @@ -97,6 +116,7 @@ class KDLInvKinChainLMA : public InverseKinematics private: KDLChainData kdl_data_; /**< @brief KDL data parsed from Scene Graph */ + Config kdl_config_; /**< @brief KDL configuration data parsed from YAML */ std::unique_ptr ik_solver_; /**< @brief KDL Inverse kinematic solver */ std::string solver_name_{ KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME }; /**< @brief Name of this solver */ mutable std::mutex mutex_; /**< @brief KDL is not thread safe due to mutable variables in Joint Class */ diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h index 025e0b03435..8e1a1d61e00 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h @@ -56,6 +56,23 @@ class KDLInvKinChainNR : public InverseKinematics using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; + /** + * @brief The Config struct + * + * This contains parameters that can be used to customize the KDL solver for your application. + * They are ultimately passed to the constuctors of the undelying ChainIkSolver. + * The NR version creates both position and velocity solvers with different defaults for each. + * + * The defaults provided here are the same defaults imposed by the KDL library. + */ + struct Config + { + double vel_eps{ 0.00001 }; + int vel_iterations{ 150 }; + double pos_eps{ 1e-6 }; + int pos_iterations{ 100 }; + }; + ~KDLInvKinChainNR() override = default; KDLInvKinChainNR(const KDLInvKinChainNR& other); KDLInvKinChainNR& operator=(const KDLInvKinChainNR& other); @@ -73,6 +90,7 @@ class KDLInvKinChainNR : public InverseKinematics KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, + Config kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME); /** @@ -84,6 +102,7 @@ class KDLInvKinChainNR : public InverseKinematics */ KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector >& chains, + Config kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME); IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses, @@ -99,6 +118,7 @@ class KDLInvKinChainNR : public InverseKinematics private: KDLChainData kdl_data_; /**< @brief KDL data parsed from Scene Graph */ + Config kdl_config_; /**< @brief KDL configuration data parsed from YAML */ 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 */ diff --git a/tesseract_kinematics/kdl/src/kdl_factories.cpp b/tesseract_kinematics/kdl/src/kdl_factories.cpp index 020bb6ea744..913fbacf728 100644 --- a/tesseract_kinematics/kdl/src/kdl_factories.cpp +++ b/tesseract_kinematics/kdl/src/kdl_factories.cpp @@ -75,6 +75,7 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name, { std::string base_link; std::string tip_link; + KDLInvKinChainLMA::Config kdl_config; try { @@ -87,6 +88,25 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name, tip_link = n.as(); else throw std::runtime_error("KDLInvKinChainLMAFactory, missing 'tip_link' entry"); + + // Optional configuration parameters + if (YAML::Node n = config["task_weights"]) + { + // Make sure the length matches the constructor interface + if (n.size() != 6) + throw std::runtime_error("KDLInvKinChainLMAFactory, size of task_weights needs to be 6"); + + kdl_config.task_weights = n.as>(); + } + + if (YAML::Node n = config["eps"]) + kdl_config.eps = n.as(); + + if (YAML::Node n = config["max_iterations"]) + kdl_config.max_iterations = n.as(); + + if (YAML::Node n = config["eps_joints"]) + kdl_config.eps_joints = n.as(); } catch (const std::exception& e) { @@ -94,7 +114,7 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name, return nullptr; } - return std::make_unique(scene_graph, base_link, tip_link, solver_name); + return std::make_unique(scene_graph, base_link, tip_link, kdl_config, solver_name); } std::unique_ptr @@ -106,6 +126,7 @@ KDLInvKinChainNRFactory::create(const std::string& solver_name, { std::string base_link; std::string tip_link; + KDLInvKinChainNR::Config kdl_config; try { @@ -118,6 +139,19 @@ KDLInvKinChainNRFactory::create(const std::string& solver_name, tip_link = n.as(); else throw std::runtime_error("KDLInvKinChainNRFactory, missing 'tip_link' entry"); + + // Optional configuration parameters + if (YAML::Node n = config["velocity_eps"]) + kdl_config.vel_eps = n.as(); + + if (YAML::Node n = config["velocity_iterations"]) + kdl_config.vel_iterations = n.as(); + + if (YAML::Node n = config["position_eps"]) + kdl_config.pos_eps = n.as(); + + if (YAML::Node n = config["position_iterations"]) + kdl_config.pos_iterations = n.as(); } catch (const std::exception& e) { @@ -125,7 +159,7 @@ KDLInvKinChainNRFactory::create(const std::string& solver_name, return nullptr; } - return std::make_unique(scene_graph, base_link, tip_link, solver_name); + return std::make_unique(scene_graph, base_link, tip_link, kdl_config, solver_name); } std::unique_ptr diff --git a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp index 23c73e5076e..1779ec615d7 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp @@ -40,8 +40,9 @@ using Eigen::VectorXd; KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector>& chains, + Config kdl_config, std::string solver_name) - : solver_name_(std::move(solver_name)) + : kdl_config_(std::move(kdl_config)), solver_name_(std::move(solver_name)) { if (!scene_graph.getLink(scene_graph.getRoot())) throw std::runtime_error("The scene graph has an invalid root."); @@ -50,14 +51,23 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& sc throw std::runtime_error("Failed to parse KDL data from Scene Graph"); // Create KDL IK Solver - ik_solver_ = std::make_unique(kdl_data_.robot_chain); + ik_solver_ = + std::make_unique(kdl_data_.robot_chain, + Eigen::Matrix{ kdl_config_.task_weights.data() }, + kdl_config_.eps, + kdl_config_.max_iterations, + kdl_config_.eps_joints); } KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, + Config kdl_config, std::string solver_name) - : KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, std::move(solver_name)) + : KDLInvKinChainLMA(scene_graph, + { std::make_pair(base_link, tip_link) }, + std::move(kdl_config), + std::move(solver_name)) { } @@ -68,7 +78,13 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const KDLInvKinChainLMA& other) { *this = o KDLInvKinChainLMA& KDLInvKinChainLMA::operator=(const KDLInvKinChainLMA& other) { kdl_data_ = other.kdl_data_; - ik_solver_ = std::make_unique(kdl_data_.robot_chain); + kdl_config_ = other.kdl_config_; + ik_solver_ = + std::make_unique(kdl_data_.robot_chain, + Eigen::Matrix{ kdl_config_.task_weights.data() }, + kdl_config_.eps, + kdl_config_.max_iterations, + kdl_config_.eps_joints); solver_name_ = other.solver_name_; return *this; diff --git a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp index 223186ab720..ecf98caecb5 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp @@ -40,8 +40,9 @@ using Eigen::VectorXd; KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector>& chains, + Config kdl_config, std::string solver_name) - : solver_name_(std::move(solver_name)) + : kdl_config_(std::move(kdl_config)), solver_name_(std::move(solver_name)) { if (!scene_graph.getLink(scene_graph.getRoot())) throw std::runtime_error("The scene graph has an invalid root."); @@ -51,15 +52,21 @@ KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scen // 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, *fk_solver_, *ik_vel_solver_); + ik_vel_solver_ = std::make_unique( + kdl_data_.robot_chain, kdl_config_.vel_eps, kdl_config_.vel_iterations); + ik_solver_ = std::make_unique( + kdl_data_.robot_chain, *fk_solver_, *ik_vel_solver_, kdl_config_.pos_iterations, kdl_config_.pos_eps); } KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, + Config kdl_config, std::string solver_name) - : KDLInvKinChainNR(scene_graph, { std::make_pair(base_link, tip_link) }, std::move(solver_name)) + : KDLInvKinChainNR(scene_graph, + { std::make_pair(base_link, tip_link) }, + std::move(kdl_config), + std::move(solver_name)) { } @@ -70,9 +77,12 @@ KDLInvKinChainNR::KDLInvKinChainNR(const KDLInvKinChainNR& other) { *this = othe KDLInvKinChainNR& KDLInvKinChainNR::operator=(const KDLInvKinChainNR& other) { kdl_data_ = other.kdl_data_; + kdl_config_ = other.kdl_config_; 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, *fk_solver_, *ik_vel_solver_); + ik_vel_solver_ = std::make_unique( + kdl_data_.robot_chain, kdl_config_.vel_eps, kdl_config_.vel_iterations); + ik_solver_ = std::make_unique( + kdl_data_.robot_chain, *fk_solver_, *ik_vel_solver_, kdl_config_.pos_iterations, kdl_config_.pos_eps); solver_name_ = other.solver_name_; return *this; diff --git a/tesseract_kinematics/test/kdl_kinematics_unit.cpp b/tesseract_kinematics/test/kdl_kinematics_unit.cpp index 0e43e7c2b9f..c47a942eec0 100644 --- a/tesseract_kinematics/test/kdl_kinematics_unit.cpp +++ b/tesseract_kinematics/test/kdl_kinematics_unit.cpp @@ -17,7 +17,8 @@ TEST(TesseractKinematicsUnit, KDLKinChainLMAInverseKinematicUnit) // NOLINT { auto scene_graph = getSceneGraphIIWA(); - tesseract_kinematics::KDLInvKinChainLMA derived_kin(*scene_graph, "base_link", "tool0"); + tesseract_kinematics::KDLInvKinChainLMA::Config config; + tesseract_kinematics::KDLInvKinChainLMA derived_kin(*scene_graph, "base_link", "tool0", config); tesseract_kinematics::KinematicsPluginFactory factory; runInvKinIIWATest(factory, "KDLInvKinChainLMAFactory", "KDLFwdKinChainFactory"); @@ -27,7 +28,8 @@ TEST(TesseractKinematicsUnit, KDLKinChainNRInverseKinematicUnit) // NOLINT { auto scene_graph = getSceneGraphIIWA(); - tesseract_kinematics::KDLInvKinChainNR derived_kin(*scene_graph, "base_link", "tool0"); + tesseract_kinematics::KDLInvKinChainNR::Config config; + tesseract_kinematics::KDLInvKinChainNR derived_kin(*scene_graph, "base_link", "tool0", config); tesseract_kinematics::KinematicsPluginFactory factory; runInvKinIIWATest(factory, "KDLInvKinChainNRFactory", "KDLFwdKinChainFactory"); diff --git a/tesseract_kinematics/test/kinematics_factory_unit.cpp b/tesseract_kinematics/test/kinematics_factory_unit.cpp index 98ae2f56d67..6b1f0ddf0ca 100644 --- a/tesseract_kinematics/test/kinematics_factory_unit.cpp +++ b/tesseract_kinematics/test/kinematics_factory_unit.cpp @@ -1138,7 +1138,25 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT class: KDLInvKinChainNR_JLFactory config: base_link: base_link - tip_link: tool0)"; + tip_link: tool0 + KDLInvKinChainLMA_AllParams: + class: KDLInvKinChainLMAFactory + config: + base_link: base_link + tip_link: tool0 + task_weights: [1, 1, 1, 0.1, 0.1, 0.1] + eps: 1e-5 + max_iterations: 500 + eps_joints: 1e-15 + KDLInvKinChainNR_AllParams: + class: KDLInvKinChainNRFactory + config: + base_link: base_link + tip_link: tool0 + velocity_eps: 0.00001 + velocity_iterations: 150 + position_eps: 1e-6 + position_iterations: 100)"; { KinematicsPluginFactory factory(YAML::Load(yaml_string));