State cost function interface #2153

sjahr · 2023-05-02T14:15:39Z

Description

This PR enhances the API to use a user-defined state cost function in the planners.

Testing can be done with moveit/moveit2_tutorials#679

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

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

mamoll · 2023-05-03T15:17:32Z

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

+[[nodiscard]] planning_interface::StateCostFn getClearnaceCostFn();
+
+[[nodiscard]] planning_interface::StateCostFn
+getWeightedCostFnSum(std::vector<std::pair<double, planning_interface::StateCostFn>> weight_cost_vector);


As convenience functions you can define operator*(double, planning_interface::StateCostFn) and operator+(planning_interface::StateCostFn,planning_interface::StateCostFn) so that you can write weighted sums simply as:

planning_interface::StateCostFn fn1 = ...; planning_interface::StateCostFn fn2 = ...; planning_interface::StateCostFn fn3 = ...; planning_interface::StateCostFn fn4 = 2. * fn1 + 3. * fn2 + 4. * fn3;

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

mamoll · 2023-05-03T15:23:15Z

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

@@ -144,6 +150,9 @@ class PlanningContext

  /// The planning request for this context
  MotionPlanRequest request_;
+
+  // Cost function for individual states
+  StateCostFn state_cost_function_ = StateCostFn();


Maybe a more reasonable default value is a function that always returns 1.

Good point! I'd like to have a way to check for an "unset" cost function, so I think this should be the default, but specific implementations should default to cost = 1

mergify · 2023-05-12T07:45:37Z

This pull request is in conflict. Could you fix it @sjahr?

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

codecov · 2023-05-18T10:24:00Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 50.74%. Comparing base (87d5945) to head (7cc9f0b).
Report is 57 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2153      +/-   ##
==========================================
+ Coverage   50.73%   50.74%   +0.01%     
==========================================
  Files         386      388       +2     
  Lines       32087    32092       +5     
==========================================
+ Hits        16276    16281       +5     
  Misses      15811    15811

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

mergify · 2023-06-01T08:55:35Z

This pull request is in conflict. Could you fix it @sjahr?

moveit_core/cost_functions/src/cost_functions.cpp

mergify · 2023-08-15T19:40:41Z

This pull request is in conflict. Could you fix it @sjahr?

henningkayser

Finally trying to come to a conclusion on this, testing right now...

henningkayser · 2023-09-28T14:00:03Z

moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp

