handle continuous joints in getLowerAndUpperLimits

moveit · Dec 10, 2024 · 1c1b818 · 1c1b818
1 parent f82cdcd
commit 1c1b818
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 4 deletions.
diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp
@@ -588,8 +588,10 @@ class JointModelGroup
 
   /**
    * @brief Get the lower and upper position limits of all active variables in the group.
-   *
-   * @return std::pair<Eigen::VectorXd, Eigen::VectorXd> Containing the lower and upper joint limits for all active variables.
+   * @details In the case of variable without position bounds (e.g. continuous joints), the lower and upper limits are
+   * set to infinity.
+   * @return std::pair<Eigen::VectorXd, Eigen::VectorXd> Containing the lower and upper joint limits for all active
+   * variables.
    */
   [[nodiscard]] std::pair<Eigen::VectorXd, Eigen::VectorXd> getLowerAndUpperLimits() const;
 

diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp
@@ -842,8 +842,10 @@ std::pair<Eigen::VectorXd, Eigen::VectorXd> JointModelGroup::getLowerAndUpperLim
   {
     for (const moveit::core::VariableBounds& variable_bounds : *joint_bounds)
     {
-      lower_limits[variable_index] = variable_bounds.min_position_;
-      upper_limits[variable_index] = variable_bounds.max_position_;
+      lower_limits[variable_index] =
+          variable_bounds.position_bounded_ ? variable_bounds.min_position_ : -std::numeric_limits<double>::infinity();
+      upper_limits[variable_index] =
+          variable_bounds.position_bounded_ ? variable_bounds.max_position_ : std::numeric_limits<double>::infinity();
       variable_index++;
     }
   }