Add support for KDL::ChainIkSolverPos_NR_JL, which takes joint limits…

… into account

tesseract_kinematics/CMakeLists.txt

@@ -92,15 +92,15 @@ string(
 target_compile_definitions(${PROJECT_NAME}_core PRIVATE TESSERACT_KINEMATICS_PLUGINS="${KINEMATICS_PLUGINS_STRING}")
 
 # Testing
-if((TESSERACT_ENABLE_TESTING OR TESSERACT_KINEMATICS_ENABLE_TESTING)
-   AND TESSERACT_BUILD_IKFAST
-   AND TESSERACT_BUILD_KDL
-   AND TESSERACT_BUILD_OPW
-   AND TESSERACT_BUILD_UR)
+# if((TESSERACT_ENABLE_TESTING OR TESSERACT_KINEMATICS_ENABLE_TESTING)
+#    AND TESSERACT_BUILD_IKFAST
+#    AND TESSERACT_BUILD_KDL
+#    AND TESSERACT_BUILD_OPW
+#    AND TESSERACT_BUILD_UR)
   enable_testing()
   add_run_tests_target(ENABLE ${TESSERACT_ENABLE_RUN_TESTING})
   add_subdirectory(test)
-endif()
+# endif()
 
 configure_package(NAMESPACE tesseract)
 

tesseract_kinematics/kdl/CMakeLists.txt

@@ -5,6 +5,7 @@ add_library(
   src/kdl_fwd_kin_chain.cpp
   src/kdl_inv_kin_chain_lma.cpp
   src/kdl_inv_kin_chain_nr.cpp
+  src/kdl_inv_kin_chain_nr_jl.cpp
   src/kdl_utils.cpp)
 target_link_libraries(
   ${PROJECT_NAME}_kdl

tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_factories.h

@@ -57,6 +57,15 @@ class KDLInvKinChainNRFactory : public InvKinFactory
                                  const YAML::Node& config) const override final;
 };
 
+class KDLInvKinChainNR_JLFactory : public InvKinFactory
+{
+  InverseKinematics::UPtr create(const std::string& solver_name,
+                                 const tesseract_scene_graph::SceneGraph& scene_graph,
+                                 const tesseract_scene_graph::SceneState& scene_state,
+                                 const KinematicsPluginFactory& plugin_factory,
+                                 const YAML::Node& config) const override final;
+};
+
 TESSERACT_PLUGIN_ANCHOR_DECL(KDLFactoriesAnchor)
 
 }  // namespace tesseract_kinematics

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

@@ -0,0 +1,120 @@
+/**
+ * @file kdl_fwd_kin_chain_nr_jl.h
+ * @brief Tesseract KDL inverse kinematics chain Newton-Raphson implementation.
+ *
+ * @author Levi Armstrong, Roelof Oomen
+ * @date July 26, 2023
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2023, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_JL_H
+#define TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_JL_H
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <kdl/chain.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <unordered_map>
+#include <console_bridge/console.h>
+#include <mutex>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_kinematics/core/inverse_kinematics.h>
+#include <tesseract_kinematics/kdl/kdl_utils.h>
+#include <tesseract_scene_graph/graph.h>
+
+namespace tesseract_kinematics
+{
+static const std::string KDL_INV_KIN_CHAIN_NR_JL_SOLVER_NAME = "KDLInvKinChainNR_JL";
+
+/**
+ * @brief KDL Inverse kinematic chain implementation.
+ */
+class KDLInvKinChainNR_JL : public InverseKinematics
+{
+public:
+  // LCOV_EXCL_START
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  // LCOV_EXCL_STOP
+
+  using Ptr = std::shared_ptr<KDLInvKinChainNR_JL>;
+  using ConstPtr = std::shared_ptr<const KDLInvKinChainNR_JL>;
+  using UPtr = std::unique_ptr<KDLInvKinChainNR_JL>;
+  using ConstUPtr = std::unique_ptr<const KDLInvKinChainNR_JL>;
+
+  ~KDLInvKinChainNR_JL() override = default;
+  KDLInvKinChainNR_JL(const KDLInvKinChainNR_JL& other);
+  KDLInvKinChainNR_JL& operator=(const KDLInvKinChainNR_JL& other);
+  KDLInvKinChainNR_JL(KDLInvKinChainNR_JL&&) = delete;
+  KDLInvKinChainNR_JL& operator=(KDLInvKinChainNR_JL&&) = delete;
+
+  /**
+   * @brief Construct KDL Forward Kinematics
+   * Creates KDL::Chain from tesseract scene graph
+   * @param scene_graph The Tesseract Scene Graph
+   * @param base_link The name of the base link for the kinematic chain
+   * @param tip_link The name of the tip link for the kinematic chain
+   * @param solver_name The solver name of the kinematic chain
+   */
+  KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph,
+                      const std::string& base_link,
+                      const std::string& tip_link,
+                      std::string solver_name = KDL_INV_KIN_CHAIN_NR_JL_SOLVER_NAME);
+
+  /**
+   * @brief Construct Inverse Kinematics as chain
+   * Creates a inverse kinematic chain object from sequential chains
+   * @param scene_graph The Tesseract Scene Graph
+   * @param chains A vector of kinematics chains <base_link, tip_link> that get concatenated
+   * @param solver_name The solver name of the kinematic chain
+   */
+  KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph,
+                      const std::vector<std::pair<std::string, std::string> >& chains,
+                      std::string solver_name = KDL_INV_KIN_CHAIN_NR_JL_SOLVER_NAME);
+
+  IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses,
+                         const Eigen::Ref<const Eigen::VectorXd>& seed) const override final;
+
+  std::vector<std::string> getJointNames() const override final;
+  Eigen::Index numJoints() const override final;
+  std::string getBaseLinkName() const override final;
+  std::string getWorkingFrame() const override final;
+  std::vector<std::string> getTipLinkNames() const override final;
+  std::string getSolverName() const override final;
+  InverseKinematics::UPtr clone() const override final;
+
+private:
+  KDL::JntArray q_min_;                                            /**< @brief Minimum joint positions */
+  KDL::JntArray q_max_;                                            /**< @brief Maximum joint positions */
+  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 */
+  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 */
+
+  /** @brief calcFwdKin helper function */
+  IKSolutions calcInvKinHelper(const Eigen::Isometry3d& pose,
+                               const Eigen::Ref<const Eigen::VectorXd>& seed,
+                               int segment_num = -1) const;
+};
+
+}  // namespace tesseract_kinematics
+#endif  // TESSERACT_KINEMATICS_KDL_INV_KIN_CHAIN_NR_JL_H

tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h

@@ -126,5 +126,16 @@ bool parseSceneGraph(KDLChainData& results,
                      const tesseract_scene_graph::SceneGraph& scene_graph,
                      const std::string& base_name,
                      const std::string& tip_name);
+
+/**
+ * @brief Parse joint limits from the KDL chain data
+ * @param lb Lower bounds (limits) of the joints
+ * @param ub Upper bounds (limits) of the joints
+ * @param scene_graph The Scene Graph
+ */
+void parseChainData(KDL::JntArray& q_min,
+                    KDL::JntArray& q_max,
+                    const KDLChainData& kdl_data,
+                    const tesseract_scene_graph::SceneGraph& scene_graph);
 }  // namespace tesseract_kinematics
 #endif  // TESSERACT_KINEMATICS_KDL_UTILS_H

tesseract_kinematics/kdl/src/kdl_factories.cpp

@@ -28,6 +28,7 @@
 #include <tesseract_kinematics/kdl/kdl_fwd_kin_chain.h>
 #include <tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h>
 #include <tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h>
+#include <tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h>
 
 namespace tesseract_kinematics
 {
@@ -121,6 +122,36 @@ InverseKinematics::UPtr KDLInvKinChainNRFactory::create(const std::string& solve
   return std::make_unique<KDLInvKinChainNR>(scene_graph, base_link, tip_link, solver_name);
 }
 
+InverseKinematics::UPtr KDLInvKinChainNR_JLFactory::create(const std::string& solver_name,
+                                                        const tesseract_scene_graph::SceneGraph& scene_graph,
+                                                        const tesseract_scene_graph::SceneState& /*scene_state*/,
+                                                        const KinematicsPluginFactory& /*plugin_factory*/,
+                                                        const YAML::Node& config) const
+{
+  std::string base_link;
+  std::string tip_link;
+
+  try
+  {
+    if (YAML::Node n = config["base_link"])
+      base_link = n.as<std::string>();
+    else
+      throw std::runtime_error("KDLInvKinChainNR_JLFactory, missing 'base_link' entry");
+
+    if (YAML::Node n = config["tip_link"])
+      tip_link = n.as<std::string>();
+    else
+      throw std::runtime_error("KDLInvKinChainNR_JLFactory, missing 'tip_link' entry");
+  }
+  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);
+}
+
 TESSERACT_PLUGIN_ANCHOR_IMPL(KDLFactoriesAnchor)
 
 }  // namespace tesseract_kinematics
