- Added joint limits to KDLChainData

- Moved parsing of joint limits from parseChainData() to parseSceneGraph()
tesseract-robotics · Sep 25, 2023 · 353ceb2 · 353ceb2
1 parent de7e0da
commit 353ceb2
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 60 deletions.
diff --git a/.clang-format b/.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',
 }
 ...
diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h b/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr_jl.h
@@ -31,7 +31,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #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
@@ -101,8 +100,6 @@ class KDLInvKinChainNR_JL : public InverseKinematics
   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 */

diff --git a/tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_utils.h b/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 */
 };
 
 /**
@@ -126,16 +128,5 @@ 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
diff --git a/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp b/tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr_jl.cpp
@@ -50,12 +50,11 @@ KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const tesseract_scene_graph::SceneGraph
   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_);
+  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,
@@ -72,12 +71,11 @@ KDLInvKinChainNR_JL::KDLInvKinChainNR_JL(const KDLInvKinChainNR_JL& other) { *th
 
 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_);
+  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;

diff --git a/tesseract_kinematics/kdl/src/kdl_utils.cpp b/tesseract_kinematics/kdl/src/kdl_utils.cpp
@@ -108,6 +108,8 @@ bool parseSceneGraph(KDLChainData& results,
 
   results.joint_names.clear();
   results.joint_names.resize(results.robot_chain.getNrOfJoints());
+  results.q_min.resize(results.robot_chain.getNrOfJoints());
+  results.q_max.resize(results.robot_chain.getNrOfJoints());
 
   results.segment_index.clear();
   results.segment_index[results.base_link_name] = 0;
@@ -128,33 +130,7 @@ bool parseSceneGraph(KDLChainData& results,
 
     results.joint_names[j] = jnt.getName();
 
-    ++j;
-  }
-
-  return true;
-}
-
-bool parseSceneGraph(KDLChainData& results,
-                     const tesseract_scene_graph::SceneGraph& scene_graph,
-                     const std::string& base_name,
-                     const std::string& tip_name)
-{
-  std::vector<std::pair<std::string, std::string>> chains;
-  chains.emplace_back(base_name, tip_name);
-  return parseSceneGraph(results, scene_graph, chains);
-}
-
-void parseChainData(KDL::JntArray& q_min,
-                    KDL::JntArray& q_max,
-                    const KDLChainData& kdl_data,
-                    const tesseract_scene_graph::SceneGraph& scene_graph)
-{
-  q_min.resize(kdl_data.robot_chain.getNrOfJoints());
-  q_max.resize(kdl_data.robot_chain.getNrOfJoints());
-
-  for (uint joint_num = 0; joint_num < kdl_data.robot_chain.getNrOfJoints(); ++joint_num)
-  {
-    auto joint = scene_graph.getJoint(kdl_data.joint_names[joint_num]);
+    auto joint = scene_graph.getJoint(results.joint_names[j]);
     double lower = std::numeric_limits<float>::lowest();
     double upper = std::numeric_limits<float>::max();
     // Does the joint have limits?
@@ -172,8 +148,12 @@ void parseChainData(KDL::JntArray& q_min,
       }
     }
     // Assign limits
-    q_min(joint_num) = lower;
-    q_max(joint_num) = upper;
+    results.q_min(j) = lower;
+    results.q_max(j) = upper;
+
+    ++j;
   }
+
+  return true;
 }
 }  // namespace tesseract_kinematics