diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index 5ad3751114..54cb62da1a 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/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() diff --git a/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml b/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml index 9af11adad9..387cf86470 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml +++ b/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: { diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp index 315084ebef..4b22eee85a 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp +++ b/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::epsilon()) { - changed_req = true; + should_fix_state |= true; } } break; @@ -126,7 +127,7 @@ class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter if (static_cast(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(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; } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/test/test_check_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/test/test_check_start_state_bounds.cpp new file mode 100644 index 0000000000..9a65375f2f --- /dev/null +++ b/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 +#include + +#include +#include +#include + +#include +#include +#include + +class TestCheckStartStateBounds : public testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = std::make_shared("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(); + planning_scene_ = std::make_shared(urdf_model, srdf_model); + + // Load the planning request adapter. + plugin_loader_ = std::make_unique>( + "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 node_; + std::shared_ptr planning_scene_; + std::unique_ptr> plugin_loader_; + pluginlib::UniquePtr 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(); +}