From bd1a13f3ef2096759c6054ec99429e370a6c8739 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Tue, 10 Dec 2024 18:52:42 -0700 Subject: [PATCH] handle continuous joints in getLowerAndUpperLimits (#3153) (cherry picked from commit ecd5d300773af9481c842e56cc81df3769500676) --- .../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 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 Containing the lower and upper joint limits for all active + * variables. */ [[nodiscard]] std::pair 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 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::infinity(); + upper_limits[variable_index] = + variable_bounds.position_bounded_ ? variable_bounds.max_position_ : std::numeric_limits::infinity(); variable_index++; } }