diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 834744b2462..3967ff63b05 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -59,6 +59,7 @@ add_subdirectory(collision_detection) add_subdirectory(collision_distance_field) add_subdirectory(constraint_samplers) add_subdirectory(controller_manager) +add_subdirectory(cost_functions) add_subdirectory(distance_field) add_subdirectory(dynamics_solver) add_subdirectory(exceptions) @@ -116,6 +117,7 @@ install( moveit_collision_detection_fcl moveit_collision_distance_field moveit_constraint_samplers + moveit_cost_functions moveit_distance_field moveit_dynamics_solver moveit_exceptions diff --git a/moveit_core/cost_functions/CMakeLists.txt b/moveit_core/cost_functions/CMakeLists.txt new file mode 100644 index 00000000000..448c906921c --- /dev/null +++ b/moveit_core/cost_functions/CMakeLists.txt @@ -0,0 +1,20 @@ +add_library(moveit_cost_functions SHARED + src/cost_functions.cpp +) +target_include_directories(moveit_cost_functions PUBLIC + $ + $ +) +set_target_properties(moveit_cost_functions PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_cost_functions + moveit_planning_interface + moveit_planning_scene + moveit_robot_state +) + +install(DIRECTORY include/ DESTINATION include/moveit_core) + + +#if(BUILD_TESTING) +# TODO(sjahr): Add tests +#endif() diff --git a/moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp b/moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp new file mode 100644 index 00000000000..9010d07908a --- /dev/null +++ b/moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp @@ -0,0 +1,55 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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 Jahr + Desc: Cost functions for MoveIt */ + +#pragma once + +#include +#include +#include + +namespace moveit +{ +namespace cost_functions +{ +[[nodiscard]] ::planning_interface::StateCostFn +getClearanceCostFn(moveit::core::RobotState& robot_state, const std::string& group_name, + const planning_scene::PlanningSceneConstPtr& planning_scene); + +[[nodiscard]] ::planning_interface::StateCostFn +getWeightedCostFnSum(std::vector> weight_cost_vector); +} // namespace cost_functions +} // namespace moveit diff --git a/moveit_core/cost_functions/src/cost_functions.cpp b/moveit_core/cost_functions/src/cost_functions.cpp new file mode 100644 index 00000000000..b2510466a74 --- /dev/null +++ b/moveit_core/cost_functions/src/cost_functions.cpp @@ -0,0 +1,79 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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 Jahr */ + +#include + +namespace moveit +{ +namespace cost_functions +{ +[[nodiscard]] ::planning_interface::StateCostFn +getClearanceCostFn(moveit::core::RobotState& robot_state, const std::string& group_name, + const planning_scene::PlanningSceneConstPtr& planning_scene) +{ + // Create cost function + return [robot_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable { + robot_state.setJointGroupActivePositions(group_name, state_vector); + auto const shortest_distance_to_collision = planning_scene->distanceToCollisionUnpadded(robot_state); + + // Return cost based on shortest_distance if the robot is not in contact or penetrated a collision object + if (shortest_distance_to_collision > 0.0) + { + // The closer the collision object the higher the cost + return 1.0 / shortest_distance_to_collision; + } + return std::numeric_limits::infinity(); // Return a max cost cost by default + }; +} + +/* +[[nodiscard]] ::planning_interface::StateCostFn +getWeightedCostFnSum(std::vector> weight_cost_vector) +{ + return [weight_cost_vector](const moveit::core::RobotState& robot_state, + const planning_interface::MotionPlanRequest& request, + const planning_scene::PlanningSceneConstPtr& planning_scene) { + auto weighted_sum = 0.0; + for (const auto& weight_cost_pair : weight_cost_vector) + { + weighted_sum += weight_cost_pair.first * weight_cost_pair.second(robot_state, request, planning_scene); + } + return weighted_sum; + }; +}*/ + +} // namespace cost_functions +} // namespace moveit diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 504a970d765..4345c98ab6e 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -1,6 +1,7 @@ add_library(moveit_planning_interface SHARED src/planning_interface.cpp src/planning_response.cpp + src/planning_request.cpp ) target_include_directories(moveit_planning_interface PUBLIC $ diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index ea2224a9dd6..a8b572e2521 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -213,5 +213,4 @@ class PlannerManager form "group_name" if default settings are to be used. */ PlannerConfigurationMap config_settings_; }; - } // namespace planning_interface diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h index af7e4881bdd..b288811c6db 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h @@ -36,12 +36,22 @@ #pragma once +#include #include namespace planning_interface { -// for now this is just a typedef -typedef moveit_msgs::msg::MotionPlanRequest MotionPlanRequest; +/** \brief Definition for a cost function to measure the cost of a single state during motion planning */ +using StateCostFn = std::function; + +struct MotionPlanRequest : moveit_msgs::msg::MotionPlanRequest +{ + MotionPlanRequest(moveit_msgs::msg::MotionPlanRequest request_msg = moveit_msgs::msg::MotionPlanRequest(), + planning_interface::StateCostFn state_cost_function = nullptr); + planning_interface::StateCostFn state_cost_function = nullptr; + + [[nodiscard]] moveit_msgs::msg::MotionPlanRequest toMessage() const; +}; } // namespace planning_interface diff --git a/moveit_core/planning_interface/src/planning_request.cpp b/moveit_core/planning_interface/src/planning_request.cpp new file mode 100644 index 00000000000..5a7a2907b19 --- /dev/null +++ b/moveit_core/planning_interface/src/planning_request.cpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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 Jahr */ + +#include + +namespace planning_interface +{ +MotionPlanRequest::MotionPlanRequest(moveit_msgs::msg::MotionPlanRequest request_msg, + planning_interface::StateCostFn state_cost_function) + : moveit_msgs::msg::MotionPlanRequest(request_msg), state_cost_function{ state_cost_function } +{ +} + +moveit_msgs::msg::MotionPlanRequest MotionPlanRequest::toMessage() const +{ + moveit_msgs::msg::MotionPlanRequest request_msg; + request_msg.workspace_parameters = workspace_parameters; + request_msg.start_state = start_state; + request_msg.goal_constraints = goal_constraints; + request_msg.path_constraints = path_constraints; + request_msg.trajectory_constraints = trajectory_constraints; + request_msg.reference_trajectories = reference_trajectories; + request_msg.pipeline_id = pipeline_id; + request_msg.planner_id = planner_id; + request_msg.group_name = group_name; + request_msg.num_planning_attempts = num_planning_attempts; + request_msg.allowed_planning_time = allowed_planning_time; + request_msg.max_velocity_scaling_factor = max_velocity_scaling_factor; + request_msg.max_acceleration_scaling_factor = max_acceleration_scaling_factor; + request_msg.cartesian_speed_end_effector_link = cartesian_speed_end_effector_link; + request_msg.max_cartesian_speed = max_cartesian_speed; + return request_msg; +} +} // namespace planning_interface diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index 0a7a7bddfc5..ad34ceb8182 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -75,7 +75,7 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda } catch (std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Exception caught executing adapter '%s': %s\nSkipping adapter instead.", + RCLCPP_ERROR(LOGGER, "Exception caught executing adapter '%s': %s\n Skipping adapter instead.", adapter.getDescription().c_str(), ex.what()); added_path_index.clear(); return planner(planning_scene, req, res); @@ -129,6 +129,7 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner for (int i = adapters_.size() - 1; i >= 0; --i) { + // Iterate through adapters in the chain and create a chain of callAdapter calls. fn = [&adapter = *adapters_[i], fn, &added_path_index = added_path_index_each[i]]( const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) { diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 933e19b6e1b..1d241cac7a6 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(moveit_ompl_interface SHARED src/detail/constraints_library.cpp src/detail/constrained_sampler.cpp src/detail/constrained_goal_sampler.cpp + src/detail/planning_interface_objective.cpp ) set_target_properties(moveit_ompl_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/planning_interface_objective.hpp b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/planning_interface_objective.hpp new file mode 100644 index 00000000000..f10c9b6fb7a --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/planning_interface_objective.hpp @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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 Jahr + Description: TODO +*/ + +#pragma once + +#include +#include +#include + +namespace ompl_interface +{ +/** \brief Flexible objective that can be configured using the MoveIt planning interface cost API. */ +class PlanningInterfaceObjective : public ompl::base::OptimizationObjective +{ +public: + PlanningInterfaceObjective(const ompl::base::SpaceInformationPtr& si, + const planning_interface::StateCostFn& state_cost_function); + + /** \brief Returns for a state calculated with the state_cost_function */ + ompl::base::Cost stateCost(const ompl::base::State* state) const override; + + /** \brief Adapted from ompl::base::StateCostIntegralObjective: Compute the cost of a path segment from \e state_1 to + \e state_2 (including endpoints) \param state_1 start state of the motion to be evaluated \param state_2 final + state of the motion to be evaluated \param cost the cost of the motion segment + + By default, this function computes + \f{eqnarray*}{ + \mbox{cost} &=& \frac{cost(s_1) + cost(s_2)}{2}\vert s_1 - s_2 \vert + \f} + */ + ompl::base::Cost motionCost(const ompl::base::State* state_1, const ompl::base::State* state_2) const override; + + /** \brief Adapted from ompl::base::StateCostIntegralObjective: Estimate the cost of a path segment from \e state_1 to + \e state_2 (including endpoints). \param state_1 start state of the motion to be evaluated \param state_2 final + state of the motion to be evaluated \param cost the cost of the motion segment + + This function computes + \f{eqnarray*}{ + \mbox{cost} &=& \frac{cost(s_1) + cost(s_2)}{2}\vert s_1 - s_2 \vert + \f} + regardless of whether enableMotionCostInterpolation was specified as true in + constructing this object. + */ + ompl::base::Cost motionCostBestEstimate(const ompl::base::State* state_1, const ompl::base::State* state_2) const; + +protected: + const planning_interface::StateCostFn state_cost_function_; + + /** \brief Adapted from ompl::base::StateCostIntegralObjective: Helper method which uses the trapezoidal rule + to approximate the integral of the cost between two + states of distance \e dist and costs \e c1 and \e + c2 */ + ompl::base::Cost trapezoid(ompl::base::Cost cost_1, ompl::base::Cost cost_2, double distance) const + { + return ompl::base::Cost(0.5 * distance * (cost_1.value() + cost_2.value())); + } +}; +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h index e7a80672665..d1dc3028c0a 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space_factory.h @@ -58,7 +58,7 @@ class ConstrainedPlanningStateSpaceFactory : public ModelBasedStateSpaceFactory * For more details on this state space selection process, see: * https://github.com/JeroenDM/moveit/pull/2 * **/ - int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + int canRepresentProblem(const std::string& group, const planning_interface::MotionPlanRequest& req, const moveit::core::RobotModelConstPtr& robot_model) const override; protected: diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h index d45f8b737ae..b6f5fa9e2eb 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/joint_space/joint_model_state_space_factory.h @@ -45,7 +45,7 @@ class JointModelStateSpaceFactory : public ModelBasedStateSpaceFactory public: JointModelStateSpaceFactory(); - int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + int canRepresentProblem(const std::string& group, const planning_interface::MotionPlanRequest& req, const moveit::core::RobotModelConstPtr& robot_model) const override; protected: diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h index e334322a986..5bc8adbeae0 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h @@ -39,6 +39,7 @@ #include #include #include +#include #include #include diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h index bcef1410acd..fd586aff2e1 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space_factory.h @@ -66,7 +66,7 @@ class ModelBasedStateSpaceFactory the user request \e req for group \e group. The group \e group must always be specified and takes precedence over \e req.group_name, which may be different */ - virtual int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + virtual int canRepresentProblem(const std::string& group, const planning_interface::MotionPlanRequest& req, const moveit::core::RobotModelConstPtr& robot_model) const = 0; protected: diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h index 12a6daee9ea..1ac709e6d17 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space_factory.h @@ -45,7 +45,7 @@ class PoseModelStateSpaceFactory : public ModelBasedStateSpaceFactory public: PoseModelStateSpaceFactory(); - int canRepresentProblem(const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + int canRepresentProblem(const std::string& group, const planning_interface::MotionPlanRequest& req, const moveit::core::RobotModelConstPtr& robot_model) const override; protected: diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h index f14aad9d5b9..8bfa1a82ac5 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h @@ -40,6 +40,7 @@ #include #include #include +#include #include @@ -218,11 +219,11 @@ class PlanningContextManager /** \brief This is the function that constructs new planning contexts if no previous ones exist that are suitable */ ModelBasedPlanningContextPtr getPlanningContext(const planning_interface::PlannerConfigurationSettings& config, const ModelBasedStateSpaceFactoryPtr& factory, - const moveit_msgs::msg::MotionPlanRequest& req) const; + const planning_interface::MotionPlanRequest& req) const; const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& factory_type) const; const ModelBasedStateSpaceFactoryPtr& getStateSpaceFactory(const std::string& group_name, - const moveit_msgs::msg::MotionPlanRequest& req) const; + const planning_interface::MotionPlanRequest& req) const; /** \brief The kinematic model for which motion plans are computed */ moveit::core::RobotModelConstPtr robot_model_; diff --git a/moveit_planners/ompl/ompl_interface/src/detail/planning_interface_objective.cpp b/moveit_planners/ompl/ompl_interface/src/detail/planning_interface_objective.cpp new file mode 100644 index 00000000000..2949a779849 --- /dev/null +++ b/moveit_planners/ompl/ompl_interface/src/detail/planning_interface_objective.cpp @@ -0,0 +1,82 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik Inc. 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 Jahr */ + +#include +#include + +namespace ompl_interface +{ +PlanningInterfaceObjective::PlanningInterfaceObjective(const ompl::base::SpaceInformationPtr& si, + const ::planning_interface::StateCostFn& state_cost_function) + : OptimizationObjective(si), state_cost_function_(state_cost_function) +{ + description_ = "Planning Interface Objective"; +} + +ompl::base::Cost PlanningInterfaceObjective::stateCost(const ompl::base::State* state_ptr) const +{ + if (!state_cost_function_) + { + // TODO(sjahr): Print warning or move check else where + return ompl::base::Cost(1.0); + } + Eigen::VectorXd state_vector; + state_vector.resize(si_->getStateDimension()); + // Update state vector + auto space_values = state_ptr->as()->values; + for (std::size_t i = 0; i < si_->getStateDimension(); ++i) + { + state_vector(i) = space_values[i]; + } + return ompl::base::Cost(state_cost_function_(state_vector)); +} + +ompl::base::Cost PlanningInterfaceObjective::motionCost(const ompl::base::State* state_1, + const ompl::base::State* state_2) const +{ + // Adapted from ompl::base::StateCostIntegralObjective: Uses the trapezoidal rule to approximate the integral of the + // cost between two states of distance and cost of state 1 and cost of state 2*/ + return trapezoid(stateCost(state_1), stateCost(state_2), si_->distance(state_1, state_2)); +} + +ompl::base::Cost PlanningInterfaceObjective::motionCostBestEstimate(const ompl::base::State* state_1, + const ompl::base::State* state_2) const +{ + // Adapted from ompl::base::StateCostIntegralObjective: Uses the trapezoidal rule to approximate the integral of the + // cost between two states of distance and cost of state 1 and cost of state 2*/ + return trapezoid(stateCost(state_1), stateCost(state_2), si_->distance(state_1, state_2)); +} +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 58b1e06c53e..36aa42fc627 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -44,10 +44,12 @@ #include #include #include +#include #include #include #include +#include #include @@ -331,38 +333,57 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() optimizer = it->second; cfg.erase(it); - if (optimizer == "PathLengthOptimizationObjective") + // Warn if this config will be overwritten + if (request_.state_cost_function != nullptr) { - objective = - std::make_shared(ompl_simple_setup_->getSpaceInformation()); - } - else if (optimizer == "MinimaxObjective") - { - objective = std::make_shared(ompl_simple_setup_->getSpaceInformation()); - } - else if (optimizer == "StateCostIntegralObjective") - { - objective = std::make_shared(ompl_simple_setup_->getSpaceInformation()); - } - else if (optimizer == "MechanicalWorkOptimizationObjective") - { - objective = - std::make_shared(ompl_simple_setup_->getSpaceInformation()); - } - else if (optimizer == "MaximizeMinClearanceObjective") - { - objective = - std::make_shared(ompl_simple_setup_->getSpaceInformation()); + RCLCPP_WARN(LOGGER, "%s: User defined optimization function will overwrite OMPL config optimization objective %s", + name_.c_str(), optimizer.c_str()); } else { - RCLCPP_WARN(LOGGER, - "Optimization objective %s is invalid or not defined, using PathLengthOptimizationObjective instead", - optimizer.c_str()); - objective = - std::make_shared(ompl_simple_setup_->getSpaceInformation()); + if (optimizer == "PathLengthOptimizationObjective") + { + objective = + std::make_shared(ompl_simple_setup_->getSpaceInformation()); + } + else if (optimizer == "MinimaxObjective") + { + objective = std::make_shared(ompl_simple_setup_->getSpaceInformation()); + } + else if (optimizer == "StateCostIntegralObjective") + { + objective = std::make_shared(ompl_simple_setup_->getSpaceInformation()); + } + else if (optimizer == "MechanicalWorkOptimizationObjective") + { + objective = std::make_shared( + ompl_simple_setup_->getSpaceInformation()); + } + else if (optimizer == "MaximizeMinClearanceObjective") + { + objective = + std::make_shared(ompl_simple_setup_->getSpaceInformation()); + } + else + { + RCLCPP_WARN( + LOGGER, + "Optimization objective %s is invalid or not defined, using PathLengthOptimizationObjective instead", + optimizer.c_str()); + objective = + std::make_shared(ompl_simple_setup_->getSpaceInformation()); + } + + ompl_simple_setup_->setOptimizationObjective(objective); } + } + if (request_.state_cost_function != nullptr) + { + // TODO(sjahr): Remove debugging print output + RCLCPP_WARN(LOGGER, "%s: Using user defined state cost function!", name_.c_str()); + objective = std::make_shared(ompl_simple_setup_->getSpaceInformation(), + request_.state_cost_function); ompl_simple_setup_->setOptimizationObjective(objective); } diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index ba06cf3d439..837170ebdd6 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -90,7 +90,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager return true; } - bool canServiceRequest(const moveit_msgs::msg::MotionPlanRequest& req) const override + bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override { return req.trajectory_constraints.constraints.empty(); } diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp index 7830f81c372..4ee413c5fbd 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp @@ -46,7 +46,7 @@ ConstrainedPlanningStateSpaceFactory::ConstrainedPlanningStateSpaceFactory() : M } int ConstrainedPlanningStateSpaceFactory::canRepresentProblem( - const std::string& /*group*/, const moveit_msgs::msg::MotionPlanRequest& req, + const std::string& /*group*/, const planning_interface::MotionPlanRequest& req, const moveit::core::RobotModelConstPtr& /*robot_model*/) const { // If we have exactly one position or orientation constraint, prefer the constrained planning state space diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp index 85f54e8c3f0..0e2d8d86537 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/joint_space/joint_model_state_space_factory.cpp @@ -43,7 +43,7 @@ ompl_interface::JointModelStateSpaceFactory::JointModelStateSpaceFactory() : Mod } int ompl_interface::JointModelStateSpaceFactory::canRepresentProblem( - const std::string& /*group*/, const moveit_msgs::msg::MotionPlanRequest& /*req*/, + const std::string& /*group*/, const planning_interface::MotionPlanRequest& /*req*/, const moveit::core::RobotModelConstPtr& /*robot_model*/) const { return 100; diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp index 953b9382f3f..d66481f4aa9 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space_factory.cpp @@ -43,7 +43,7 @@ ompl_interface::PoseModelStateSpaceFactory::PoseModelStateSpaceFactory() : Model } int ompl_interface::PoseModelStateSpaceFactory::canRepresentProblem( - const std::string& group, const moveit_msgs::msg::MotionPlanRequest& req, + const std::string& group, const planning_interface::MotionPlanRequest& req, const moveit::core::RobotModelConstPtr& robot_model) const { const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index ba2d5949b74..d9f7a5c360d 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -334,7 +334,7 @@ void PlanningContextManager::setPlannerConfigurations(const planning_interface:: ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext(const planning_interface::PlannerConfigurationSettings& config, const ModelBasedStateSpaceFactoryPtr& factory, - const moveit_msgs::msg::MotionPlanRequest& req) const + const planning_interface::MotionPlanRequest& req) const { // Check for a cached planning context ModelBasedPlanningContextPtr context; @@ -439,7 +439,7 @@ const ModelBasedStateSpaceFactoryPtr& PlanningContextManager::getStateSpaceFacto const ModelBasedStateSpaceFactoryPtr& PlanningContextManager::getStateSpaceFactory(const std::string& group, - const moveit_msgs::msg::MotionPlanRequest& req) const + const planning_interface::MotionPlanRequest& req) const { // find the problem representation to use auto best = state_space_factories_.end(); @@ -469,7 +469,7 @@ PlanningContextManager::getStateSpaceFactory(const std::string& group, } ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext( - const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::msg::MotionPlanRequest& req, + const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, moveit_msgs::msg::MoveItErrorCodes& error_code, const rclcpp::Node::SharedPtr& node, bool use_constraints_approximation) const { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index 9ddfc1baf95..c0c48c4a0cc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -123,7 +123,7 @@ void CommandPlanner::getPlanningAlgorithms(std::vector& algs) const planning_interface::PlanningContextPtr CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::msg::MotionPlanRequest& req, + const planning_interface::MotionPlanRequest& req, moveit_msgs::msg::MoveItErrorCodes& error_code) const { // TODO(henningkayser): print req @@ -154,7 +154,7 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& } } -bool CommandPlanner::canServiceRequest(const moveit_msgs::msg::MotionPlanRequest& req) const +bool CommandPlanner::canServiceRequest(const planning_interface::MotionPlanRequest& req) const { if (context_loader_map_.find(req.planner_id) == context_loader_map_.end()) { diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp index b7113ca3a22..e4b1d248373 100644 --- a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp @@ -41,6 +41,7 @@ #include +#include #include #include @@ -221,6 +222,27 @@ CostFn get_constraints_cost_function(const std::shared_ptr cost_functions; + cost_functions.reserve(3); // Expect a collision, a constraint and a state cost function + cost_functions.push_back(costs::get_collision_cost_function(planning_scene, group, 1.0 /* collision penalty */)); + if (!constraints.empty()) { - cost_fn = costs::sum({ costs::get_collision_cost_function(planning_scene, group, 1.0 /* collision penalty */), - costs::get_constraints_cost_function(planning_scene, group, constraints.getAllConstraints(), - 1.0 /* constraint penalty */) }); + cost_functions.push_back(costs::get_constraints_cost_function( + planning_scene, group, constraints.getAllConstraints(), 1.0 /* constraint penalty */)); } - else + + if (context.getMotionPlanRequest().state_cost_function != nullptr) { - cost_fn = costs::get_collision_cost_function(planning_scene, group, 1.0 /* collision penalty */); + std::cout << "Using user defined state cost function!" << std::endl; + cost_functions.push_back( + costs::get_cost_function_from_moveit_state_cost_fn(context.getMotionPlanRequest().state_cost_function)); } // TODO(henningkayser): parameterize stddev @@ -174,8 +179,8 @@ stomp::TaskPtr createStompTask(const stomp::StompConfiguration& config, StompPla visualization::get_success_trajectory_publisher(context.getPathPublisher(), planning_scene, group); // Initialize and return STOMP task - stomp::TaskPtr task = - std::make_shared(noise_generator_fn, cost_fn, filter_fn, iteration_callback_fn, done_callback_fn); + stomp::TaskPtr task = std::make_shared(noise_generator_fn, costs::sum(cost_functions), filter_fn, + iteration_callback_fn, done_callback_fn); return task; } diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 4d134810c02..46f960b8247 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -189,6 +190,13 @@ class PlanningComponent /** \brief Set the trajectory constraints generated from a moveit msg Constraints */ bool setTrajectoryConstraints(const moveit_msgs::msg::TrajectoryConstraints& trajectory_constraints); + /** \brief Set planning cost function */ + bool setStateCostFunction(const planning_interface::StateCostFn& state_cost_function) + { + state_cost_function_ = state_cost_function; + return true; + }; + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using * default parameters. */ planning_interface::MotionPlanResponse plan(); @@ -230,6 +238,7 @@ class PlanningComponent const std::string group_name_; // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources const moveit::core::JointModelGroup* joint_model_group_; + planning_interface::StateCostFn state_cost_function_; // Planning // The start state used in the planning motion request diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index f7101404ba0..4bb2d97b267 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -338,6 +338,8 @@ PlanningComponent::getMotionPlanRequest(const PlanRequestParameters& plan_reques request.path_constraints = current_path_constraints_; request.trajectory_constraints = current_trajectory_constraints_; + request.state_cost_function = state_cost_function_; + // Set start state moveit::core::RobotStatePtr start_state = considered_start_state_; if (!start_state) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index bf4407ff250..64076aead10 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -248,8 +248,8 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const { - std::vector dummy; - return generatePlan(planning_scene, req, res, dummy); + std::vector unused_adapter_added_state_index; + return generatePlan(planning_scene, req, res, unused_adapter_added_state_index); } bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene, @@ -263,7 +263,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla // broadcast the request we are about to work on, if needed if (publish_received_requests_) { - received_request_publisher_->publish(req); + received_request_publisher_->publish(req.toMessage()); } adapter_added_state_index.clear(); diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp index e4b0ec11e84..88f5fb62db5 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include