Pass cost function with motion plan request to planners

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

@@ -117,11 +117,6 @@ class PlanningContext
   /** \brief Set the planning request for this context */
   void setMotionPlanRequest(const MotionPlanRequest& request);
 
-  /** \brief Set the planning request for this context */
-  void setStateCostFunction(const StateCostFn& state_cost_function);
-
-  [[nodiscard]] const StateCostFn& getStateCostFunction() const;
-
   /** \brief Solve the motion planning problem and store the result in \e res. This function should not clear data
    * structures before computing. The constructor and clear() do that. */
   virtual bool solve(MotionPlanResponse& res) = 0;
@@ -149,9 +144,6 @@ class PlanningContext
 
   /// The planning request for this context
   MotionPlanRequest request_;
-
-  // Cost function for individual states
-  StateCostFn state_cost_function_ = StateCostFn();
 };
 
 MOVEIT_CLASS_FORWARD(PlannerManager);  // Defines PlannerManagerPtr, ConstPtr, WeakPtr... etc

moveit_core/planning_interface/src/planning_interface.cpp

@@ -79,16 +79,6 @@ void PlanningContext::setPlanningScene(const planning_scene::PlanningSceneConstP
   planning_scene_ = planning_scene;
 }
 
-void PlanningContext::setStateCostFunction(const StateCostFn& state_cost_function)
-{
-  state_cost_function_ = state_cost_function;
-}
-
-const StateCostFn& PlanningContext::getStateCostFunction() const
-{
-  return state_cost_function_;
-}
-
 void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request)
 {
   request_ = request;

moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h

@@ -58,7 +58,7 @@ class PlanningRequestAdapter
   /// \brief Functional interface to call a planning function
   using PlannerFn =
       std::function<bool(const planning_scene::PlanningSceneConstPtr&, const planning_interface::MotionPlanRequest&,
-                         planning_interface::MotionPlanResponse&, const planning_interface::StateCostFn&)>;
+                         planning_interface::MotionPlanResponse&)>;
 
   virtual ~PlanningRequestAdapter() = default;
 
@@ -84,11 +84,10 @@ class PlanningRequestAdapter
    *  @param req Motion planning request with a set of constraints
    *  @param res Reference to which the generated motion plan response is written to
    *  @return True if response got generated correctly */
-  bool
-  adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-               const planning_scene::PlanningSceneConstPtr& planning_scene,
-               const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-               const planning_interface::StateCostFn& state_cost_function = planning_interface::StateCostFn()) const;
+  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
+                    const planning_scene::PlanningSceneConstPtr& planning_scene,
+                    const planning_interface::MotionPlanRequest& req,
+                    planning_interface::MotionPlanResponse& res) const;
 
   /** \brief Adapt the planning request if needed, call the planner
       function \e planner and update the planning response if
@@ -104,12 +103,10 @@ class PlanningRequestAdapter
      current state itself appears to touch obstacles). This is helpful because the added states should not be considered
      invalid in all situations.
    *  @return True if response got generated correctly */
-  bool
-  adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-               const planning_scene::PlanningSceneConstPtr& planning_scene,
-               const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-               std::vector<std::size_t>& added_path_index,
-               const planning_interface::StateCostFn& state_cost_function = planning_interface::StateCostFn()) const;
+  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
+                    const planning_scene::PlanningSceneConstPtr& planning_scene,
+                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
+                    std::vector<std::size_t>& added_path_index) const;
 
   /** \brief Adapt the planning request if needed, call the planner
       function \e planner and update the planning response if
@@ -125,11 +122,10 @@ class PlanningRequestAdapter
         current state itself appears to touch obstacles). This is helpful because the added states should not be
    considered invalid in all situations.
    *  @return True if response got generated correctly */
-  virtual bool
-  adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
-               const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-               std::vector<std::size_t>& added_path_index,
-               const planning_interface::StateCostFn& state_cost_function = planning_interface::StateCostFn()) const = 0;
+  virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
+                            const planning_interface::MotionPlanRequest& req,
+                            planning_interface::MotionPlanResponse& res,
+                            std::vector<std::size_t>& added_path_index) const = 0;
 
 protected:
   /** \brief Helper param for getting a parameter using a namespace **/
@@ -167,11 +163,10 @@ class PlanningRequestAdapterChain
    *  @param req Motion planning request with a set of constraints
    *  @param res Reference to which the generated motion plan response is written to
    *  @return True if response got generated correctly */
