Skip to content

Commit

Permalink
Fix logic in CheckStartStateBounds adapter (#3143)
Browse files Browse the repository at this point in the history
* Fix logic in CheckStartStateBoundsAdapter

* Explicit fallthrough

* Fix switch statement and add basic test scaffolding

* Remove extra variable

* Fix logic and add tests for continuous joints

* Validate changed start state

* Appeasing our Lord and Savior clang-tidy

* Update status message
  • Loading branch information
sea-bass authored Dec 4, 2024
1 parent e7872eb commit 1de408f
Show file tree
Hide file tree
Showing 4 changed files with 220 additions and 43 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Original file line number Diff line number Diff line change
@@ -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: {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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;
Expand All @@ -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;
}
Expand All @@ -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;
}

Expand Down
Original file line number Diff line number Diff line change
@@ -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();
}

0 comments on commit 1de408f

Please sign in to comment.