From e94c891b819bce2a3cc925e1292f1dabc3367437 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 10 Dec 2024 23:05:31 -0500 Subject: [PATCH] handle continuous joints in getLowerAndUpperLimits (#3153) (#3155) (cherry picked from commit ecd5d300773af9481c842e56cc81df3769500676) Co-authored-by: Mario Prats --- .../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++; } }