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

…#928)

.clang-format

@@ -8,7 +8,6 @@ AllowAllParametersOfDeclarationOnNextLine: false
 AllowShortFunctionsOnASingleLine: true
 AllowShortIfStatementsOnASingleLine: false
 AllowShortLoopsOnASingleLine: false
-AllowShortLoopsOnASingleLine: false
 AlwaysBreakBeforeMultilineStrings: false
 AlwaysBreakTemplateDeclarations: true
 BinPackArguments: false
@@ -54,15 +53,15 @@ BreakBeforeBraces: Custom
 
 # Control of individual brace wrapping cases
 BraceWrapping: {
-    AfterClass: 'true'
-    AfterControlStatement: 'true'
-    AfterEnum : 'true'
-    AfterFunction : 'true'
-    AfterNamespace : 'true'
-    AfterStruct : 'true'
-    AfterUnion : 'true'
-    BeforeCatch : 'true'
-    BeforeElse : 'true'
-    IndentBraces : 'false'
+    AfterClass: 'true',
+    AfterControlStatement: 'true',
+    AfterEnum : 'true',
+    AfterFunction : 'true',
+    AfterNamespace : 'true',
+    AfterStruct : 'true',
+    AfterUnion : 'true',
+    BeforeCatch : 'true',
+    BeforeElse : 'true',
+    IndentBraces : 'false',
 }
 ...

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,117 @@
+/**
+ * @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 <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:
+  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

@@ -100,6 +100,8 @@ struct KDLChainData
   std::string tip_link_name;                /**< @brief Link name of last kink in the kinematic object */
   std::map<std::string, int> segment_index; /**< @brief A map from chain link name to kdl chain segment number */
   std::vector<std::pair<std::string, std::string>> chains; /**< The chains used to create the object */
+  KDL::JntArray q_min;                                     /**< @brief Lower joint limits */
+  KDL::JntArray q_max;                                     /**< @brief Upper joint limits */
 };
 
 /**

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,162 @@
+/**
+ * @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");
+
+  // 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_);
+}
+
+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)
+{
+  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, kdl_data_.q_min, kdl_data_.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