-
Notifications
You must be signed in to change notification settings - Fork 948
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
Pass RobotModel to kinematics plugins #1166
Pass RobotModel to kinematics plugins #1166
Conversation
6bb28dc
to
16bec15
Compare
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.
#150 eventually got stuck due to circular reference of RobotModelPtr as detailed in #150 (comment).
To avoid/break this circular reference #1153 transferred ownership of KinematicSolvers (KS) from JointModelGroup (JMG) to KinematicsPluginLoader (KPL) / RobotModelLoader (RML). While this works, I think this is an evil hack. Ownership definitely belongs to JMG. This already becomes obvious from the fact that a RM with JMGs can (theoretically) exist without a KPL.
A JMG however always needs to be feed with setSolverAllocators
by someone else, so it is not really self contained in the first place. This dependency could be made more explicit and then also for your case of having JMG/RM without KPL it would be obvious that you need a replacement class (and storage) for KPL.
In this PR, ownership stays with JMGs. To break the circular reference, the RM is passed as reference to the KS. There, in contrast to #150, a new shared_ptr is created from the raw pointer, thus performing its own use counting. This way we decouple the use of the RM within the external KS from MoveIt. MoveIt just ensures that the RM is alive as long as the KS is. The KinematicsBase destructor could even check that the decoupled shared_ptr was correctly released by KS (and issue a warning if not).
speaking of evil hacks ... (see inline comment)
I'm not suggesting that I'm perfectly happy with the approach in #1153. I think the right solution would actually be to have RobotState store (and accept in it's constructor) a raw pointer. It could then be documented that a RobotModel needs to exist before and until after the lifetime of any RobotState (and any plugin using RobotModel). Violations to this requirement would crash pretty quickly (ie during testing and before deployment). As said in the inline comment, a shared pointer communicates (potential) transfer of ownership. There is however no intention to do transfer the ownership of RobotModel to some random plugin.
const std::string& base_frame, const std::vector<std::string>& tip_frames, | ||
double search_discretization) | ||
{ | ||
robot_model_ = moveit::core::RobotModelConstPtr(&robot_model, &no_deleter); |
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.
My objections boil down to this line, a shared_ptr signifies a (potential) transfer of ownership. This construct does not do that however. So it should be either of raw pointer, reference or weak_ptr. (reference would need to be passed in the constructor which I'm not sure is possible due to plugin stuff.) So it should be either raw pointer (which encodes your assumption of it living longer than the KinematicsBase anyway) or weak_ptr (which could check for end-of-life, but could also be stored as shared_ptr and create the loop again)
I know that this is not very useful with the current API of RobotState, but maybe that should be changed to one of the two above options (or maybe even 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.
I wouldn't pull in RobotState into the discussion. Passing a reference instead of a shared_ptr into the KS plugin, I indicate that ownership is not transferred. Rather, I guarantee that this reference is alive as long as the KS is. It's the decision of the KS plugin how to proceed with the reference. To comply with the existing RobotState API, I form a new shared_ptr, which can be freely used by all entities of the KS - as long as the KS is alive.
This is safe and clean.
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 agree with "safe enough" but not with clean. As we are currently defining the API that is going to used for the entire lifetime of melodic I think we need to also consider the API of RobotState and look at the whole picture.
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.
Can we continue this discussion on the phone? You can find my phone number from here.
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.
After the phone call I still have my reservations against that construct but since this approach is otherwise minimally invasive I'm fine with having it merged. Let's await reviews from others
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 believe, this no-deleter
pointer just feels weird because it's unusual. However it's a clean solution to decouple the usage counting in MoveIt (who actually owns the passed RobotModel) from the one in the external plugin lib (which should only receive and manage a borrowed reference). If the external plugin lib messes up it's usage counting (e.g. introducing circular references), this should not affect MoveIt.
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 is indeed odd and warrants a lot of inline comments as to why this design decision was made. in particular, i'd like to hear your response to @simonschmeisser 's question about raw pointers - why not use those instead?
* | ||
* When returning false, the KinematicsPlugingLoader will use the old method, passing a robot_description. | ||
* Default implementation returns false and issues a warning to implement this new API. | ||
* TODO: Make this method virtual after some soaking time, replacing the fallback. |
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.
pure virtual?
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.
Yes, that's what I meant ;-)
That's true.
However, this dependency can but doesn't need to be satisfied by KPL. In principle you could directly link against a KS plugin and pass
I disagree and I hope you don't consider that as viable option as well. Changing this at such a central spot, would dramatically increase fragility of the framework.
Agreed.
Indeed. Which is why I pass a reference, indicating that this is a borrowed pointer. |
moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
Outdated
Show resolved
Hide resolved
Now that the RobotModel is being passed in as a reference, the kinematics plugins could theoretically respond to changes in the model after the kinematics solver has been instantiated, right? Before, because the URDF was passed in as a string and then parsed, it was clear that the kinematics solver would not get any updates to the model. I think this is actually a good thing, since the solver may generate cached values for the model which would need to be updated if the model changes. One option would be to copy the robot model in the kinematics plugin instead of storing a shared pointer. This would make it clearer that changes to the model won't be taken into account by the kinematics solver, and it would also resolve the ownership problems. |
One idea of this PR was to ensure that the RobotModels used across the system are indeed identical and not out-of-sync. Creating a copy would raise this issue again. |
Sounds like there's two somewhat separate issues here:
Keeping a copy would satisfy point 1. To satisfy point 2, some amount of reloading may be necessary, but I think between reloads it would make sense for the IK solver's internally stored robot model to match what its internal datastructures were constructed with. |
While the second issue (updating the kinematics solvers or other parts of the framework if the RobotModel has changed) is an interesting and useful feature request, we should not tackle this in the scope of this PR. I think this feature will require more intense discussions on how to handle such changes. Please open a feature request for this. |
I agree - my point is that if we're just trying to achieve 1. without adding any smart pointer loops, then copying the robot state may be the simplest solution. |
Oops, meant to say "copying the robot model". |
2102d8d
to
abdd612
Compare
@v4hn, @davetcoleman: Would be great to get feedback on the proposed approach. I we agree, it remains to tackle the other plugins ( |
abdd612
to
febce50
Compare
Rebased to resolve conflicts. |
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 PR is too long!
MIGRATION.md
Outdated
@@ -8,6 +8,7 @@ API changes in MoveIt! releases | |||
- The move_group capability ``ExecuteTrajectoryServiceCapability`` has been removed in favor of the improved ``ExecuteTrajectoryActionCapability`` capability. Since Indigo, both capabilities were supported. If you still load default capabilities in your ``config/launch/move_group.launch``, you can just remove them from the capabilities parameter. The correct default capabilities will be loaded automatically. | |||
- Deprecated method ``CurrentStateMonitor::waitForCurrentState(double wait_time)`` was finally removed. | |||
- Renamed ``RobotState::getCollisionBodyTransforms`` to ``getCollisionBodyTransform`` as it returns a single transform only. | |||
- KinematicsBase: Deprecate members tip_frame_, search_discretization_. Use tip_frames_ and redundant_joint_discretization_ instead. |
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 deprecation change could easily be merged in via a separate PR, making this one easier to review
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.
Usually, when my PRs comprise several individual commits, they all have a distinct purpose and can/should be reviewed one-by-one. I don't think that it makes much sense to split these related commits into several PRs.
@@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_kinematics_base) | |||
|
|||
add_library(${MOVEIT_LIB_NAME} src/kinematics_base.cpp) | |||
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) | |||
target_compile_options(${MOVEIT_LIB_NAME} PRIVATE -Wno-deprecated-declarations) |
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.
what is the reasoning for this? can we keep this warning on? if not, add comment as to why not
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 is need to build kinematics_base.cpp without deprecation warnings as it - of course - still uses (initializes) deprecated member variables tip_frame_
and search_discretization_
.
Will add a comment.
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 don't think we want to disable deprecation warnings though... otherwise, what's the point of adding deprecation warnings?
better would be to just add comments saying something is deprecated if there's no way to actually switch to a new API
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.
Note, that I don't disable deprecation warnings per se, but only for building kinematics_base.cpp
.
Deprecation of the variables is foremost to warn derived classes, which still use those variables.
moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
Show resolved
Hide resolved
double search_discretization_; // DEPRECATED - this variable only still exists for backwards compatibility | ||
// with previous implementations. Discretization values for each joint are | ||
// now stored in the redundant_joint_discretization_ member | ||
// The next two variables still exists for backwards compatibility |
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.
@mlautman any thoughts on these variables w.r.t. IkFast?
@@ -647,6 +647,10 @@ class KinematicsBase | |||
return false; | |||
} | |||
|
|||
void storeValues(const moveit::core::RobotModel& robot_model, const std::string& group_name, |
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.
add comment - what does this function do? what is it storing where?
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 add a comment. It's a replacement for setValues()
.
void KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name, | ||
const std::string& base_frame, const std::string& tip_frame, | ||
double search_discretization) | ||
static void no_deleter(const moveit::core::RobotModel*) |
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.
why is this function using underscore format?
const std::string& base_frame, const std::vector<std::string>& tip_frames, | ||
double search_discretization) | ||
{ | ||
robot_model_ = moveit::core::RobotModelConstPtr(&robot_model, &no_deleter); |
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 is indeed odd and warrants a lot of inline comments as to why this design decision was made. in particular, i'd like to hear your response to @simonschmeisser 's question about raw pointers - why not use those instead?
const std::string& base_frame, const std::vector<std::string>& tip_frames, | ||
double search_discretization) | ||
{ | ||
ROS_WARN_NAMED("kinematics_base", "IK plugin for group '%s' relies on deprecated API. " |
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.
use LOGNAME
const std::string& base_frame, const std::vector<std::string>& tip_frames, | ||
double search_discretization) override | ||
{ | ||
return initImpl(robot_model, group_name, base_frame, tip_frames, search_discretization); |
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.
rename to: initializeImpl
@davetcoleman I addressed your formal comments. Do you have any content-wise objections? |
No need to store a separate pointer for constness.
@mlautman Could you take care of migrating the IKFast template to the new API please, i.e. passing the |
Do we really need to migrate all IK plugins at once? as @davetcoleman said this is already quite big |
I would like to get rid of corresponding deprecation warnings. As I said, the PR is split up into several individual commits to facilitate review and of course, this PR should not be squash-merged! |
b3b01ae
to
797975f
Compare
PRs like this are very hard to review, you can totally cherry-pick the simple style stuff into one or more separate PRs (e.g. "use LOGNAME", "cleanup KinematicsPluginLoad"). Those changes require very little cognitive load, whereas the magical pointer / reference stuff is tricky to understand when mixed in with a bunch of other files. |
It all seems reasonable enough |
6509801
to
6966e7d
Compare
|
||
protected: | ||
moveit::core::RobotModelConstPtr robot_model_; |
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.
Shouldn't we add a warning about the fake nature of this raw pointer in disguise here?
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.
The shared_ptr can be used as usual. There are no restrictions.
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.
Except for the fact that future maintainers (us) should remember it's not the usual...
I'm getting segmentation faults using trac_ik from debs and compiling melodic-devel branch of moveit from source with this PR merged. That's expected, right? |
No, that's not expected. I tested with IKFast only. Can you provide more details / a moveit_config to reproduce this? |
I found an TracIK setup and can reproduce the error. Will have a look. |
Thanks! I did just test with trac_ik from source, and no segfault. So that's good at least. |
I'm afraid that a rebuild is required as KinematicsBase has ABI-incompatible changes. |
|
||
protected: | ||
moveit::core::RobotModelConstPtr robot_model_; |
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.
@rhaschke does adding this robot_model_
member variable break ABI compatibility? Woudn't that mean all kinematics plugins will need to be recompiled?
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.
nvm - just saw your comment to this effect.
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.
Just as an echo. Yes, this was intended and it was clear from the beginning that passing in the robot model would break ABI compatibility with released plugins.
Recompiling source code is easy - but this will mean that all IK packages used from debs cannot be used with source version of moveit until moveit is re-released and everything is re-released. @davetcoleman @130s do we have a timeline on next release? |
I'm planning to re-release as soon as #1096 is merged - after the planned sync on this weekend... |
Thanks @rhaschke . I appreciate your work on this! |
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.
Partial post-mortem review. Sorry for not looking at it before.
I still consider the nodeleter very ugly, but at least you made it look okish in the implementation.
|
||
protected: | ||
moveit::core::RobotModelConstPtr robot_model_; |
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.
Except for the fact that future maintainers (us) should remember it's not the usual...
|
||
protected: | ||
moveit::core::RobotModelConstPtr robot_model_; |
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.
Just as an echo. Yes, this was intended and it was clear from the beginning that passing in the robot model would break ABI compatibility with released plugins.
void KinematicsBase::setValues(const std::string& robot_description, const std::string& group_name, | ||
const std::string& base_frame, const std::string& tip_frame, | ||
double search_discretization) | ||
static void noDeleter(const moveit::core::RobotModel*) |
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.
A comment would have been useful..
Do the plugin maintainers have to re-release themselves or is there a way to force rebuilds of non-broken packages on the buildfarm (I would have thought they rebuild everything either way before syncs, but I never looked into this domain before) Also, the problem will only really be fixed once the next sync is through and this will take a bit anyway. |
Good point about the sync - and yes, I meant to say that the problem won't be fixed until moveit is re-leased and everything is rebuilt (typo on my part). I do believe that a moveit release will trigger a buildfarm rebuild of all packages which depend on it. |
That's a perfectly valid solution. Not breaking API everywhere could be even seen as beautyful ;-)
No, all downstream packages are automatically rebuild and re-released (if they have modeled their dependencies correctly, of course). |
That's a perfectly valid solution.
Not breaking API everywhere could be even seen as beautyful ;-)
No. It is practical from a maintainance perspective.
Not beautiful from the perspective of good code.
|
Ok. I guess, we have different opinions on this... |
This is an alternative implemenation of #150 and #1153 to directly pass a RobotModel (RM) to kinematics plugins, thus
RobotModel
(particularly if there are dozens of groups)#150 eventually got stuck due to circular reference of
RobotModelPtr
as detailed in #150 (comment).To avoid/break this circular reference #1153 transferred ownership of KinematicSolvers (KS) from JointModelGroup (JMG) to KinematicsPluginLoader (KPL) / RobotModelLoader (RML). While this works, I think this is an evil hack. Ownership definitely belongs to JMG. This already becomes obvious from the fact that a RM with JMGs can (theoretically) exist without a KPL.
In this PR, ownership stays with JMGs. To break the circular reference, the RM is passed as reference to the KS. There, in contrast to #150, a new shared_ptr is created from the raw pointer, thus performing its own use counting. This way we decouple the use of the RM within the external KS from MoveIt. MoveIt just ensures that the RM is alive as long as the KS is. The KinematicsBase destructor could even check that the decoupled shared_ptr was correctly released by KS (and issue a warning if not).
To allow a smooth migration path, KS implementations should be able to choose which API for
initialize()
to implement. This is not (yet) considered in #1153. Here, KPL first tries the new API and - on failure - the old API. KinematicsBase provides default implementations for them, returning false, i.e. indicating failure.Adaptation of CachedIK was a little bit tricky: This one wraps an arbitrary other KS. To relay
initialize()
calls of the wrapper to the available implementation(s) of the wrapped KS, I use SFINAE magic. This works for KDL IK, which already has the old API removed.Please, keep commit history when merging!