-  bool
-  adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-               const planning_scene::PlanningSceneConstPtr& planning_scene,
-               const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-               const planning_interface::StateCostFn& state_cost_function = planning_interface::StateCostFn()) const;
+  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
+                    const planning_scene::PlanningSceneConstPtr& planning_scene,
+                    const planning_interface::MotionPlanRequest& req,
+                    planning_interface::MotionPlanResponse& res) const;
 
   /** \brief Iterate through the chain and call all adapters and planners in the correct order
    *  @param planner Pointer to the planner used to solve the passed problem
@@ -183,12 +178,10 @@ class PlanningRequestAdapterChain
           current state itself appears to touch obstacles). This is helpful because the added states should not be
           considered invalid in all situations.
    *  @return True if response got generated correctly */
-  bool
-  adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
-               const planning_scene::PlanningSceneConstPtr& planning_scene,
-               const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-               std::vector<std::size_t>& added_path_index,
-               const planning_interface::StateCostFn& state_cost_function = planning_interface::StateCostFn()) const;
+  bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
+                    const planning_scene::PlanningSceneConstPtr& planning_scene,
+                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
+                    std::vector<std::size_t>& added_path_index) const;
 
 private:
   std::vector<PlanningRequestAdapterConstPtr> adapters_;

moveit_core/planning_request_adapter/src/planning_request_adapter.cpp

@@ -46,15 +46,14 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.planning_request
 
 namespace
 {
-bool callPlannerInterfaceSolve(
-    const planning_interface::PlannerManager& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
-    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-    const planning_interface::StateCostFn& state_cost_function = planning_interface::StateCostFn())
+bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner,
+                               const planning_scene::PlanningSceneConstPtr& planning_scene,
+                               const planning_interface::MotionPlanRequest& req,
+                               planning_interface::MotionPlanResponse& res)
 {
   planning_interface::PlanningContextPtr context = planner.getPlanningContext(planning_scene, req, res.error_code);
   if (context)
   {
-    context->setStateCostFunction(state_cost_function);
     return context->solve(res);
   }
   else
@@ -66,12 +65,11 @@ bool callPlannerInterfaceSolve(
 bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAdapter::PlannerFn& planner,
                  const planning_scene::PlanningSceneConstPtr& planning_scene,
                  const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                 std::vector<std::size_t>& added_path_index,
-                 const planning_interface::StateCostFn& state_cost_function = planning_interface::StateCostFn())
+                 std::vector<std::size_t>& added_path_index)
 {
   try
   {
-    bool result = adapter.adaptAndPlan(planner, planning_scene, req, res, added_path_index, state_cost_function);
+    bool result = adapter.adaptAndPlan(planner, planning_scene, req, res, added_path_index);
     RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::error_code_to_string(res.error_code));
     return result;
   }
@@ -80,7 +78,7 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda
     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, state_cost_function);
+    return planner(planning_scene, req, res);
   }
 }
 
@@ -90,26 +88,23 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag
                                           const planning_scene::PlanningSceneConstPtr& planning_scene,
                                           const planning_interface::MotionPlanRequest& req,
                                           planning_interface::MotionPlanResponse& res,
-                                          std::vector<std::size_t>& added_path_index,
-                                          const planning_interface::StateCostFn& state_cost_function) const
+                                          std::vector<std::size_t>& added_path_index) const
 {
   return adaptAndPlan(
       [&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
-                 planning_interface::MotionPlanResponse& res,
-                 const planning_interface::StateCostFn& state_cost_function) {
-        return callPlannerInterfaceSolve(*planner, scene, req, res, state_cost_function);
+                 planning_interface::MotionPlanResponse& res) {
+        return callPlannerInterfaceSolve(*planner, scene, req, res);
       },
-      planning_scene, req, res, added_path_index, state_cost_function);
+      planning_scene, req, res, added_path_index);
 }
 
 bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                           const planning_scene::PlanningSceneConstPtr& planning_scene,
                                           const planning_interface::MotionPlanRequest& req,
-                                          planning_interface::MotionPlanResponse& res,
-                                          const planning_interface::StateCostFn& state_cost_function) const
+                                          planning_interface::MotionPlanResponse& res) const
 {
   std::vector<std::size_t> empty_added_path_index;
-  return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index, state_cost_function);
+  return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index);
 }
 
 void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter)
@@ -120,51 +115,47 @@ void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPt
 bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                const planning_interface::MotionPlanRequest& req,
-                                               planning_interface::MotionPlanResponse& res,
-                                               const planning_interface::StateCostFn& state_cost_function) const
+                                               planning_interface::MotionPlanResponse& res) const
 {
   std::vector<std::size_t> empty_added_path_index;
-  return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index, state_cost_function);
+  return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index);
 }
 
 bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                const planning_interface::MotionPlanRequest& req,
                                                planning_interface::MotionPlanResponse& res,