@@ -131,3 +162,5 @@ TESSERACT_ADD_FWD_KIN_PLUGIN(tesseract_kinematics::KDLFwdKinChainFactory, KDLFwd
 TESSERACT_ADD_INV_KIN_PLUGIN(tesseract_kinematics::KDLInvKinChainLMAFactory, KDLInvKinChainLMAFactory);
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 TESSERACT_ADD_INV_KIN_PLUGIN(tesseract_kinematics::KDLInvKinChainNRFactory, KDLInvKinChainNRFactory);
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+TESSERACT_ADD_INV_KIN_PLUGIN(tesseract_kinematics::KDLInvKinChainNR_JLFactory, KDLInvKinChainNR_JLFactory);

tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp

@@ -0,0 +1,164 @@
+/**
+ * @file kdl_fwd_kin_chain_nr_jl.cpp
+ * @brief Tesseract KDL inverse kinematics chain Newton-Raphson implementation.
+ *
+ * @author Levi Armstrong, Roelof Oomen
+ * @date July 26, 2023
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2023, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <kdl/segment.hpp>
+#include <tesseract_scene_graph/kdl_parser.h>
+#include <memory>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h>
+#include <tesseract_kinematics/kdl/kdl_utils.h>
+#include <tesseract_kinematics/core/utils.h>
+
+namespace tesseract_kinematics
+{
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+
+KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph,
+                                         const std::vector<std::pair<std::string, std::string>>& chains,
+                                         std::string solver_name)
+  : solver_name_(std::move(solver_name))
+{
+  if (!scene_graph.getLink(scene_graph.getRoot()))
+    throw std::runtime_error("The scene graph has an invalid root.");
+
+  if (!parseSceneGraph(kdl_data_, scene_graph, chains))
+    throw std::runtime_error("Failed to parse KDL data from Scene Graph");
+
+  parseChainData(q_min_, q_max_, kdl_data_, scene_graph);
+
+  // 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, q_min_, q_max_, *fk_solver_, *ik_vel_solver_);
+}
+
+KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph& scene_graph,
+                                         const std::string& base_link,
+                                         const std::string& tip_link,
+                                         std::string solver_name)
+  : KDLInvKinChainNR_JL(scene_graph, { std::make_pair(base_link, tip_link) }, std::move(solver_name))
+{
+}
+
+InverseKinematics::UPtr KDLInvKinChainNR_JL::clone() const { return std::make_unique<KDLInvKinChainNR_JL>(*this); }
+
+KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const KDLInvKinChainNR_JL& other) { *this = other; }
+
+KDLInvKinChainNR_JL& KDLInvKinChainNR_JL::operator=(const KDLInvKinChainNR_JL& other)
+{
+  q_min_ = other.q_min_;
+  q_max_ = other.q_max_;
+  kdl_data_ = other.kdl_data_;
+  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, q_min_, q_max_, *fk_solver_, *ik_vel_solver_);
+  solver_name_ = other.solver_name_;
+
+  return *this;
+}
+
+IKSolutions KDLInvKinChainNR_JL::calcInvKinHelper(const Eigen::Isometry3d& pose,
+                                                  const Eigen::Ref<const Eigen::VectorXd>& seed,
+                                                  int /*segment_num*/) const
+{
+  assert(std::abs(1.0 - pose.matrix().determinant()) < 1e-6);  // NOLINT
+  KDL::JntArray kdl_seed, 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. Need to create an IK solver for each seg.
+  KDL::Frame kdl_pose;
+  EigenToKDL(pose, kdl_pose);
+  int status{ -1 };
+  {
+    std::lock_guard<std::mutex> guard(mutex_);
+    status = ik_solver_->CartToJnt(kdl_seed, kdl_pose, kdl_solution);
+  }
+
+  if (status < 0)
+  {
+    // 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)");
+    }
+    else if (status == KDL::ChainIkSolverPos_NR_JL::E_IKSOLVERVEL_FAILED)
+    {
+      CONSOLE_BRIDGE_logDebug("KDL NR 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");
+    }
+    else if (status == KDL::ChainIkSolverPos_NR_JL::E_NO_CONVERGE)
+    {
+      CONSOLE_BRIDGE_logDebug("KDL NR 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");
+    }
+#endif
+    else
+    {
+      CONSOLE_BRIDGE_logDebug("KDL NR Failed to calculate IK");
+    }
+    // LCOV_EXCL_STOP
+    return {};
+  }
+
+  KDLToEigen(kdl_solution, solution);
+
+  return { solution };
+}
+
+IKSolutions KDLInvKinChainNR_JL::calcInvKin(const tesseract_common::TransformMap& tip_link_poses,
+                                            const Eigen::Ref<const Eigen::VectorXd>& seed) const
+{
+  assert(tip_link_poses.find(kdl_data_.tip_link_name) != tip_link_poses.end());
+  return calcInvKinHelper(tip_link_poses.at(kdl_data_.tip_link_name), seed);
+}
+
+std::vector<std::string> KDLInvKinChainNR_JL::getJointNames() const { return kdl_data_.joint_names; }
+
+Eigen::Index KDLInvKinChainNR_JL::numJoints() const { return kdl_data_.robot_chain.getNrOfJoints(); }
+
+std::string KDLInvKinChainNR_JL::getBaseLinkName() const { return kdl_data_.base_link_name; }
+
+std::string KDLInvKinChainNR_JL::getWorkingFrame() const { return kdl_data_.base_link_name; }
+
+std::vector<std::string> KDLInvKinChainNR_JL::getTipLinkNames() const { return { kdl_data_.tip_link_name }; }
+
+std::string KDLInvKinChainNR_JL::getSolverName() const { return solver_name_; }
+
+}  // namespace tesseract_kinematics