-
Notifications
You must be signed in to change notification settings - Fork 509
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Add generic cost function to KinematicsBase, CartesianInterpolator, Robot State #1386
Conversation
f09f86e
to
7190a61
Compare
Codecov Report
@@ 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
Continue to review full report at Codecov.
|
@@ -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&)>; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Will this be enough to cover our use cases? I thought we might want to have the seed state in here as well
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I also added a JointModelGroup*
parameter, since a lot of what we can do with RobotState
requires it
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
* @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, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
67c115b
to
176065d
Compare
Testing can be done with this new tutorial in moveit2_tutorials |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
One small comment; otherwise looks really good.
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()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Gotcha, I've changed them all to be passed by value
176065d
to
aa9b3ea
Compare
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, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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).
aa9b3ea
to
e3fc735
Compare
e3fc735
to
b4d32dd
Compare
7cd9c27
to
a522040
Compare
Description
This adds a generic cost function to be utilized by kinematics solvers,
kinematics::KinematicsBase::IKCostFn
. An instance of such may be passed toKinematicsBase::searchPositionIK
. Additionally, it may be passed toCartesianInterpolator::computeCartesianPath
andRobotState::setFromIK
, where it is simply passed through to the kinematics solver.Checklist