From 1c1b818065f84e657b96422fea5bdfc034f3f686 Mon Sep 17 00:00:00 2001
From: Mario Prats <mario.prats@picknik.ai>
Date: Tue, 10 Dec 2024 06:16:20 -0700
Subject: [PATCH] handle continuous joints in getLowerAndUpperLimits

---
 .../include/moveit/robot_model/joint_model_group.hpp        | 6 ++++--
 moveit_core/robot_model/src/joint_model_group.cpp           | 6 ++++--
 2 files changed, 8 insertions(+), 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
index 54299304c1..195629c098 100644
--- 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
index a9d23c33ea..bed2f9dd0c 100644
--- 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++;
     }
   }