Add draft cost function interface

Add state cost function to planning context

Add clearance cost function

Add OMPL objective

Make use of custom cost function in OMPL objective

Apply suggestions from code review

Co-authored-by: Mark Moll <Mark.Moll@gmail.com>

Make accessible with moveit_cpp

TMP: Debugging prints

Format!

Update stomp planning interface

Update cost function API

Update use_config

Cleanup function signature

Cleanups

Cost function interface for STOMP

Update function signature to Eigen

Pass cost function with motion plan request to planners

Set state cost function in PlanningInterfaceObjective constructor

cartesian_speed_end_effector_link -> cartesian_speed_limited_link

Make clang-tidy happy

Add getMinJointDisplacementCostFn

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

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/moveit_core>
+)
+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()

moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp

@@ -0,0 +1,59 @@
+/*********************************************************************
+ * 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 <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_interface/planning_request.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+namespace moveit
+{
+namespace cost_functions
+{
+[[nodiscard]] ::planning_interface::StateCostFn
+getMinJointDisplacementCostFn(moveit::core::RobotState& robot_state, const std::string& group_name,
+                              const planning_scene::PlanningSceneConstPtr& planning_scene);
+
+[[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<std::pair<double, ::planning_interface::StateCostFn>> weight_cost_vector);
+}  // namespace cost_functions
+}  // namespace moveit

moveit_core/cost_functions/src/cost_functions.cpp

@@ -0,0 +1,91 @@
+/*********************************************************************
+ * 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 <moveit/cost_functions/cost_functions.hpp>
+
+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->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<double>::infinity();  // Return a max cost cost by default
+  };
+}
+
+[[nodiscard]] ::planning_interface::StateCostFn
+getMinJointDisplacementCostFn(moveit::core::RobotState& robot_state, const std::string& group_name,
+                              const planning_scene::PlanningSceneConstPtr& planning_scene)
+{
+  return [robot_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable {
+    robot_state.setJointGroupActivePositions(group_name, state_vector);
+    auto const current_state = planning_scene->getCurrentState();
+
+    return current_state.distance(robot_state);
+  };
+}
+
+/*
+[[nodiscard]] ::planning_interface::StateCostFn
+getWeightedCostFnSum(std::vector<std::pair<double, ::planning_interface::StateCostFn>> 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

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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

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

moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h

@@ -36,12 +36,22 @@
 
 #pragma once
 
+#include <Eigen/Core>
 #include <moveit_msgs/msg/motion_plan_request.hpp>
 
 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<double(const Eigen::VectorXd& state_vector)>;
+
+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

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 <moveit/planning_interface/planning_request.h>
+
+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) }
+{
+}
+
+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_limited_link = cartesian_speed_limited_link;
+  request_msg.max_cartesian_speed = max_cartesian_speed;
+  return request_msg;
+}
+}  // namespace planning_interface

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:

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:

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/model_based_state_space.h

@@ -39,6 +39,7 @@
 #include <ompl/base/StateSpace.h>
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_state/robot_state.h>
+#include <moveit/planning_interface/planning_request.h>
 #include <moveit/kinematic_constraints/kinematic_constraint.h>
 #include <moveit/constraint_samplers/constraint_sampler.h>
 

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:

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:

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h

@@ -40,6 +40,7 @@
 #include <moveit/ompl_interface/parameterization/model_based_state_space_factory.h>
 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
 #include <moveit/macros/class_forward.h>
+#include <moveit/planning_interface/planning_request.h>
 
 #include <ompl/base/PlannerDataStorage.h>
 
@@ -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_;

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();
   }

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

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;

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);