diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 138349555c..b45616f1ab 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -51,6 +51,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) @@ -107,6 +108,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 0000000000..448c906921 --- /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 0000000000..b30032ceb1 --- /dev/null +++ b/moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp @@ -0,0 +1,56 @@ +/********************************************************************* + * 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 +createMinJointDisplacementCostFn(moveit::core::RobotState& robot_state, const std::string& group_name, + const planning_scene::PlanningSceneConstPtr& planning_scene); + +[[nodiscard]] ::planning_interface::StateCostFn +createClearanceCostFn(moveit::core::RobotState& robot_state, const std::string& group_name, + const planning_scene::PlanningSceneConstPtr& planning_scene); +} // 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 0000000000..55fd0b71ac --- /dev/null +++ b/moveit_core/cost_functions/src/cost_functions.cpp @@ -0,0 +1,76 @@ +/********************************************************************* + * 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 +createClearanceCostFn(moveit::core::RobotState& robot_state, const std::string& group_name, + const planning_scene::PlanningSceneConstPtr& planning_scene) +{ + // Create cost function + return [&](const Eigen::VectorXd& state_vector) mutable { + assert(state_vector.size() == robot_state.getRobotModel()->getJointModelGroup(group_name)->getActiveVariableCount()); + robot_state.setJointGroupActivePositions(group_name, state_vector); + auto const shortest_distance_to_collision = planning_scene->distanceToCollision(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 +createMinJointDisplacementCostFn(moveit::core::RobotState& robot_state, const std::string& group_name, + const planning_scene::PlanningSceneConstPtr& planning_scene) +{ + return [&](const Eigen::VectorXd& state_vector) mutable { + assert(state_vector.size() == robot_state.getRobotModel()->getJointModelGroup(group_name)->getActiveVariableCount()); + robot_state.setJointGroupActivePositions(group_name, state_vector); + const auto& current_state = planning_scene->getCurrentState(); + + return current_state.distance(robot_state, robot_state.getJointModelGroup(group_name)); + }; +} +} // namespace cost_functions +} // namespace moveit diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 46e7ff7789..a219d7bd92 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_request.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h index 39200155ad..0c9dd8c420 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,20 @@ #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; +}; } // 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 0000000000..71ab7c5ae7 --- /dev/null +++ b/moveit_core/planning_interface/src/planning_request.cpp @@ -0,0 +1,46 @@ +/********************************************************************* + * 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{ std::move(request_msg) }, state_cost_function{ std::move(state_cost_function) } +{ +} +} // namespace planning_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 e7a8067266..d1dc3028c0 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 d45f8b737a..b6f5fa9e2e 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 e334322a98..5bc8adbeae 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 bcef1410ac..fd586aff2e 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 12a6daee9e..1ac709e6d1 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 f14aad9d5b..8bfa1a82ac 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/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 9e69e99d72..e8fe04acca 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -96,7 +96,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 7830f81c37..4ee413c5fb 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 85f54e8c3f..0e2d8d8653 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 953b9382f3..d66481f4aa 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 bb435c6a4c..04b9735ded 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -341,7 +341,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; @@ -446,7 +446,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(); @@ -476,7 +476,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 f57118636c..67ce4f0051 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 @@ -130,7 +130,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 @@ -162,7 +162,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 4d1dd2993a..6c788140dc 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 @@ -88,8 +89,8 @@ CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator // with a gaussian, penalizing neighboring valid states as well. for (int timestep = 0; timestep < values.cols() - 1; ++timestep) { - Eigen::VectorXd current = values.col(timestep); - Eigen::VectorXd next = values.col(timestep + 1); + const Eigen::VectorXd& current = values.col(timestep); + const Eigen::VectorXd& next = values.col(timestep + 1); const double segment_distance = (next - current).norm(); double interpolation_fraction = 0.0; const double interpolation_step = std::min(0.5, interpolation_step_size / segment_distance); @@ -219,7 +220,28 @@ CostFn getConstraintsCostFunction(const std::shared_ptr cost_functions; + cost_functions.push_back(costs::getCollisionCostFunction(planning_scene, group, 1.0 /* collision penalty */)); + if (!constraints.empty()) { - cost_fn = costs::sum({ costs::getCollisionCostFunction(planning_scene, group, 1.0 /* collision penalty */), - costs::getConstraintsCostFunction(planning_scene, group, constraints.getAllConstraints(), - 1.0 /* constraint penalty */) }); + cost_functions.push_back(costs::getConstraintsCostFunction(planning_scene, group, constraints.getAllConstraints(), + 1.0 /* constraint penalty */)); } - else + + if (context.getMotionPlanRequest().state_cost_function != nullptr) { - cost_fn = costs::getCollisionCostFunction(planning_scene, group, 1.0 /* collision penalty */); + cost_functions.push_back( + costs::getCostFunctionFromMoveitStateCostFn(context.getMotionPlanRequest().state_cost_function)); } // TODO(henningkayser): parameterize stddev @@ -182,8 +185,8 @@ stomp::TaskPtr createStompTask(const stomp::StompConfiguration& config, StompPla visualization::getSuccessTrajectoryPublisher(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 88668e1f01..e89357b2a3 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_ = nullptr; // 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 270b9357cc..7791e44779 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -341,6 +341,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_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 9c9760e2e4..fe877b2c29 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