Fix logic in CheckStartStateBounds adapter (backport #3143)

moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt

@@ -14,3 +14,14 @@ set_target_properties(moveit_default_planning_request_adapter_plugins
                       PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 ament_target_dependencies(moveit_default_planning_request_adapter_plugins
                           moveit_core rclcpp pluginlib)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+
+  ament_add_gmock(test_check_start_state_bounds
+                  test/test_check_start_state_bounds.cpp)
+  target_link_libraries(test_check_start_state_bounds moveit_planning_pipeline
+                        moveit_default_planning_request_adapter_plugins)
+
+endif()

moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml

@@ -1,8 +1,8 @@
-# This file contains the parameters for all of MoveIt's default PlanRequestAdapters
+# This file contains the parameters for all of MoveIt's default PlanningRequestAdapters
 default_request_adapter_parameters:
   fix_start_state: {
     type: bool,
-    description: "CheckStartStateBounds: If set true and the start state is out of bounds, the MotionPlanRequests start will be updated to a start state with enforced bounds for revolute or continuous joints.",
+    description: "CheckStartStateBounds: If true and the start state has continuous, planar, or floating joints, the start state of the MotionPlanRequest will be updated to normalize the rotation values of those joints.",
     default_value: false,
   }
   default_workspace_bounds: {

moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp

@@ -94,14 +94,15 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
     // Read parameters
     const auto params = param_listener_->get_params();
 
-    bool changed_req = false;
+    bool should_fix_state = false;
+    bool is_out_of_bounds = false;
     for (const moveit::core::JointModel* jmodel : jmodels)
     {
       // Check if we have a revolute, continuous joint. If we do, then we only need to make sure
       // it is within the model's declared bounds (usually -Pi, Pi), since the values wrap around.
       // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
       // how many times the joint was wrapped. Because of this, we remember the offsets for continuous
-      // joints, and we un-do them when the plan comes from the planner
+      // joints, and we undo them when the plan comes from the planner.
       switch (jmodel->getType())
       {
         case moveit::core::JointModel::REVOLUTE:
@@ -113,7 +114,7 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
             double after = start_state.getJointPositions(jmodel)[0];
             if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
             {
-              changed_req = true;
+              should_fix_state |= true;
             }
           }
           break;
@@ -126,7 +127,7 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
           if (static_cast<const moveit::core::PlanarJointModel*>(jmodel)->normalizeRotation(copy))
           {
             start_state.setJointPositions(jmodel, copy);
-            changed_req = true;
+            should_fix_state |= true;
           }
           break;
         }
@@ -138,58 +139,60 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter
           if (static_cast<const moveit::core::FloatingJointModel*>(jmodel)->normalizeRotation(copy))
           {
             start_state.setJointPositions(jmodel, copy);
-            changed_req = true;
+            should_fix_state |= true;
           }
           break;
         }
         default:
         {
-          if (!start_state.satisfiesBounds(jmodel))
-          {
-            std::stringstream joint_values;
-            std::stringstream joint_bounds_low;
-            std::stringstream joint_bounds_hi;
-            const double* p = start_state.getJointPositions(jmodel);
-            for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k)
-            {
-              joint_values << p[k] << ' ';
-            }
-            const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds();
-            for (const moveit::core::VariableBounds& variable_bounds : b)
-            {
-              joint_bounds_low << variable_bounds.min_position_ << ' ';
-              joint_bounds_hi << variable_bounds.max_position_ << ' ';
-            }
-            RCLCPP_ERROR(logger_,
-                         "Joint '%s' from the starting state is outside bounds by: [%s] should be in "
-                         "the range [%s], [%s].",
-                         jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(),
-                         joint_bounds_hi.str().c_str());
-          }
+          break;  // do nothing
         }
       }
-    }
 
-    // If we made any changes, consider using them them
-    if (params.fix_start_state && changed_req)
-    {
-      RCLCPP_WARN(logger_, "Changing start state.");
-      moveit::core::robotStateToRobotStateMsg(start_state, req.start_state);
-      return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, std::string(""),
-                                           getDescription());
+      // Check the joint against its bounds.
+      if (!start_state.satisfiesBounds(jmodel))
+      {
+        is_out_of_bounds |= true;
+
+        std::stringstream joint_values;
+        std::stringstream joint_bounds_low;
+        std::stringstream joint_bounds_hi;
+        const double* p = start_state.getJointPositions(jmodel);
+        for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k)
+        {
+          joint_values << p[k] << ' ';
+        }
+        const moveit::core::JointModel::Bounds& b = jmodel->getVariableBounds();
+        for (const moveit::core::VariableBounds& variable_bounds : b)
+        {
+          joint_bounds_low << variable_bounds.min_position_ << ' ';
+          joint_bounds_hi << variable_bounds.max_position_ << ' ';
+        }
+        RCLCPP_ERROR(logger_,
+                     "Joint '%s' from the starting state is outside bounds by: [%s] should be in "
+                     "the range [%s], [%s].",
+                     jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(),
+                     joint_bounds_hi.str().c_str());
+      }
     }
 
+    // Package up the adapter result, changing the state if applicable.
     auto status = moveit::core::MoveItErrorCode();
-    if (!changed_req)
-    {
-      status.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
-    }
-    else
+    status.source = getDescription();
+    status.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
+
+    if (is_out_of_bounds || (!params.fix_start_state && should_fix_state))
     {
       status.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_INVALID;
       status.message = std::string("Start state out of bounds.");
     }
-    status.source = getDescription();
+    else if (params.fix_start_state && should_fix_state)
+    {
+      constexpr auto msg_string = "Normalized start state.";
+      status.message = msg_string;
+      RCLCPP_WARN(logger_, msg_string);
+      moveit::core::robotStateToRobotStateMsg(start_state, req.start_state);
+    }
     return status;
   }
 

