Add generic cost function to KinematicsBase, CartesianInterpolator, Robot State

[wyattrees]

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]

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]

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

[wyattrees]

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

[wyattrees]

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

[tylerjw]

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]

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]

Testing can be done with this new tutorial in moveit2_tutorials

[tylerjw]

One small comment; otherwise looks really good.

[tylerjw]

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

[wyattrees]

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

[tylerjw]

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