-                                               std::vector<std::size_t>& added_path_index,
-                                               const planning_interface::StateCostFn& state_cost_function) const
+                                               std::vector<std::size_t>& added_path_index) const
 {
   // if there are no adapters, run the planner directly
   if (adapters_.empty())
   {
     added_path_index.clear();
-    return callPlannerInterfaceSolve(*planner, planning_scene, req, res, state_cost_function);
+    return callPlannerInterfaceSolve(*planner, planning_scene, req, res);
   }
   // the index values added by each adapter
   std::vector<std::vector<std::size_t>> added_path_index_each(adapters_.size());
 
   // if there are adapters, construct a function for each, in order,
   // so that in the end we have a nested sequence of functions that calls all adapters
   // and eventually the planner in the correct order.
-  PlanningRequestAdapter::PlannerFn fn = [&planner =
-                                              *planner](const planning_scene::PlanningSceneConstPtr& scene,
-                                                        const planning_interface::MotionPlanRequest& req,
-                                                        planning_interface::MotionPlanResponse& res,
-                                                        const planning_interface::StateCostFn& state_cost_function) {
-    return callPlannerInterfaceSolve(planner, scene, req, res, state_cost_function);
+  PlanningRequestAdapter::PlannerFn fn = [&planner = *planner](const planning_scene::PlanningSceneConstPtr& scene,
+                                                               const planning_interface::MotionPlanRequest& req,
+                                                               planning_interface::MotionPlanResponse& res) {
+    return callPlannerInterfaceSolve(planner, scene, req, res);
   };
 
   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, const planning_interface::StateCostFn& state_cost_function) {
-      return callAdapter(adapter, fn, scene, req, res, added_path_index, state_cost_function);
+             planning_interface::MotionPlanResponse& res) {
+      return callAdapter(adapter, fn, scene, req, res, added_path_index);
     };
   }
 
-  bool result = fn(planning_scene, req, res, state_cost_function);
+  bool result = fn(planning_scene, req, res);
   added_path_index.clear();
 
   // merge the index values from each adapter

moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp

@@ -172,17 +172,15 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
     return "CHOMP Optimizer";
   }
 
-  bool adaptAndPlan(
-      const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps,
-      const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-      std::vector<std::size_t>& /*added_path_index*/,
-      const planning_interface::StateCostFn& state_cost_function = planning_interface::StateCostFn()) const override
+  bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps,
+                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
+                    std::vector<std::size_t>& /*added_path_index*/) const override
   {
     RCLCPP_DEBUG(LOGGER, "CHOMP: adaptAndPlan ...");
 
     // following call to planner() calls the OMPL planner and stores the trajectory inside the MotionPlanResponse res
     // variable which is then used by CHOMP for optimization of the computed trajectory
-    if (!planner(ps, req, res, state_cost_function))
+    if (!planner(ps, req, res))
       return false;
 
     // create a hybrid collision detector to set the collision checker as hybrid

moveit_planners/ompl/ompl_interface/src/detail/planning_interface_objective.cpp

@@ -41,7 +41,7 @@ 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)
+  : OptimizationObjective(si)
 {
   description_ = "Planning Interface Objective";
 }

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -333,7 +333,7 @@ void ompl_interface::ModelBasedPlanningContext::useConfig()
     cfg.erase(it);
 
     // Warn if this config will be overwritten
-    if (state_cost_function_ != nullptr)
+    if (request_.state_cost_function != nullptr)
     {
       RCLCPP_WARN(LOGGER, "%s: User defined optimization function will overwrite OMPL config optimization objective %s",
                   name_.c_str(), optimizer.c_str());
@@ -373,11 +373,12 @@ void ompl_interface::ModelBasedPlanningContext::useConfig()
     }
   }
 
-  if (state_cost_function_ != nullptr)
+  if (request_.state_cost_function != nullptr)
   {
-    RCLCPP_WARN(LOGGER, "%s: Use user defined optimization function!", name_.c_str());
+    // TODO(sjahr): Remove debugging print output
+    RCLCPP_WARN(LOGGER, "%s: Using user defined state cost function!", name_.c_str());
     objective = std::make_shared<ompl_interface::PlanningInterfaceObjective>(ompl_simple_setup_->getSpaceInformation(),
-                                                                             state_cost_function_);
+                                                                             request_.state_cost_function);
     ompl_simple_setup_->setOptimizationObjective(objective);
   }
 

moveit_planners/stomp/src/stomp_moveit_planning_context.cpp

@@ -161,9 +161,11 @@ stomp::TaskPtr createStompTask(const stomp::StompConfiguration& config, StompPla
         planning_scene, group, constraints.getAllConstraints(), 1.0 /* constraint penalty */));
   }
 
-  if (context.getStateCostFunction() != nullptr)
+  if (context.getMotionPlanRequest().state_cost_function != nullptr)
   {
-    cost_functions.push_back(costs::get_cost_function_from_moveit_state_cost_fn(context.getStateCostFunction()));
+    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