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

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,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 <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
+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,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 <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->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<double>::infinity();  // Return a max cost cost by default
+  };
+}
+
+/*
+[[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(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

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

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}")
 

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 <moveit/cost_functions/cost_functions.hpp>
+#include <moveit/planning_scene/planning_scene.h>
+#include <ompl/base/OptimizationObjective.h>
+
+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