+CostFn get_cost_function_from_moveit_state_cost_fn(const ::planning_interface::StateCostFn& state_cost_function)
+{
+  CostFn cost_fn = [&](const Eigen::MatrixXd& values, Eigen::VectorXd& costs, bool& validity) {
+    validity = true;  // TODO(sjahr): Discuss if that is always the case


just found this... No, it's not always the case at all. STOMP will only plan until validity, and only continue optimizing if explicitly configured that way. For general usage, we would need a validity threshold for any cost we want to optimize.

🤔 How would this validity threshold look like? Is that fixed with #2418?

The threshold needs to be provided by the cost function implementation. Unless we normalize all costs (which is not easy to do) there is no way to define a general threshold. The linked PR only changed the cost metric to use actual distance values, it doesn't really deal with validity.

henningkayser · 2023-09-28T14:12:02Z

moveit_core/cost_functions/src/cost_functions.cpp

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


Isn't there a max distance associated with the distance check? We don't want to return infinity if the returned distance is larger than that.

Good question, I have not found anything about this in the documentation/ comments 😅 On the other hand, it only returns infinity, if the distance check returns a negative distance (penetration of the collision object). For simplicity sake I decided that this is always associated with the highest possible cost

I still think that infinity will basically break optimization since there is no way to approximate a gradient based in this value. IMO, a better cost function would take a specific clearance target as 0 (or max_distance if set), and then returning a linear cost for distances smaller than that, including negative distances. It's very useful to also consider negative distances since they also produce a gradient and STOMP can optimize out collisions. The cost function could looks like this max(0, target_clearance - distance)

moveit_core/cost_functions/src/cost_functions.cpp

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

marioprats

Looks good!
I have a few minor comments and optional changes, but I'm probably missing context, so feel free to ignore!
I also have a question about how to add support for cost functions on the ROS message, if that's planned.
Other than that, with some additional documentation and testing I think this should be good to merge.

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

marioprats · 2023-10-25T08:47:01Z

moveit_core/cost_functions/src/cost_functions.cpp

+                   const planning_scene::PlanningSceneConstPtr& planning_scene)
+{
+  // Create cost function
+  return [robot_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable {


An alternative for additional transparency and simplicity would be to define the actual cost functions in the header, and then std::bind them directly from where they are needed, e.g.:

// This is a transparent cost function declaration. Takes what it needs, returns what it computes. // Can be used in other contexts without the planning_interface dependency. double computeClearanceCost(moveit::core::RobotState& robot_state, const std::string& group_name, const planning_scene::PlanningSceneConstPtr& planning_scene, const Eigen::VectorXd& state_vector) { ... // what you have in the return lambda. } // And where you need to create a StateCostFn, you can use std::bind to make the wrapper, e.g.: StateCostFn clearance_cost_fn = std::bind(computeClearanceCost, robot_state, group_name, planning_scene, std::placeholders::_1);

In C++20 there's also std::bind_front to get rid of the std::placeholders::_1.
But maybe not super important at this point, so as you prefer.

Good thought, thanks! I found multiple sources that discourage using std::bind (like abseil and turner) because of the compile and runtime overhead it causes. Maybe C++20's std::bind_front fixes that but since we're not using it at the moment I'd prefer the lambdas for now. I guess if I'd like to avoid the dependency on the planning_interface I could also just abstain from using the typedef and use std::function<double(const Eigen::VectorXd& state_vector)> instead of StateCostFn. So the dependency is not mandatory.

What I mean with avoiding the dependency is that if you just have the function that computes the cost here, you can turn that into a StateCostFn or any other function signature at the caller side (e.g. stomp or other libraries may have their own cost function signature, but they could still use these cost computation functions and transform them into what they need via bind or lambdas. Of course this doesn't matter if we think that double(Eigen::VectorXd) is a general enough cost-definition function, which it probably is. I'm okay with leaving this as is for now and iterate later if needed, but see my comment below for the lambda captures.

On a separate note, I wish we had the absl dependency to be able to use absl::bind_front, StatusOr and others, and to follow their TotW and style guide.

moveit_core/cost_functions/src/cost_functions.cpp

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

moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp

moveit_planners/stomp/src/stomp_moveit_planning_context.cpp

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp

marioprats

I think this looks good to start experimenting and iterating on what cost functions work and make sense.
Longer term, if we want to add cost functions to the PlanningRequest msg as enums, I would maybe update the PlanningRequest C++ type to reflect that (i.e. an enum instead of a StateCostFn, or go back to use the original msg), and handle the enum inside stomp. That would also avoid the double type-erasure with the lambda functions which I find hard to read (i.e. cost-function -> StateCostFn -> CostFn), since stomp could wrap directly MoveIt cost functions into CostFn. But I would leave all this for the future if needed.

marioprats · 2023-10-27T07:31:04Z

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

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


We will need comments on what these do, and tests for them.

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

mergify · 2023-10-27T14:40:03Z

This pull request is in conflict. Could you fix it @sjahr?

github-actions · 2023-12-13T12:08:55Z

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

henningkayser · 2023-12-15T17:57:13Z

@sjahr I think this looks good for merging. Can you please add the comments that @marioprats requested, and track my suggestions about the cost functions in a follow-up issue?

mergify · 2024-01-16T16:09:13Z

This pull request is in conflict. Could you fix it @sjahr?

github-actions · 2024-03-15T12:08:09Z

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

sjahr changed the title ~~WIP: state cost function interface~~ WIP: State cost function interface May 2, 2023

mamoll reviewed May 3, 2023

View reviewed changes

sjahr force-pushed the pr-cost_function_interface branch from c8387e2 to f982f86 Compare May 8, 2023 11:58

sjahr mentioned this pull request May 11, 2023

WIP: Cost function example moveit/moveit2_tutorials#679

Draft

2 tasks

sjahr force-pushed the pr-cost_function_interface branch from 1f22372 to 9cdde7d Compare May 15, 2023 09:14

henningkayser reviewed May 16, 2023

View reviewed changes

moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h Outdated Show resolved Hide resolved

sjahr force-pushed the pr-cost_function_interface branch from e90e60f to 20c9e39 Compare May 17, 2023 09:03

sjahr force-pushed the pr-cost_function_interface branch from cd5ed0b to e9e811f Compare June 14, 2023 09:38

sjahr changed the title ~~WIP: State cost function interface~~ State cost function interface Jun 14, 2023

sjahr mentioned this pull request Jun 16, 2023

Cost function API for MoveIt 2 #2240

Open

10 tasks

sjahr force-pushed the pr-cost_function_interface branch from a22d090 to 49d5e08 Compare July 31, 2023 07:32

sjahr commented Jul 31, 2023

View reviewed changes

moveit_core/cost_functions/src/cost_functions.cpp Outdated Show resolved Hide resolved

sjahr self-assigned this Aug 8, 2023

sjahr force-pushed the pr-cost_function_interface branch from 0a1cdae to 78bdf82 Compare September 14, 2023 11:01

sjahr marked this pull request as ready for review September 14, 2023 14:57

henningkayser reviewed Oct 10, 2023

View reviewed changes

sjahr requested a review from marioprats October 24, 2023 14:54

sjahr commented Oct 24, 2023

View reviewed changes

moveit_core/cost_functions/include/moveit/cost_functions/cost_functions.hpp Outdated Show resolved Hide resolved

marioprats reviewed Oct 25, 2023

View reviewed changes

sjahr requested review from marioprats, henningkayser and mamoll October 26, 2023 13:34

marioprats reviewed Oct 27, 2023

View reviewed changes

github-actions bot added the stale Inactive issues and PRs are marked as stale and may be closed automatically. label Dec 13, 2023

Add cost function interface

7870bc9

sjahr force-pushed the pr-cost_function_interface branch from d95e6e2 to 7870bc9 Compare December 13, 2023 17:52

Merge branch 'main' into pr-cost_function_interface

1d8f8b3

github-actions bot removed the stale Inactive issues and PRs are marked as stale and may be closed automatically. label Dec 14, 2023

Merge branch 'main' into pr-cost_function_interface

7cc9f0b

sjahr mentioned this pull request Dec 15, 2023

Update to pipeline planner to moveit2 API changes moveit/moveit_task_constructor#517

Closed

github-actions bot added the stale Inactive issues and PRs are marked as stale and may be closed automatically. label Mar 15, 2024

sjahr removed the stale Inactive issues and PRs are marked as stale and may be closed automatically. label Mar 15, 2024

sjahr mentioned this pull request May 19, 2024

Implement fuzzy-matching Trajectory Cache 🔥 #2838

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

State cost function interface #2153

State cost function interface #2153

sjahr commented May 2, 2023 •

edited

Loading

mamoll May 3, 2023

mamoll May 3, 2023

sjahr May 18, 2023

mergify bot commented May 12, 2023

codecov bot commented May 18, 2023 •

edited

Loading

mergify bot commented Jun 1, 2023

mergify bot commented Aug 15, 2023

henningkayser left a comment

henningkayser Sep 28, 2023

sjahr Oct 16, 2023 •

edited

Loading

henningkayser Dec 15, 2023

henningkayser Sep 28, 2023

sjahr Oct 16, 2023

henningkayser Dec 15, 2023

marioprats left a comment

marioprats Oct 25, 2023

sjahr Oct 26, 2023 •

edited

Loading

marioprats Oct 27, 2023

marioprats left a comment

marioprats Oct 27, 2023

mergify bot commented Oct 27, 2023

github-actions bot commented Dec 13, 2023

henningkayser commented Dec 15, 2023

mergify bot commented Jan 16, 2024

github-actions bot commented Mar 15, 2024

State cost function interface #2153

Are you sure you want to change the base?

State cost function interface #2153

Conversation

sjahr commented May 2, 2023 • edited Loading

Description

Checklist

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mergify bot commented May 12, 2023

codecov bot commented May 18, 2023 • edited Loading

Codecov Report

mergify bot commented Jun 1, 2023

mergify bot commented Aug 15, 2023

henningkayser left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sjahr Oct 16, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

marioprats left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sjahr Oct 26, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

marioprats left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mergify bot commented Oct 27, 2023

github-actions bot commented Dec 13, 2023

henningkayser commented Dec 15, 2023

mergify bot commented Jan 16, 2024

github-actions bot commented Mar 15, 2024

sjahr commented May 2, 2023 •

edited

Loading

codecov bot commented May 18, 2023 •

edited

Loading

sjahr Oct 16, 2023 •

edited

Loading

sjahr Oct 26, 2023 •

edited

Loading