State cost function interface

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

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,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 <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
+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

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 <moveit/cost_functions/cost_functions.hpp>
+
+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<double>::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

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_request.h

@@ -36,12 +36,20 @@
 
 #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;
+};
 
 }  // namespace planning_interface

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 <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) }
+{
+}
+}  // 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

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

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

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
 {

moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp

@@ -130,7 +130,7 @@ void CommandPlanner::getPlanningAlgorithms(std::vector<std::string>& 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())
   {