- Also add KDL parameters to KDLInvKinChainNR_JL (see #843)

- Some clang_tidy and typo fixes

tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_fwd_kin_chain.h

@@ -30,7 +30,6 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainjnttojacsolver.hpp>
-#include <unordered_map>
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 

tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h

@@ -28,7 +28,6 @@
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <kdl/chainiksolverpos_lma.hpp>
-#include <unordered_map>
 #include <array>
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
@@ -59,7 +58,7 @@ class KDLInvKinChainLMA : public InverseKinematics
    * @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.
+   * They are ultimately passed to the constuctor of the underlying ChainIkSolver.
    *
    * The defaults provided here are the same defaults imposed by the KDL library.
    */

tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h

@@ -30,7 +30,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <kdl/chainiksolverpos_nr.hpp>
 #include <kdl/chainiksolvervel_pinv.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
-#include <unordered_map>
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
@@ -60,7 +59,7 @@ class KDLInvKinChainNR : public InverseKinematics
    * @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.
+   * They are ultimately passed to the constuctors of the underlying 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.

tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h

@@ -55,6 +55,23 @@ class KDLInvKinChainNR_JL : public InverseKinematics
   using UPtr = std::unique_ptr<KDLInvKinChainNR_JL>;
   using ConstUPtr = std::unique_ptr<const KDLInvKinChainNR_JL>;
 
+  /**
+   * @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 underlying 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_JL() override = default;
   KDLInvKinChainNR_JL(const KDLInvKinChainNR_JL& other);
   KDLInvKinChainNR_JL& operator=(const KDLInvKinChainNR_JL& other);
@@ -72,6 +89,7 @@ class KDLInvKinChainNR_JL : public InverseKinematics
   KDLInvKinChainNR_JL(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_JL_SOLVER_NAME);
 
   /**
@@ -83,6 +101,7 @@ class KDLInvKinChainNR_JL : public InverseKinematics
    */
   KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph,
                       const std::vector<std::pair<std::string, std::string> >& chains,
+                      Config kdl_config,
                       std::string solver_name = KDL_INV_KIN_CHAIN_NR_JL_SOLVER_NAME);
 
   IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses,
@@ -97,10 +116,11 @@ class KDLInvKinChainNR_JL : public InverseKinematics
   InverseKinematics::UPtr clone() const override final;
 
 private:
-  KDLChainData kdl_data_;                                          /**< @brief KDL data parsed from Scene Graph */
-  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_JL> ik_solver_;         /**< @brief KDL Inverse kinematic solver */
+  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_JL> 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 */
 

tesseract_kinematics/kdl/src/kdl_factories.cpp

@@ -171,6 +171,7 @@ KDLInvKinChainNR_JLFactory::create(const std::string& solver_name,
 {
   std::string base_link;
   std::string tip_link;
+  KDLInvKinChainNR_JL::Config kdl_config;
 
   try
   {
@@ -183,14 +184,26 @@ KDLInvKinChainNR_JLFactory::create(const std::string& solver_name,
       tip_link = n.as<std::string>();
     else
       throw std::runtime_error("KDLInvKinChainNR_JLFactory, missing 'tip_link' entry");
+    // Optional configuration parameters
+    if (YAML::Node n = config["velocity_eps"])
+      kdl_config.vel_eps = n.as<double>();
+
+    if (YAML::Node n = config["velocity_iterations"])
+      kdl_config.vel_iterations = n.as<int>();
+
+    if (YAML::Node n = config["position_eps"])
+      kdl_config.pos_eps = n.as<double>();
+
+    if (YAML::Node n = config["position_iterations"])
+      kdl_config.pos_iterations = n.as<int>();
   }
   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<KDLInvKinChainNR_JL>(scene_graph, base_link, tip_link, solver_name);
+  return std::make_unique<KDLInvKinChainNR_JL>(scene_graph, base_link, tip_link, kdl_config, solver_name);
 }
 
 TESSERACT_PLUGIN_ANCHOR_IMPL(KDLFactoriesAnchor)

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<std::pair<std::string, std::string>>& chains,
                                      Config kdl_config,
                                      std::string solver_name)
-  : kdl_config_(std::move(kdl_config)), 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.");
@@ -64,10 +64,7 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& sc
                                      const std::string& tip_link,
                                      Config kdl_config,
                                      std::string solver_name)
