Tiny optimizations in enforcePositionBounds() for RevoluteJointModel (#…

…1803)
moveit · Dec 13, 2022 · 2e13217 · 2e13217
1 parent e23fb8f
commit 2e13217
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 6 deletions.
diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h
@@ -287,15 +287,15 @@ class JointModel
   virtual bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const = 0;
 
   /** \brief Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous
-     joints are made between -Pi and Pi.
+     revolute joints are made between -Pi and Pi.
       Returns true if changes were made. */
   bool enforcePositionBounds(double* values) const
   {
     return enforcePositionBounds(values, variable_bounds_);
   }
 
   /** \brief Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous
-     joints are made between -Pi and Pi.
+     revolute joints are made between -Pi and Pi.
       Return true if changes were made. */
   virtual bool enforcePositionBounds(double* values, const Bounds& other_bounds) const = 0;
 

diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp
@@ -231,23 +231,20 @@ bool RevoluteJointModel::enforcePositionBounds(double* values, const Bounds& bou
       {
         v -= 2.0 * M_PI;
       }
-      return true;
     }
   }
   else
   {
     if (values[0] < bounds[0].min_position_)
     {
       values[0] = bounds[0].min_position_;
-      return true;
     }
     else if (values[0] > bounds[0].max_position_)
     {
       values[0] = bounds[0].max_position_;
-      return true;
     }
   }
-  return false;
+  return true;
 }
 
 void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const