Add generic cost function to KinematicsBase, CartesianInterpolator, Robot State #1386

wyattrees · 2022-06-21T19:50:37Z

Description

This adds a generic cost function to be utilized by kinematics solvers, kinematics::KinematicsBase::IKCostFn. An instance of such may be passed to KinematicsBase::searchPositionIK. Additionally, it may be passed to CartesianInterpolator::computeCartesianPath and RobotState::setFromIK, where it is simply passed through to the kinematics solver.

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

codecov · 2022-06-21T20:52:41Z

Codecov Report

Merging #1386 (7cd9c27) into main (4597319) will increase coverage by 0.01%.
The diff coverage is 53.34%.

❗ Current head 7cd9c27 differs from pull request most recent head a522040. Consider uploading reports for the commit a522040 to get more accurate results

@@            Coverage Diff             @@
##             main    #1386      +/-   ##
==========================================
+ Coverage   50.85%   50.85%   +0.01%     
==========================================
  Files         381      381              
  Lines       31735    31740       +5     
==========================================
+ Hits        16135    16138       +3     
- Misses      15600    15602       +2

Impacted Files	Coverage Δ
...nclude/moveit/robot_state/cartesian_interpolator.h	`57.90% <ø> (ø)`
...bot_state/include/moveit/robot_state/robot_state.h	`89.96% <ø> (ø)`
...it_core/robot_state/src/cartesian_interpolator.cpp	`39.42% <0.00%> (ø)`
moveit_core/robot_state/src/robot_state.cpp	`47.37% <57.15%> (-0.07%)`	⬇️
...e/include/moveit/kinematics_base/kinematics_base.h	`76.32% <80.00%> (+0.56%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 47c52f2...a522040. Read the comment docs.

henningkayser · 2022-06-28T20:01:29Z

moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h

@@ -154,6 +154,9 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
  using IKCallbackFn = std::function<void(const geometry_msgs::msg::Pose&, const std::vector<double>&,
                                          moveit_msgs::msg::MoveItErrorCodes&)>;

+  /** @brief Signature for a cost function used to evaluate IK solutions. */
+  using IKCostFn = std::function<double(const geometry_msgs::msg::Pose&, const moveit::core::RobotState&)>;


Will this be enough to cover our use cases? I thought we might want to have the seed state in here as well

I think the seed state is a good idea to put in here as well. For our use case, there's actually a lot more that can go into evaluating a cost function. While including all of this here is probably superfluous, seed state will allow us to do things like a minimal displacement cost function

I also added a JointModelGroup* parameter, since a lot of what we can do with RobotState requires it

One thing to note is that because this is a std::function if you have the state externally you want to pass in there is always the option of using a lambda and capturing those values.

henningkayser · 2022-06-28T20:04:55Z

moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h

+   * @return True if a valid solution was found, false otherwise
+   */
+  virtual bool
+  searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,


This function signature is huge, just like the others. It should be simplified if possible, cleaning up return types and so on. I think most of the function arguments should be moved into KinematicsQueryOptions or something similar at some point, maybe using a builder pattern. I won't hold up the PR over this, though.

wyattrees · 2022-07-08T18:02:05Z

Testing can be done with this new tutorial in moveit2_tutorials

tylerjw

One small comment; otherwise looks really good.

tylerjw · 2022-07-08T19:14:20Z

moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h

+      const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
+      const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
+      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
+      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());


std::function objects should usually be passed by value because it is more optimal when the implementation saves this function object somewhere and nearly the same otherwise.

Here is an explanation: https://stackoverflow.com/questions/18365532/should-i-pass-an-stdfunction-by-const-reference

Gotcha, I've changed them all to be passed by value

tylerjw · 2022-07-11T18:57:10Z

moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h

+  virtual bool
+  searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
+                   double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
+                   const IKCallbackFn& solution_callback, const IKCostFn cost_function,


You don't want to mark the IKCostFn const here. Value parameters marked const in the declaration has no meaning as they can't modify the original version as they are passed by value.

tylerjw

One more tiny thing. When you pass by the value you shouldn't mark the value const in the declaration (the code that goes in the header file describing the interface). This is because making it const has no meaning in this case.

You can mark them const in the source files as that does have meaning. The compiler will ignore the const word in this case in the header as the declaration of a function or method only describes the contract with the user of that function (not if internally it modifies the value or not).

…nd RobotState

…nd RobotState (moveit#1386)

wyattrees force-pushed the pr-ik-cost-functions branch from f09f86e to 7190a61 Compare June 21, 2022 20:03

wyattrees mentioned this pull request Jun 22, 2022

Add cost function to pass to setFromIK in robot_model_and_robot_state tutorial moveit/moveit2_tutorials#451

Closed

2 tasks

wyattrees marked this pull request as ready for review June 24, 2022 14:46

wyattrees requested review from henningkayser and tylerjw June 24, 2022 14:47

henningkayser reviewed Jun 28, 2022

View reviewed changes

wyattrees force-pushed the pr-ik-cost-functions branch 2 times, most recently from 67c115b to 176065d Compare July 7, 2022 15:26

wyattrees mentioned this pull request Jul 8, 2022

Adding kinematics cost function tutorial moveit/moveit2_tutorials#462

Closed

2 tasks

tylerjw reviewed Jul 8, 2022

View reviewed changes

wyattrees force-pushed the pr-ik-cost-functions branch from 176065d to aa9b3ea Compare July 8, 2022 19:54

tylerjw reviewed Jul 11, 2022

View reviewed changes

wyattrees force-pushed the pr-ik-cost-functions branch from aa9b3ea to e3fc735 Compare July 11, 2022 21:11

tylerjw approved these changes Jul 11, 2022

View reviewed changes

tylerjw force-pushed the pr-ik-cost-functions branch from e3fc735 to b4d32dd Compare July 11, 2022 21:15

Add generic cost function to KinematicsBase, CartesianInterpolator, a…

a522040

…nd RobotState

tylerjw force-pushed the pr-ik-cost-functions branch from 7cd9c27 to a522040 Compare July 12, 2022 18:00

tylerjw merged commit 18ffec0 into moveit:main Jul 12, 2022

peterdavidfagan pushed a commit to peterdavidfagan/moveit2 that referenced this pull request Jul 14, 2022

Add generic cost function to KinematicsBase, CartesianInterpolator, a…

7e2acb8

…nd RobotState (moveit#1386)

wyattrees mentioned this pull request Aug 11, 2022

Singularity Avoidance in Cartesian Planning & Servo #1370

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add generic cost function to KinematicsBase, CartesianInterpolator, Robot State #1386

Add generic cost function to KinematicsBase, CartesianInterpolator, Robot State #1386

wyattrees commented Jun 21, 2022 •

edited

Loading

codecov bot commented Jun 21, 2022 •

edited

Loading

henningkayser Jun 28, 2022

wyattrees Jun 29, 2022

wyattrees Jun 30, 2022

tylerjw Jul 8, 2022

henningkayser Jun 28, 2022

wyattrees commented Jul 8, 2022

tylerjw left a comment

tylerjw Jul 8, 2022

wyattrees Jul 8, 2022

tylerjw Jul 11, 2022

tylerjw left a comment

Add generic cost function to KinematicsBase, CartesianInterpolator, Robot State #1386

Add generic cost function to KinematicsBase, CartesianInterpolator, Robot State #1386

Conversation

wyattrees commented Jun 21, 2022 • edited Loading

Description

Checklist

codecov bot commented Jun 21, 2022 • edited Loading

Codecov Report

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

wyattrees commented Jul 8, 2022

tylerjw left a 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

tylerjw left a comment

Choose a reason for hiding this comment

wyattrees commented Jun 21, 2022 •

edited

Loading

codecov bot commented Jun 21, 2022 •

edited

Loading