moveit_ros/planning/planning_request_adapter_plugins/test/test_check_start_state_bounds.cpp

@@ -0,0 +1,163 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, Sebastian Castro
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Sebastian Castro */
+
+#include <memory>
+#include <string>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include <moveit/planning_pipeline/planning_pipeline.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/utils/robot_model_test_utils.hpp>
+
+class TestCheckStartStateBounds : public testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    node_ = std::make_shared<rclcpp::Node>("test_check_start_state_bounds_adapter", "");
+
+    // Load a robot model and place it in a planning scene.
+    urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
+    srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
+    planning_scene_ = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
+
+    // Load the planning request adapter.
+    plugin_loader_ = std::make_unique<pluginlib::ClassLoader<planning_interface::PlanningRequestAdapter>>(
+        "moveit_core", "planning_interface::PlanningRequestAdapter");
+    adapter_ = plugin_loader_->createUniqueInstance("default_planning_request_adapters/CheckStartStateBounds");
+    adapter_->initialize(node_, "");
+  }
+
+  void TearDown() override
+  {
+    rclcpp::shutdown();
+  }
+
+  std::shared_ptr<rclcpp::Node> node_;
+  std::shared_ptr<planning_scene::PlanningScene> planning_scene_;
+  std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlanningRequestAdapter>> plugin_loader_;
+  pluginlib::UniquePtr<planning_interface::PlanningRequestAdapter> adapter_;
+};
+
+TEST_F(TestCheckStartStateBounds, TestWithinBounds)
+{
+  planning_interface::MotionPlanRequest request;
+  request.group_name = "right_arm";
+  request.start_state.joint_state.name = {
+    "r_shoulder_pan_joint", "r_shoulder_lift_joint", "r_upper_arm_roll_joint", "r_forearm_roll_joint",
+    "r_elbow_flex_joint",   "r_wrist_flex_joint",    "r_wrist_roll_joint",
+  };
+  request.start_state.joint_state.position = {
+    0.0, 0.0, 0.0, 0.0, -0.5, -0.5, 0.0,
+  };
+
+  const auto result = adapter_->adapt(planning_scene_, request);
+  EXPECT_EQ(result.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  EXPECT_EQ(result.message, "");
+}
+
+TEST_F(TestCheckStartStateBounds, TestRevoluteJointOutOfBounds)
+{
+  planning_interface::MotionPlanRequest request;
+  request.group_name = "right_arm";
+  request.start_state.joint_state.name = {
+    "r_shoulder_pan_joint", "r_shoulder_lift_joint", "r_upper_arm_roll_joint", "r_forearm_roll_joint",
+    "r_elbow_flex_joint",   "r_wrist_flex_joint",    "r_wrist_roll_joint",
+  };
+  request.start_state.joint_state.position = {
+    1.0,  // revolute joint out of bounds
+    0.0, 0.0, 0.0, -0.5, -0.5, 0.0,
+  };
+
+  const auto result = adapter_->adapt(planning_scene_, request);
+  EXPECT_EQ(result.val, moveit_msgs::msg::MoveItErrorCodes::START_STATE_INVALID);
+  EXPECT_EQ(result.message, "Start state out of bounds.");
+}
+
+TEST_F(TestCheckStartStateBounds, TestContinuousJointOutOfBounds)
+{
+  planning_interface::MotionPlanRequest request;
+  request.group_name = "right_arm";
+  request.start_state.joint_state.name = {
+    "r_shoulder_pan_joint", "r_shoulder_lift_joint", "r_upper_arm_roll_joint", "r_forearm_roll_joint",
+    "r_elbow_flex_joint",   "r_wrist_flex_joint",    "r_wrist_roll_joint",
+  };
+  request.start_state.joint_state.position = {
+    0.0,  0.0,  0.0, 100.0,  // continuous joint out of bounds
+    -0.5, -0.5, 0.0,
+  };
+
+  const auto result = adapter_->adapt(planning_scene_, request);
+  EXPECT_EQ(result.val, moveit_msgs::msg::MoveItErrorCodes::START_STATE_INVALID);
+  EXPECT_EQ(result.message, "Start state out of bounds.");
+}
+
+TEST_F(TestCheckStartStateBounds, TestContinuousJointFixedBounds)
+{
+  planning_interface::MotionPlanRequest request;
+  request.group_name = "right_arm";
+  request.start_state.joint_state.name = {
+    "r_shoulder_pan_joint", "r_shoulder_lift_joint", "r_upper_arm_roll_joint", "r_forearm_roll_joint",
+    "r_elbow_flex_joint",   "r_wrist_flex_joint",    "r_wrist_roll_joint",
+  };
+  request.start_state.joint_state.position = {
+    0.0,  0.0,  0.0, 100.0,  // continuous joint out of bounds
+    -0.5, -0.5, 0.0,
+  };
+
+  // Modify the start state. The adapter should succeed.
+  node_->set_parameter(rclcpp::Parameter("fix_start_state", true));
+
+  const auto result = adapter_->adapt(planning_scene_, request);
+  EXPECT_EQ(result.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+  EXPECT_EQ(result.message, "Normalized start state.");
+
+  // Validate that the large continuous joint position value in the request start state was normalized.
+  const auto& joint_names = request.start_state.joint_state.name;
+  const size_t joint_idx =
+      std::find(joint_names.begin(), joint_names.end(), "r_forearm_roll_joint") - joint_names.begin();
+  EXPECT_NEAR(request.start_state.joint_state.position[joint_idx], -0.530965, 1.0e-4);
+}
+
+int main(int argc, char** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}