-  : KDLInvKinChainLMA(scene_graph,
-                      { std::make_pair(base_link, tip_link) },
-                      std::move(kdl_config),
-                      std::move(solver_name))
+  : KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name))
 {
 }
 
@@ -95,7 +92,8 @@ IKSolutions KDLInvKinChainLMA::calcInvKinHelper(const Eigen::Isometry3d& pose,
                                                 int /*segment_num*/) const
 {
   assert(std::abs(1.0 - pose.matrix().determinant()) < 1e-6);  // NOLINT
-  KDL::JntArray kdl_seed, kdl_solution;
+  KDL::JntArray kdl_seed;
+  KDL::JntArray kdl_solution;
   EigenToKDL(seed, kdl_seed);
   kdl_solution.resize(static_cast<unsigned>(seed.size()));
   Eigen::VectorXd solution(seed.size());

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<std::pair<std::string, std::string>>& chains,
                                    Config kdl_config,
                                    std::string solver_name)
-  : kdl_config_(std::move(kdl_config)), 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.");
@@ -63,10 +63,7 @@ KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scen
                                    const std::string& tip_link,
                                    Config kdl_config,
                                    std::string solver_name)
-  : KDLInvKinChainNR(scene_graph,
-                     { std::make_pair(base_link, tip_link) },
-                     std::move(kdl_config),
-                     std::move(solver_name))
+  : KDLInvKinChainNR(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name))
 {
 }
 
@@ -93,13 +90,14 @@ IKSolutions KDLInvKinChainNR::calcInvKinHelper(const Eigen::Isometry3d& pose,
                                                int /*segment_num*/) const
 {
   assert(std::abs(1.0 - pose.matrix().determinant()) < 1e-6);  // NOLINT
-  KDL::JntArray kdl_seed, kdl_solution;
+  KDL::JntArray kdl_seed;
+  KDL::JntArray kdl_solution;
   EigenToKDL(seed, kdl_seed);
   kdl_solution.resize(static_cast<unsigned>(seed.size()));
   Eigen::VectorXd solution(seed.size());
 
   // run IK solver
-  // TODO: Need to update to handle seg number. Neet to create an IK solver for each seg.
+  // 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 };

tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp

@@ -40,8 +40,9 @@ using Eigen::VectorXd;
 
 KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph,
                                          const std::vector<std::pair<std::string, std::string>>& chains,
+                                         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.");
@@ -51,16 +52,23 @@ KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph
 
   // 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);
-  ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_NR_JL>(
-      kdl_data_.robot_chain, kdl_data_.q_min, kdl_data_.q_max, *fk_solver_, *ik_vel_solver_);
+  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_JL>(kdl_data_.robot_chain,
+                                                             kdl_data_.q_min,
+                                                             kdl_data_.q_max,
+                                                             *fk_solver_,
+                                                             *ik_vel_solver_,
+                                                             kdl_config_.pos_iterations,
+                                                             kdl_config_.pos_eps);
 }
 
 KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(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_JL(scene_graph, { std::make_pair(base_link, tip_link) }, std::move(solver_name))
+  : KDLInvKinChainNR_JL(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name))
 {
 }
 
