From 754873fb04c77de1ed42fc340e8e7c91d6985953 Mon Sep 17 00:00:00 2001 From: Lucian James Date: Thu, 10 Apr 2025 09:24:49 +0100 Subject: [PATCH] add multi state validation service --- moveit_ros/move_group/CMakeLists.txt | 1 + ...efault_capabilities_plugin_description.xml | 6 + .../moveit/move_group/capability_names.hpp | 2 + ...ti_state_validation_service_capability.cpp | 110 ++++++++++++++++++ ...ulti_state_validation_service_capability.h | 22 ++++ 5 files changed, 141 insertions(+) create mode 100644 moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.cpp create mode 100644 moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.h diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 2cdebb6ee9..7389985043 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -52,6 +52,7 @@ add_library( src/default_capabilities/query_planners_service_capability.cpp src/default_capabilities/save_geometry_to_file_service_capability.cpp src/default_capabilities/state_validation_service_capability.cpp + src/default_capabilities/multi_state_validation_service_capability.cpp src/default_capabilities/tf_publisher_capability.cpp) target_include_directories( moveit_move_group_default_capabilities diff --git a/moveit_ros/move_group/default_capabilities_plugin_description.xml b/moveit_ros/move_group/default_capabilities_plugin_description.xml index 785dfc09b4..ac9a2daf28 100644 --- a/moveit_ros/move_group/default_capabilities_plugin_description.xml +++ b/moveit_ros/move_group/default_capabilities_plugin_description.xml @@ -48,6 +48,12 @@ + + + Provide a ROS service that allows for testing multiple joint states validity + + + Provide a ROS service that allows for querying the planning scene diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.hpp b/moveit_ros/move_group/include/moveit/move_group/capability_names.hpp index 53af52c223..4fecc58961 100644 --- a/moveit_ros/move_group/include/moveit/move_group/capability_names.hpp +++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.hpp @@ -54,6 +54,8 @@ static const std::string IK_SERVICE_NAME = "compute_ik"; // name of ik service static const std::string FK_SERVICE_NAME = "compute_fk"; // name of fk service static const std::string STATE_VALIDITY_SERVICE_NAME = "check_state_validity"; // name of the service that validates states +static const std::string MULTI_STATE_VALIDITY_SERVICE_NAME = + "check_multi_state_validity"; // name of the service that validates multiple joint states static const std::string CARTESIAN_PATH_SERVICE_NAME = "compute_cartesian_path"; // name of the service that computes cartesian paths static const std::string GET_PLANNING_SCENE_SERVICE_NAME = diff --git a/moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.cpp new file mode 100644 index 0000000000..c3a62e37fb --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.cpp @@ -0,0 +1,110 @@ +#include "multi_state_validation_service_capability.h" +#include +#include +#include +#include +#include + +namespace move_group +{ +MoveGroupMultiStateValidationService::MoveGroupMultiStateValidationService() : MoveGroupCapability("StateValidationService") +{ +} + +void MoveGroupMultiStateValidationService::initialize() +{ + validity_service_ = context_->moveit_cpp_->getNode()->create_service( + MULTI_STATE_VALIDITY_SERVICE_NAME, [this](const std::shared_ptr& request_header, + const std::shared_ptr& req, + const std::shared_ptr& res) { + return computeService(request_header, req, res); + }); +} + +bool MoveGroupMultiStateValidationService::computeService( + const std::shared_ptr& /* unused */, + const std::shared_ptr& req, + const std::shared_ptr& res) +{ + res->valid = true; + planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_); + + auto robot_state = req->robot_state; + moveit::core::RobotState rs = ls->getCurrentState(); + moveit::core::robotStateMsgToRobotState(robot_state, rs); + + for(int i=0; ijoint_states.size(); i++){ + + // Update robot state with next set of joint states + rs.setVariableValues(req->joint_states[i]); + + // configure collision request + collision_detection::CollisionRequest creq; + creq.group_name = req->group_name; + creq.cost = true; + creq.contacts = true; + creq.max_contacts = ls->getWorld()->size() + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size(); + creq.max_cost_sources = creq.max_contacts; + creq.max_contacts *= creq.max_contacts; + collision_detection::CollisionResult cres; + + // check collision + ls->checkCollision(creq, cres, rs); + + // copy contacts if any + if (cres.collision) + { + rclcpp::Time time_now = context_->moveit_cpp_->getNode()->get_clock()->now(); + res->contacts.reserve(cres.contact_count); + res->valid = false; + for (collision_detection::CollisionResult::ContactMap::const_iterator it = cres.contacts.begin(); + it != cres.contacts.end(); ++it) + for (const collision_detection::Contact& contact : it->second) + { + res->contacts.resize(res->contacts.size() + 1); + collision_detection::contactToMsg(contact, res->contacts.back()); + res->contacts.back().header.frame_id = ls->getPlanningFrame(); + res->contacts.back().header.stamp = time_now; + } + } + + // copy cost sources + res->cost_sources.reserve(cres.cost_sources.size()); + for (const collision_detection::CostSource& cost_source : cres.cost_sources) + { + res->cost_sources.resize(res->cost_sources.size() + 1); + collision_detection::costSourceToMsg(cost_source, res->cost_sources.back()); + } + + // evaluate constraints + if (!moveit::core::isEmpty(req->constraints)) + { + kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel()); + kset.add(req->constraints, ls->getTransforms()); + std::vector kres; + kinematic_constraints::ConstraintEvaluationResult total_result = kset.decide(rs, kres); + if (!total_result.satisfied) + res->valid = false; + + // copy constraint results + res->constraint_result.resize(kres.size()); + for (std::size_t k = 0; k < kres.size(); ++k) + { + res->constraint_result[k].result = kres[k].satisfied; + res->constraint_result[k].distance = kres[k].distance; + } + } + + // No need to check any more joint states after the first invalid one + if(!res->valid){ + break; + } + } + + return true; +} +} // namespace move_group + +#include + +PLUGINLIB_EXPORT_CLASS(move_group::MoveGroupMultiStateValidationService, move_group::MoveGroupCapability) diff --git a/moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.h b/moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.h new file mode 100644 index 0000000000..d09b67a8d6 --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.h @@ -0,0 +1,22 @@ +#pragma once + +#include +#include + +namespace move_group +{ +class MoveGroupMultiStateValidationService : public MoveGroupCapability +{ +public: +MoveGroupMultiStateValidationService(); + + void initialize() override; + +private: + bool computeService(const std::shared_ptr& request_header, + const std::shared_ptr& req, + const std::shared_ptr& res); + + rclcpp::Service::SharedPtr validity_service_; +}; +} // namespace move_group \ No newline at end of file