From 08db7160bea5798a9e19390c6e499d7e99f0ffdc Mon Sep 17 00:00:00 2001 From: Sean Cardello Date: Tue, 31 Jan 2023 11:52:18 -0500 Subject: [PATCH 1/4] Add the ability to change KDL parameters from kinematics configuration --- .../kdl/kdl_inv_kin_chain_lma.h | 2 ++ .../tesseract_kinematics/kdl/kdl_utils.h | 16 +++++++++++++ .../kdl/src/kdl_factories.cpp | 24 ++++++++++++++++++- .../kdl/src/kdl_inv_kin_chain_lma.cpp | 8 +++---- 4 files changed, 45 insertions(+), 5 deletions(-) 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..24923ca25cd 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 @@ -71,6 +71,7 @@ class KDLInvKinChainLMA : public InverseKinematics KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, + const KDLConfig& kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME); /** @@ -82,6 +83,7 @@ class KDLInvKinChainLMA : public InverseKinematics */ KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector >& chains, + const KDLConfig& kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME); IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses, 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 5b6ca20e705..69cd627f455 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h @@ -101,6 +101,22 @@ struct KDLChainData KDL::JntArray q_max; /**< @brief Upper joint limits */ }; +/** + * @brief The KDLConfig 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 KDLConfig +{ + Eigen::VectorXd weights = Eigen::VectorXd::Constant(6, 1.0); + double eps = 1E-5; + int max_iterations = 500; + double eps_joints = 1E-15; +}; + /** * @brief Parse KDL chain data from the scene graph * @param results KDL Chain data diff --git a/tesseract_kinematics/kdl/src/kdl_factories.cpp b/tesseract_kinematics/kdl/src/kdl_factories.cpp index 020bb6ea744..a6a35abb003 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; + KDLConfig kdl_config; try { @@ -87,6 +88,27 @@ 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"); + + auto w = 0; + for (auto const &v : n) + kdl_config.weights(w++) = v.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 +116,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 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..36e5d99e194 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp @@ -40,7 +40,7 @@ using Eigen::VectorXd; KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector>& chains, - std::string solver_name) + const KDLConfig& kdl_config, std::string solver_name) : solver_name_(std::move(solver_name)) { if (!scene_graph.getLink(scene_graph.getRoot())) @@ -50,14 +50,14 @@ 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, kdl_config.weights, 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, - std::string solver_name) - : KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, std::move(solver_name)) + const KDLConfig& kdl_config, std::string solver_name) + : KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name)) { } From 896c1d0fbb5b327960ec9d14d4f9f5a1bd9b18e2 Mon Sep 17 00:00:00 2001 From: Sean Cardello Date: Thu, 2 Feb 2023 18:14:04 -0500 Subject: [PATCH 2/4] - Move config struct into class - Add Eigen alignment - Trivialize assignment operators to avoid storing kdl_config (though I can do this if preferred) - Format --- .../kdl/kdl_inv_kin_chain_lma.h | 23 ++++++++++++-- .../kdl/kdl_inv_kin_chain_nr.h | 22 ++++++++++++++ .../kdl/src/kdl_factories.cpp | 30 +++++++++++++++---- .../kdl/src/kdl_inv_kin_chain_lma.cpp | 11 ++++--- .../kdl/src/kdl_inv_kin_chain_nr.cpp | 16 ++++++---- 5 files changed, 84 insertions(+), 18 deletions(-) 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 24923ca25cd..f00454b27dd 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 @@ -54,6 +54,25 @@ 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 + { + Eigen::Matrix task_weights{ std::vector{ 1.0, 1.0, 1.0, 0.1, 0.1, 0.1 }.data() }; + double eps{ 1E-5 }; + int max_iterations{ 500 }; + double eps_joints{ 1E-15 }; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + ~KDLInvKinChainLMA() override = default; KDLInvKinChainLMA(const KDLInvKinChainLMA& other); KDLInvKinChainLMA& operator=(const KDLInvKinChainLMA& other); @@ -71,7 +90,7 @@ class KDLInvKinChainLMA : public InverseKinematics KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, - const KDLConfig& kdl_config, + const Config& kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME); /** @@ -83,7 +102,7 @@ class KDLInvKinChainLMA : public InverseKinematics */ KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector >& chains, - const KDLConfig& kdl_config, + const Config& kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME); IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses, 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..16e922714bf 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,26 @@ 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 }; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + ~KDLInvKinChainNR() override = default; KDLInvKinChainNR(const KDLInvKinChainNR& other); KDLInvKinChainNR& operator=(const KDLInvKinChainNR& other); @@ -73,6 +93,7 @@ class KDLInvKinChainNR : public InverseKinematics KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, + const Config& kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME); /** @@ -84,6 +105,7 @@ class KDLInvKinChainNR : public InverseKinematics */ KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector >& chains, + const Config& kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME); IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses, diff --git a/tesseract_kinematics/kdl/src/kdl_factories.cpp b/tesseract_kinematics/kdl/src/kdl_factories.cpp index a6a35abb003..c08d77d0b96 100644 --- a/tesseract_kinematics/kdl/src/kdl_factories.cpp +++ b/tesseract_kinematics/kdl/src/kdl_factories.cpp @@ -75,7 +75,7 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name, { std::string base_link; std::string tip_link; - KDLConfig kdl_config; + KDLInvKinChainLMA::Config kdl_config; try { @@ -93,12 +93,16 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name, 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"); + if (n.size() != kdl_config.task_weights.size()) + { + std::ostringstream what; + what << "KDLInvKinChainLMAFactory, size of task_weights needs to be " << kdl_config.task_weights.size(); + throw std::runtime_error(what.str()); + } auto w = 0; - for (auto const &v : n) - kdl_config.weights(w++) = v.as(); + for (auto const& v : n) + kdl_config.task_weights(w++) = v.as(); } if (YAML::Node n = config["eps"]) @@ -128,6 +132,7 @@ KDLInvKinChainNRFactory::create(const std::string& solver_name, { std::string base_link; std::string tip_link; + KDLInvKinChainNR::Config kdl_config; try { @@ -140,6 +145,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) { @@ -147,7 +165,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 36e5d99e194..0fc86a3ab8f 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp @@ -40,7 +40,8 @@ using Eigen::VectorXd; KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector>& chains, - const KDLConfig& kdl_config, std::string solver_name) + const Config& kdl_config, + std::string solver_name) : solver_name_(std::move(solver_name)) { if (!scene_graph.getLink(scene_graph.getRoot())) @@ -50,13 +51,15 @@ 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, kdl_config.weights, kdl_config.eps, kdl_config.max_iterations, kdl_config.eps_joints); + ik_solver_ = std::make_unique( + kdl_data_.robot_chain, kdl_config.task_weights, 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, - const KDLConfig& kdl_config, std::string solver_name) + const Config& kdl_config, + std::string solver_name) : KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name)) { } @@ -68,7 +71,7 @@ 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); + ik_solver_ = other.ik_solver_; 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..26718df3d10 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp @@ -40,6 +40,7 @@ using Eigen::VectorXd; KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector>& chains, + const Config& kdl_config, std::string solver_name) : solver_name_(std::move(solver_name)) { @@ -51,15 +52,18 @@ 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, + const 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) }, kdl_config, std::move(solver_name)) { } @@ -70,9 +74,9 @@ KDLInvKinChainNR::KDLInvKinChainNR(const KDLInvKinChainNR& other) { *this = othe KDLInvKinChainNR& KDLInvKinChainNR::operator=(const KDLInvKinChainNR& 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, *fk_solver_, *ik_vel_solver_); + fk_solver_ = other.fk_solver_; + ik_vel_solver_ = other.ik_vel_solver_; + ik_solver_ = other.ik_solver_; solver_name_ = other.solver_name_; return *this; From 000c18f173024fc388b2857f8e624d12e6908977 Mon Sep 17 00:00:00 2001 From: Sean Cardello Date: Fri, 3 Feb 2023 10:00:01 -0500 Subject: [PATCH 3/4] Adddress review comments --- .../kdl/kdl_inv_kin_chain_lma.h | 1 + .../kdl/kdl_inv_kin_chain_nr.h | 4 +--- .../include/tesseract_kinematics/kdl/kdl_utils.h | 16 ---------------- .../kdl/src/kdl_inv_kin_chain_lma.cpp | 7 ++++--- .../kdl/src/kdl_inv_kin_chain_nr.cpp | 15 +++++++++------ 5 files changed, 15 insertions(+), 28 deletions(-) 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 f00454b27dd..46b1abcdb7f 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 @@ -118,6 +118,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 16e922714bf..80cacf8a8f8 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 @@ -71,9 +71,6 @@ class KDLInvKinChainNR : public InverseKinematics int vel_iterations{ 150 }; double pos_eps{ 1e-6 }; int pos_iterations{ 100 }; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; ~KDLInvKinChainNR() override = default; @@ -121,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/include/tesseract_kinematics/kdl/kdl_utils.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h index 69cd627f455..5b6ca20e705 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h @@ -101,22 +101,6 @@ struct KDLChainData KDL::JntArray q_max; /**< @brief Upper joint limits */ }; -/** - * @brief The KDLConfig 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 KDLConfig -{ - Eigen::VectorXd weights = Eigen::VectorXd::Constant(6, 1.0); - double eps = 1E-5; - int max_iterations = 500; - double eps_joints = 1E-15; -}; - /** * @brief Parse KDL chain data from the scene graph * @param results KDL Chain data 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 0fc86a3ab8f..c1f41d5cc69 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp @@ -42,7 +42,7 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& sc const std::vector>& chains, const Config& kdl_config, std::string solver_name) - : solver_name_(std::move(solver_name)) + : kdl_config_(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."); @@ -52,7 +52,7 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& sc // Create KDL IK Solver ik_solver_ = std::make_unique( - kdl_data_.robot_chain, kdl_config.task_weights, kdl_config.eps, kdl_config.max_iterations, kdl_config.eps_joints); + kdl_data_.robot_chain, kdl_config_.task_weights, kdl_config_.eps, kdl_config_.max_iterations, kdl_config_.eps_joints); } KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, @@ -71,7 +71,8 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const KDLInvKinChainLMA& other) { *this = o KDLInvKinChainLMA& KDLInvKinChainLMA::operator=(const KDLInvKinChainLMA& other) { kdl_data_ = other.kdl_data_; - ik_solver_ = other.ik_solver_; + kdl_config_ = other.kdl_config_; + ik_solver_ = std::make_unique(kdl_data_.robot_chain, kdl_config_.task_weights, 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 26718df3d10..23f60d9b434 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp @@ -42,7 +42,7 @@ KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scen const std::vector>& chains, const Config& kdl_config, std::string solver_name) - : solver_name_(std::move(solver_name)) + : kdl_config_(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."); @@ -53,9 +53,9 @@ 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, kdl_config.vel_eps, kdl_config.vel_iterations); + 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); + 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, @@ -74,9 +74,12 @@ KDLInvKinChainNR::KDLInvKinChainNR(const KDLInvKinChainNR& other) { *this = othe KDLInvKinChainNR& KDLInvKinChainNR::operator=(const KDLInvKinChainNR& other) { kdl_data_ = other.kdl_data_; - fk_solver_ = other.fk_solver_; - ik_vel_solver_ = other.ik_vel_solver_; - ik_solver_ = other.ik_solver_; + kdl_config_ = other.kdl_config_; + fk_solver_ = std::make_unique(kdl_data_.robot_chain); + 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; From d7dda3139b6689d0509395b41114df92eca6a86e Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 2 Jun 2024 09:27:13 -0500 Subject: [PATCH 4/4] Fix unit tests --- .../kdl/kdl_inv_kin_chain_lma.h | 10 +++---- .../kdl/kdl_inv_kin_chain_nr.h | 4 +-- .../kdl/src/kdl_factories.cpp | 14 +++------- .../kdl/src/kdl_inv_kin_chain_lma.cpp | 26 ++++++++++++++----- .../kdl/src/kdl_inv_kin_chain_nr.cpp | 11 +++++--- .../test/kdl_kinematics_unit.cpp | 6 +++-- .../test/kinematics_factory_unit.cpp | 20 +++++++++++++- 7 files changed, 59 insertions(+), 32 deletions(-) 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 46b1abcdb7f..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 @@ -64,13 +65,10 @@ class KDLInvKinChainLMA : public InverseKinematics */ struct Config { - Eigen::Matrix task_weights{ std::vector{ 1.0, 1.0, 1.0, 0.1, 0.1, 0.1 }.data() }; + 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 }; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; ~KDLInvKinChainLMA() override = default; @@ -90,7 +88,7 @@ class KDLInvKinChainLMA : public InverseKinematics KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, - const Config& kdl_config, + Config kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME); /** @@ -102,7 +100,7 @@ class KDLInvKinChainLMA : public InverseKinematics */ KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector >& chains, - const Config& kdl_config, + Config kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME); IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses, 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 80cacf8a8f8..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 @@ -90,7 +90,7 @@ class KDLInvKinChainNR : public InverseKinematics KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, - const Config& kdl_config, + Config kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME); /** @@ -102,7 +102,7 @@ class KDLInvKinChainNR : public InverseKinematics */ KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector >& chains, - const Config& kdl_config, + Config kdl_config, std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME); IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses, diff --git a/tesseract_kinematics/kdl/src/kdl_factories.cpp b/tesseract_kinematics/kdl/src/kdl_factories.cpp index c08d77d0b96..913fbacf728 100644 --- a/tesseract_kinematics/kdl/src/kdl_factories.cpp +++ b/tesseract_kinematics/kdl/src/kdl_factories.cpp @@ -93,16 +93,10 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name, if (YAML::Node n = config["task_weights"]) { // Make sure the length matches the constructor interface - if (n.size() != kdl_config.task_weights.size()) - { - std::ostringstream what; - what << "KDLInvKinChainLMAFactory, size of task_weights needs to be " << kdl_config.task_weights.size(); - throw std::runtime_error(what.str()); - } - - auto w = 0; - for (auto const& v : n) - kdl_config.task_weights(w++) = v.as(); + 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"]) 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 c1f41d5cc69..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,9 +40,9 @@ using Eigen::VectorXd; KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector>& chains, - const Config& kdl_config, + Config kdl_config, std::string solver_name) - : kdl_config_(kdl_config), 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,16 +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, kdl_config_.task_weights, kdl_config_.eps, kdl_config_.max_iterations, kdl_config_.eps_joints); + 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, - const Config& kdl_config, + Config kdl_config, std::string solver_name) - : KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name)) + : KDLInvKinChainLMA(scene_graph, + { std::make_pair(base_link, tip_link) }, + std::move(kdl_config), + std::move(solver_name)) { } @@ -72,7 +79,12 @@ KDLInvKinChainLMA& KDLInvKinChainLMA::operator=(const KDLInvKinChainLMA& other) { kdl_data_ = other.kdl_data_; kdl_config_ = other.kdl_config_; - ik_solver_ = std::make_unique(kdl_data_.robot_chain, kdl_config_.task_weights, kdl_config_.eps, kdl_config_.max_iterations, kdl_config_.eps_joints); + 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 23f60d9b434..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,9 +40,9 @@ using Eigen::VectorXd; KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::vector>& chains, - const Config& kdl_config, + Config kdl_config, std::string solver_name) - : kdl_config_(kdl_config), 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."); @@ -61,9 +61,12 @@ KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scen KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& base_link, const std::string& tip_link, - const Config& kdl_config, + Config kdl_config, std::string solver_name) - : KDLInvKinChainNR(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name)) + : KDLInvKinChainNR(scene_graph, + { std::make_pair(base_link, tip_link) }, + std::move(kdl_config), + std::move(solver_name)) { } 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));