@@ -71,10 +79,17 @@ KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const KDLInvKinChainNR_JL& other) { *th
 KDLInvKinChainNR_JL& KDLInvKinChainNR_JL::operator=(const KDLInvKinChainNR_JL& other)
 {
   kdl_data_ = other.kdl_data_;
+  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);
-  ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_NR_JL>(
-      kdl_data_.robot_chain, kdl_data_.q_min, kdl_data_.q_max, *fk_solver_, *ik_vel_solver_);
+  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_JL>(kdl_data_.robot_chain,
+                                                             kdl_data_.q_min,
+                                                             kdl_data_.q_max,
+                                                             *fk_solver_,
+                                                             *ik_vel_solver_,
+                                                             kdl_config_.pos_iterations,
+                                                             kdl_config_.pos_eps);
   solver_name_ = other.solver_name_;
 
   return *this;
@@ -85,7 +100,8 @@ IKSolutions KDLInvKinChainNR_JL::calcInvKinHelper(const Eigen::Isometry3d& pose,
                                                   int /*segment_num*/) const
 {
   assert(std::abs(1.0 - pose.matrix().determinant()) < 1e-6);  // NOLINT
-  KDL::JntArray kdl_seed, kdl_solution;
+  KDL::JntArray kdl_seed;
+  KDL::JntArray kdl_solution;
   EigenToKDL(seed, kdl_seed);
   kdl_solution.resize(static_cast<unsigned>(seed.size()));
   Eigen::VectorXd solution(seed.size());
@@ -105,30 +121,30 @@ IKSolutions KDLInvKinChainNR_JL::calcInvKinHelper(const Eigen::Isometry3d& pose,
     // LCOV_EXCL_START
     if (status == KDL::ChainIkSolverPos_NR_JL::E_DEGRADED)
     {
-      CONSOLE_BRIDGE_logDebug("KDL NR Failed to calculate IK, solution converged to <eps in maxiter, but solution is "
-                              "degraded in quality (e.g. pseudo-inverse in iksolver is singular)");
+      CONSOLE_BRIDGE_logDebug("KDL NR_JL Failed to calculate IK, solution converged to <eps in maxiter, but solution "
+                              "is degraded in quality (e.g. pseudo-inverse in iksolver is singular)");
     }
     else if (status == KDL::ChainIkSolverPos_NR_JL::E_IKSOLVERVEL_FAILED)
     {
-      CONSOLE_BRIDGE_logDebug("KDL NR Failed to calculate IK, velocity IK solver failed");
+      CONSOLE_BRIDGE_logDebug("KDL NR_JL Failed to calculate IK, velocity IK solver failed");
     }
     else if (status == KDL::ChainIkSolverPos_NR_JL::E_FKSOLVERPOS_FAILED)
     {
-      CONSOLE_BRIDGE_logDebug("KDL NR Failed to calculate IK, position FK solver failed");
+      CONSOLE_BRIDGE_logDebug("KDL NR_JL Failed to calculate IK, position FK solver failed");
     }
     else if (status == KDL::ChainIkSolverPos_NR_JL::E_NO_CONVERGE)
     {
-      CONSOLE_BRIDGE_logDebug("KDL NR Failed to calculate IK, no solution found");
+      CONSOLE_BRIDGE_logDebug("KDL NR_JL Failed to calculate IK, no solution found");
     }
 #ifndef KDL_LESS_1_4_0
     else if (status == KDL::ChainIkSolverPos_NR_JL::E_MAX_ITERATIONS_EXCEEDED)
     {
-      CONSOLE_BRIDGE_logDebug("KDL NR Failed to calculate IK, max iteration exceeded");
+      CONSOLE_BRIDGE_logDebug("KDL NR_JL Failed to calculate IK, max iteration exceeded");
     }
 #endif
     else
     {
-      CONSOLE_BRIDGE_logDebug("KDL NR Failed to calculate IK");
+      CONSOLE_BRIDGE_logDebug("KDL NR_JL Failed to calculate IK");
     }
     // LCOV_EXCL_STOP
     return {};

tesseract_kinematics/test/kdl_kinematics_unit.cpp

@@ -39,7 +39,8 @@ TEST(TesseractKinematicsUnit, KDLKinChainNR_JLInverseKinematicUnit)  // NOLINT
 {
   auto scene_graph = getSceneGraphIIWA();
 
-  tesseract_kinematics::KDLInvKinChainNR_JL derived_kin(*scene_graph, "base_link", "tool0");
+  tesseract_kinematics::KDLInvKinChainNR_JL::Config config;
+  tesseract_kinematics::KDLInvKinChainNR_JL derived_kin(*scene_graph, "base_link", "tool0", config);
 
   tesseract_kinematics::KinematicsPluginFactory factory;
   runInvKinIIWATest(factory, "KDLInvKinChainNR_JLFactory", "KDLFwdKinChainFactory");

tesseract_kinematics/test/kinematics_factory_unit.cpp

@@ -1150,6 +1150,15 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit)  // NOLINT
                      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
+                 KDLInvKinChainNR_JL_AllParams:
+                   class: KDLInvKinChainNR_JLFactory
                    config:
                      base_link: base_link
                      tip_link: tool0