Adddress review comments

tesseract-robotics · Feb 3, 2023 · f51d9ca · f51d9ca
1 parent 8bbabcb
commit f51d9ca
Show file tree

Hide file tree

Showing 5 changed files with 15 additions and 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
@@ -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<KDL::ChainIkSolverPos_LMA> 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
@@ -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<KDL::ChainFkSolverPos_recursive> fk_solver_;  /**< @brief KDL Forward Kinematic Solver */
   std::unique_ptr<KDL::ChainIkSolverVel_pinv> ik_vel_solver_;   /**< @brief KDL Inverse kinematic velocity solver */
   std::unique_ptr<KDL::ChainIkSolverPos_NR> 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
@@ -102,22 +102,6 @@ struct KDLChainData
   std::vector<std::pair<std::string, std::string>> 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
@@ -43,7 +43,7 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& sc
                                      const std::vector<std::pair<std::string, std::string>>& 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::ChainIkSolverPos_LMA>(
-      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::ChainIkSolverPos_LMA>(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
@@ -43,7 +43,7 @@ KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scen
                                    const std::vector<std::pair<std::string, std::string>>& 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::ChainFkSolverPos_recursive>(kdl_data_.robot_chain);
   ik_vel_solver_ = std::make_unique<KDL::ChainIkSolverVel_pinv>(
-      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::ChainIkSolverPos_NR>(
-      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::ChainFkSolverPos_recursive>(kdl_data_.robot_chain);
+  ik_vel_solver_ = std::make_unique<KDL::ChainIkSolverVel_pinv>(
+      kdl_data_.robot_chain, kdl_config_.vel_eps, kdl_config_.vel_iterations);
+  ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_NR>(
+      kdl_data_.robot_chain, *fk_solver_, *ik_vel_solver_, kdl_config_.pos_iterations, kdl_config_.pos_eps);
   solver_name_ = other.solver_name_;
 
   return *this;