From 9e85e6f47fbdb9ceddc00cd102b29c845277cac6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 6 Nov 2023 19:41:41 +0100 Subject: [PATCH] Update planning request adapters --- .../default_configs/chomp_planning.yaml | 6 +- .../default_configs/ompl_planning.yaml | 6 +- .../default_configs/stomp_planning.yaml | 6 +- .../prbt_moveit_config/launch/demo.launch.py | 2 +- .../test/launch/hybrid_planning_common.py | 2 +- ...lt_request_adapters_plugin_description.xml | 6 +- .../CMakeLists.txt | 7 +- .../res/default_request_adapter_params.yaml | 28 +-- ...ounds.cpp => check_start_state_bounds.cpp} | 144 +++++++------- .../src/check_start_state_collision.cpp | 93 ++++++++++ .../src/fix_start_state_collision.cpp | 175 ------------------ .../src/resolve_constraint_frames.cpp | 11 +- ...unds.cpp => validate_workspace_bounds.cpp} | 17 +- .../ompl-chomp_planning_pipeline.launch.xml | 6 +- 14 files changed, 204 insertions(+), 305 deletions(-) rename moveit_ros/planning/planning_request_adapter_plugins/src/{fix_start_state_bounds.cpp => check_start_state_bounds.cpp} (58%) create mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp delete mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp rename moveit_ros/planning/planning_request_adapter_plugins/src/{fix_workspace_bounds.cpp => validate_workspace_bounds.cpp} (81%) diff --git a/moveit_configs_utils/default_configs/chomp_planning.yaml b/moveit_configs_utils/default_configs/chomp_planning.yaml index 0d3f790e25a..830e15d849d 100644 --- a/moveit_configs_utils/default_configs/chomp_planning.yaml +++ b/moveit_configs_utils/default_configs/chomp_planning.yaml @@ -4,9 +4,9 @@ jiggle_fraction: 0.05 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. request_adapters: - default_planning_request_adapters/ResolveConstraintFrames - - default_planning_request_adapters/FixWorkspaceBounds - - default_planning_request_adapters/FixStartStateBounds - - default_planning_request_adapters/FixStartStateCollision + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision response_adapters: - default_planning_response_adapters/AddTimeOptimalParameterization ridge_factor: 0.01 diff --git a/moveit_configs_utils/default_configs/ompl_planning.yaml b/moveit_configs_utils/default_configs/ompl_planning.yaml index f7f36c22c84..c69f2b71aa2 100644 --- a/moveit_configs_utils/default_configs/ompl_planning.yaml +++ b/moveit_configs_utils/default_configs/ompl_planning.yaml @@ -4,8 +4,8 @@ jiggle_fraction: 0.05 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. request_adapters: - default_planning_request_adapters/ResolveConstraintFrames - - default_planning_request_adapters/FixWorkspaceBounds - - default_planning_request_adapters/FixStartStateBounds - - default_planning_request_adapters/FixStartStateCollision + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision response_adapters: - default_planning_response_adapters/AddTimeOptimalParameterization diff --git a/moveit_configs_utils/default_configs/stomp_planning.yaml b/moveit_configs_utils/default_configs/stomp_planning.yaml index 44633e9d519..da44135e43c 100644 --- a/moveit_configs_utils/default_configs/stomp_planning.yaml +++ b/moveit_configs_utils/default_configs/stomp_planning.yaml @@ -2,9 +2,9 @@ planning_plugin: stomp_moveit/StompPlanner # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. request_adapters: - default_planning_request_adapters/ResolveConstraintFrames - - default_planning_request_adapters/FixWorkspaceBounds - - default_planning_request_adapters/FixStartStateBounds - - default_planning_request_adapters/FixStartStateCollision + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision response_adapters: - default_planning_response_adapters/AddTimeOptimalParameterization diff --git a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py index 70162536610..b2f9c6adb41 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py +++ b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py @@ -87,7 +87,7 @@ def generate_launch_description(): }, "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/FixWorkspaceBounds default_planning_request_adapters/FixStartStateBounds default_planning_request_adapters/FixStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""", + "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, }, } diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py index e84213f4589..1299e7c2d1a 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -71,7 +71,7 @@ def generate_common_hybrid_launch_description(): planning_pipelines_config = { "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/FixWorkspaceBounds default_planning_request_adapters/FixStartStateBounds default_planning_request_adapters/FixStartStateCollision""", + "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision""", "start_state_max_bounds_error": 0.1, } } diff --git a/moveit_ros/planning/default_request_adapters_plugin_description.xml b/moveit_ros/planning/default_request_adapters_plugin_description.xml index 133fc9ce811..3ddcedfd357 100644 --- a/moveit_ros/planning/default_request_adapters_plugin_description.xml +++ b/moveit_ros/planning/default_request_adapters_plugin_description.xml @@ -1,18 +1,18 @@ - + Fixes the start state to be within the joint limits specified in the URDF. - + Specify a default workspace for planning if none is specified in the planning request. - + If start state is in collision this adapter attempts to sample a new collision-free configuration near a specified configuration (in collision) by perturbing the joint values by a small amount. diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index 9ff255882fe..fe60a67adb1 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -2,9 +2,9 @@ generate_parameter_library(default_request_adapter_parameters res/default_reques add_library(moveit_default_planning_request_adapter_plugins SHARED src/check_for_stacked_constraints.cpp - src/fix_start_state_bounds.cpp - src/fix_start_state_collision.cpp - src/fix_workspace_bounds.cpp + src/check_start_state_bounds.cpp + src/check_start_state_collision.cpp + src/validate_workspace_bounds.cpp src/resolve_constraint_frames.cpp ) @@ -12,7 +12,6 @@ target_link_libraries(moveit_default_planning_request_adapter_plugins default_re set_target_properties(moveit_default_planning_request_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(moveit_default_planning_request_adapter_plugins - Boost moveit_core rclcpp pluginlib 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 8a261e63171..93a6c086bc4 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,30 +1,12 @@ # This files contains the parameters for all of MoveIt's default PlanRequestAdapters default_request_adapter_parameters: - start_state_max_dt: { - type: double, - description: "FixStartStateCollision/FixStartStateBounds: Maximum temporal distance of the fixed start state from the original state.", - default_value: 0.5, - } - jiggle_fraction: { - type: double, - description: "FixStartStateCollision: Joint value perturbation as a percentage of the total range of motion for the joint.", - default_value: 0.02, - } - max_sampling_attempts: { - type: int, - description: "FixStartStateCollision: Maximum number of attempts to re-sample a valid start state.", - default_value: 100, - validation: { - gt_eq<>: [ 0 ], - } - } - start_state_max_bounds_error: { - type: double, - description: "FixStartStateBounds: Maximum allowable error outside joint bounds for the starting configuration.", - default_value: 0.05, + 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.", + default_value: false, } default_workspace_bounds: { type: double, - description: "FixWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.", + description: "ValidateWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.", default_value: 10.0, } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp similarity index 58% rename from moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp rename to moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp index 155bbba7c96..ea8828e2bf8 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp @@ -33,18 +33,17 @@ *********************************************************************/ /* Author: Ioan Sucan - * Desc: Plan request adapter to fix start state bounds adapter fixes the start state to be within the joint limits + * Desc: The CheckStartStateBounds adapter validates if the start state is within the joint limits * specified in the URDF. The need for this adapter arises in situations where the joint limits for the physical robot * are not properly configured. The robot may then end up in a configuration where one or more of its joints is slightly * outside its joint limits. In this case, the motion planner is unable to plan since it will think that the starting - * state is outside joint limits. The “FixStartStateBounds” planning request adapter will “fix” the start state by + * state is outside joint limits. The “CheckStartStateBounds” planning request adapter can “fix” the start state by * moving it to the joint limit. However, this is obviously not the right solution every time - e.g. where the joint is * really outside its joint limits by a large amount. A parameter for the adapter specifies how much the joint can be * outside its limits for it to be “fixable”. */ #include -#include #include #include #include @@ -59,11 +58,11 @@ namespace default_planning_request_adapters { -/** @brief Fix start state bounds adapter fixes the start state to be within the joint limits specified in the URDF. */ -class FixStartStateBounds : public planning_interface::PlanningRequestAdapter +/** @brief The CheckStartStateBounds adapter validates if the start state is within the joint limits specified in the URDF. */ +class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter { public: - FixStartStateBounds() : logger_(moveit::makeChildLogger("fix_start_state_bounds")) + CheckStartStateBounds() : logger_(moveit::makeChildLogger("check_start_state_bounds")) { } @@ -74,7 +73,7 @@ class FixStartStateBounds : public planning_interface::PlanningRequestAdapter std::string getDescription() const override { - return "Fix Start State Bounds"; + return std::string("CheckStartStateBounds"); } bool adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, @@ -82,111 +81,106 @@ class FixStartStateBounds : public planning_interface::PlanningRequestAdapter { RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); - // get the specified start state + // Get the specified start state moveit::core::RobotState start_state = planning_scene->getCurrentState(); moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + // Get joint models const std::vector& jmodels = planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : planning_scene->getRobotModel()->getJointModels(); - bool change_req = false; - for (const moveit::core::JointModel* jm : jmodels) + // Read parameters + const auto params = param_listener_->get_params(); + + bool changed_req = 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 de model's declared bounds (usually -Pi, Pi), since the values wrap around. + // 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 - - if (jm->getType() == moveit::core::JointModel::REVOLUTE) + switch (jmodel->getType()) { - if (static_cast(jm)->isContinuous()) + case moveit::core::JointModel::REVOLUTE: { - double initial = start_state.getJointPositions(jm)[0]; - start_state.enforceBounds(jm); - double after = start_state.getJointPositions(jm)[0]; - if (fabs(initial - after) > std::numeric_limits::epsilon()) - change_req = true; + if (static_cast(jmodel)->isContinuous()) + { + double initial = start_state.getJointPositions(jmodel)[0]; + start_state.enforceBounds(jmodel); + double after = start_state.getJointPositions(jmodel)[0]; + if (fabs(initial - after) > std::numeric_limits::epsilon()) + { + changed_req = true; + } + } + break; } - } - else // Normalize yaw; no offset needs to be remembered - if (jm->getType() == moveit::core::JointModel::PLANAR) + case moveit::core::JointModel::PLANAR: { - const double* p = start_state.getJointPositions(jm); + const double* p = start_state.getJointPositions(jmodel); double copy[3] = { p[0], p[1], p[2] }; - if (static_cast(jm)->normalizeRotation(copy)) + if (static_cast(jmodel)->normalizeRotation(copy)) { - start_state.setJointPositions(jm, copy); - change_req = true; + start_state.setJointPositions(jmodel, copy); + changed_req = true; } + break; } - else + case moveit::core::JointModel::FLOATING: + { // Normalize quaternions - if (jm->getType() == moveit::core::JointModel::FLOATING) + const double* p = start_state.getJointPositions(jmodel); + double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] }; + if (static_cast(jmodel)->normalizeRotation(copy)) { - const double* p = start_state.getJointPositions(jm); - double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] }; - if (static_cast(jm)->normalizeRotation(copy)) - { - start_state.setJointPositions(jm, copy); - change_req = true; - } + start_state.setJointPositions(jmodel, copy); + changed_req = true; } - } - - // Read parameters - const auto params = param_listener_->get_params(); - - // pointer to a prefix state we could possibly add, if we detect we have to make changes - moveit::core::RobotStatePtr prefix_state; - for (const moveit::core::JointModel* jmodel : jmodels) - { - if (!start_state.satisfiesBounds(jmodel)) - { - if (start_state.satisfiesBounds(jmodel, params.start_state_max_bounds_error)) - { - if (!prefix_state) - prefix_state = std::make_shared(start_state); - start_state.enforceBounds(jmodel); - change_req = true; - RCLCPP_INFO(logger_, "Starting state is just outside bounds (joint '%s'). Assuming within bounds.", - jmodel->getName().c_str()); + break; } - else + default: { - 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) + if (!start_state.satisfiesBounds(jmodel)) { - joint_bounds_low << variable_bounds.min_position_ << ' '; - joint_bounds_hi << variable_bounds.max_position_ << ' '; + 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()); + } } - RCLCPP_WARN(logger_, - "Joint '%s' from the starting state is outside bounds by a significant margin: [%s] should be in " - "the range [%s], [%s] but the error above the 'start_state_max_bounds_error' parameter " - "(currently set to %f)", - jmodel->getName().c_str(), joint_values.str().c_str(), joint_bounds_low.str().c_str(), - joint_bounds_hi.str().c_str(), params.start_state_max_bounds_error); } } } - // if we made any changes, use them - if (change_req) + // 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 true; } - return true; + return !changed_req; /* If the request did not get updated the start state is within bounds */ } private: @@ -195,5 +189,5 @@ class FixStartStateBounds : public planning_interface::PlanningRequestAdapter }; } // namespace default_planning_request_adapters -CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::FixStartStateBounds, +CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::CheckStartStateBounds, planning_interface::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp new file mode 100644 index 00000000000..8c12274a609 --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * 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 Willow Garage 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: Ioan Sucan, Sebastian Jahr + * Desc: This adapter checks if the start state is in collision. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace default_planning_request_adapters +{ + +/** @brief This adapter checks if the start state is in collision.*/ +class CheckStartStateCollision : public planning_interface::PlanningRequestAdapter +{ +public: + CheckStartStateCollision() : logger_(moveit::makeChildLogger("validate_start_state")) + { + } + + void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override + { + } + + std::string getDescription() const override + { + return std::string("CheckStartStateCollision"); + } + + bool adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, + planning_interface::MotionPlanRequest& req) const override + { + RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + + // Get the start state TODO(sjahr): Should we check if req.start state == planning scene start state? + moveit::core::RobotState start_state = planning_scene->getCurrentState(); + moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); + + collision_detection::CollisionRequest creq; + creq.group_name = req.group_name; + collision_detection::CollisionResult cres; + // TODO(sjahr): Would verbose make sense? + planning_scene->checkCollision(creq, cres, start_state); + return !cres.collision; // Return true if start state is not in collision + } + +private: + rclcpp::Logger logger_; +}; +} // namespace default_planning_request_adapters + +CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::CheckStartStateCollision, + planning_interface::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp deleted file mode 100644 index 12300e20bd7..00000000000 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp +++ /dev/null @@ -1,175 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * 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 Willow Garage 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: Ioan Sucan - * Desc: Fix start state collision adapter which will attempt to sample a new collision-free configuration near a - * specified configuration (in collision) by perturbing the joint values by a small amount. The amount that it will - * perturb the values by is specified by the jiggle_fraction parameter that controls the perturbation as a percentage of - * the total range of motion for the joint. The other parameter for this adapter specifies how many random perturbations - * the adapter will sample before giving up. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace default_planning_request_adapters -{ - -/** @brief This fix start state collision adapter will attempt to sample a new collision-free configuration near a - * specified configuration (in collision) by perturbing the joint values by a small amount.*/ -class FixStartStateCollision : public planning_interface::PlanningRequestAdapter -{ -public: - FixStartStateCollision() : logger_(moveit::makeChildLogger("fix_start_state_collision")) - { - } - - void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) override - { - param_listener_ = std::make_unique(node, parameter_namespace); - } - - std::string getDescription() const override - { - return "Fix Start State In Collision"; - } - - bool adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, - planning_interface::MotionPlanRequest& req) const override - { - RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); - - // get the specified start state - moveit::core::RobotState start_state = planning_scene->getCurrentState(); - moveit::core::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state); - - collision_detection::CollisionRequest creq; - creq.group_name = req.group_name; - collision_detection::CollisionResult cres; - planning_scene->checkCollision(creq, cres, start_state); - if (cres.collision) - { - // Rerun in verbose mode - collision_detection::CollisionRequest vcreq = creq; - collision_detection::CollisionResult vcres; - vcreq.verbose = true; - planning_scene->checkCollision(vcreq, vcres, start_state); - - if (creq.group_name.empty()) - { - RCLCPP_INFO(logger_, "Start state appears to be in collision"); - } - else - { - RCLCPP_INFO(logger_, "Start state appears to be in collision with respect to group %s", creq.group_name.c_str()); - } - - auto prefix_state = std::make_shared(start_state); - random_numbers::RandomNumberGenerator& rng = prefix_state->getRandomNumberGenerator(); - - const std::vector& jmodels = - planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ? - planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() : - planning_scene->getRobotModel()->getJointModels(); - - bool found = false; - const auto params = param_listener_->get_params(); - - for (int c = 0; !found && c < params.max_sampling_attempts; ++c) - { - for (std::size_t i = 0; !found && i < jmodels.size(); ++i) - { - std::vector sampled_variable_values(jmodels[i]->getVariableCount()); - const double* original_values = prefix_state->getJointPositions(jmodels[i]); - jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, - jmodels[i]->getMaximumExtent() * params.jiggle_fraction); - start_state.setJointPositions(jmodels[i], sampled_variable_values); - collision_detection::CollisionResult cres; - planning_scene->checkCollision(creq, cres, start_state); - if (!cres.collision) - { - found = true; - RCLCPP_INFO(logger_, "Found a valid state near the start state at distance %lf after %d attempts", - prefix_state->distance(start_state), c); - } - } - } - - if (found) - { - RCLCPP_WARN(logger_, "Changing start state."); - moveit::core::robotStateToRobotStateMsg(start_state, req.start_state); - return true; - } - else - { - RCLCPP_ERROR( - logger_, - "Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %lu sampling " - "attempts). Passing the original planning request to the planner.", - params.jiggle_fraction, params.max_sampling_attempts); - return false; // skip remaining adapters and/or planner - } - } - else - { - if (creq.group_name.empty()) - { - RCLCPP_DEBUG(logger_, "Start state is valid"); - } - else - { - RCLCPP_DEBUG(logger_, "Start state is valid with respect to group %s", creq.group_name.c_str()); - } - return true; - } - } - -private: - std::unique_ptr param_listener_; - rclcpp::Logger logger_; -}; -} // namespace default_planning_request_adapters - -CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::FixStartStateCollision, - planning_interface::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index a9c0c6b787c..f7bdb131d78 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -32,7 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Robert Haschke */ +/* Author: Robert Haschke + Desc: This adapter changes the link_name field of a constraint from an object's (sub-)frame name to the name of the + robot link, that object is attached to. Transforming the frame names is necessary because the frames of an attached + object are not know to a planner. +*/ #include #include @@ -41,6 +45,7 @@ namespace default_planning_request_adapters { +/// @brief Transforms frames used in constraints to link frames in the robot model. class ResolveConstraintFrames : public planning_interface::PlanningRequestAdapter { public: @@ -54,14 +59,16 @@ class ResolveConstraintFrames : public planning_interface::PlanningRequestAdapte std::string getDescription() const override { - return "Resolve constraint frames to robot links"; + return std::string("ResolveConstraintFrames"); } bool adapt(const planning_scene::PlanningSceneConstPtr& planning_scene, planning_interface::MotionPlanRequest& req) const override { RCLCPP_DEBUG(logger_, "Running '%s'", getDescription().c_str()); + // Resolve path constraint frames kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), req.path_constraints); + // Resolve goal constraint frames for (moveit_msgs::msg::Constraints& constraint : req.goal_constraints) { kinematic_constraints::resolveConstraintFrames(planning_scene->getCurrentState(), constraint); diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp similarity index 81% rename from moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp rename to moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp index ed6affb6035..b7aed8a7e17 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp @@ -33,8 +33,8 @@ *********************************************************************/ /* Author: Ioan Sucan - * Desc: A fix workspace bounds adapter which will specify a default workspace for planning: a cube of size 10 m x 10 m x - * 10 m. This workspace will only be specified if the planning request to the planner does not have these fields filled in. + * Desc: If not specified by the planning request, this request adapter will specify a default workspace for planning. + * The default workspace is a cube whose edge length is defined with a ROS 2 parameter. */ #include @@ -49,12 +49,11 @@ namespace default_planning_request_adapters { -/** @brief This fix workspace bounds adapter will specify a default workspace for planning: a cube of size 10 m x 10 m x - * 10 m. This workspace will only be specified if the planning request to the planner does not have these fields filled in. */ -class FixWorkspaceBounds : public planning_interface::PlanningRequestAdapter +/** @brief If not specified by the planning request, this request adapter will specify a default workspace for planning. */ +class ValidateWorkspaceBounds : public planning_interface::PlanningRequestAdapter { public: - FixWorkspaceBounds() : logger_(moveit::makeChildLogger("fix_workspace_bounds")) + ValidateWorkspaceBounds() : logger_(moveit::makeChildLogger("validate_workspace_bounds")) { } @@ -65,7 +64,7 @@ class FixWorkspaceBounds : public planning_interface::PlanningRequestAdapter std::string getDescription() const override { - return "Fix Workspace Bounds"; + return std::string("ValidateWorkspaceBounds"); } bool adapt(const planning_scene::PlanningSceneConstPtr& /*planning_scene*/, @@ -80,7 +79,7 @@ class FixWorkspaceBounds : public planning_interface::PlanningRequestAdapter std::abs(wparams.max_corner.y) < std::numeric_limits::epsilon() && std::abs(wparams.max_corner.z) < std::numeric_limits::epsilon()) { - RCLCPP_DEBUG(logger_, "It looks like the planning volume was not specified. Using default values."); + RCLCPP_WARN(logger_, "It looks like the planning volume was not specified. Using default values."); moveit_msgs::msg::WorkspaceParameters& default_wp = req.workspace_parameters; const auto params = param_listener_->get_params(); @@ -98,5 +97,5 @@ class FixWorkspaceBounds : public planning_interface::PlanningRequestAdapter }; } // namespace default_planning_request_adapters -CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::FixWorkspaceBounds, +CLASS_LOADER_REGISTER_CLASS(default_planning_request_adapters::ValidateWorkspaceBounds, planning_interface::PlanningRequestAdapter) diff --git a/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml index cc9c6f81ad8..c70502fca8c 100644 --- a/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml +++ b/moveit_setup_assistant/unported_templates/ompl-chomp_planning_pipeline.launch.xml @@ -3,9 +3,9 @@