From f51d9cac3a9d172d8812f21236c611e750ac5afc Mon Sep 17 00:00:00 2001 From: Sean Cardello Date: Fri, 3 Feb 2023 10:00:01 -0500 Subject: [PATCH] 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 b4ae547f4c3..194dbeb81e2 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 @@ -124,6 +124,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 a4dffa21423..1ad01f85fb6 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 @@ -76,9 +76,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; @@ -126,6 +123,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 05a368b5803..3b161b1a9ab 100644 --- a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h +++ b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h @@ -102,22 +102,6 @@ struct KDLChainData std::vector> chains; /**< The chains used to create the object */ }; -/** - * @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 4f6c1e1ebaf..15077a60c3c 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp @@ -43,7 +43,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."); @@ -53,7 +53,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, @@ -72,7 +72,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 148ac56e114..9f65c41611a 100644 --- a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp +++ b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp @@ -43,7 +43,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."); @@ -54,9 +54,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, @@ -75,9 +75,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;