Skip to content
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

Merged
merged 17 commits into from
Nov 22, 2018

Conversation

rhaschke
Copy link
Contributor

@rhaschke rhaschke commented Oct 28, 2018

This is an alternative implemenation of #150 and #1153 to directly pass a RobotModel (RM) to kinematics plugins, thus

  1. saving some loading time by reusing the RobotModel (particularly if there are dozens of groups)
  2. ensuring a consistent RM (particularly, joint limits) is used throughout the code

#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!

@rhaschke rhaschke force-pushed the pass-robot-model-to-ik branch 7 times, most recently from 6bb28dc to 16bec15 Compare October 29, 2018 08:27
Copy link
Contributor

@simonschmeisser simonschmeisser left a 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);
Copy link
Contributor

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)

Copy link
Contributor Author

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.

Copy link
Contributor

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.

Copy link
Contributor Author

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.

Copy link
Contributor

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

Copy link
Contributor Author

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.

Copy link
Member

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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

pure virtual?

Copy link
Contributor Author

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

@rhaschke
Copy link
Contributor Author

A JMG however always needs to be feed with setSolverAllocators by someone else, so it is not really self contained in the first place.

That's true.

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.

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 std::make_shared as an allocator.
Again, while the JMG needs a KS to be operational (and for this reason should own it), the KPL just caches the shared ptrs. This is a fundamental semantic difference.

I think the right solution would actually be to have RobotState store (and accept in it's constructor) a raw pointer.

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.

As said in the inline comment, a shared pointer communicates (potential) transfer of ownership.

Agreed.

There is however no intention to do transfer the ownership of RobotModel to some random plugin.

Indeed. Which is why I pass a reference, indicating that this is a borrowed pointer.

@jonbinney
Copy link
Contributor

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.

@rhaschke
Copy link
Contributor Author

rhaschke commented Nov 4, 2018

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.
If somebody should ever change the RobotModel on the fly, he should also ensure to reload the solvers, shouldn't he?

@jonbinney
Copy link
Contributor

Sounds like there's two somewhat separate issues here:

  1. making sure that the initial robot model used by the kinematics matches what the planners are using, including any configuration done by parameters outside of the URDF
  2. keeping the model used by the kinematics solver up-to-date if it changes elsewhere in the code

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.

@rhaschke
Copy link
Contributor Author

rhaschke commented Nov 6, 2018

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.

@jonbinney
Copy link
Contributor

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.

@jonbinney
Copy link
Contributor

Oops, meant to say "copying the robot model".

@rhaschke
Copy link
Contributor Author

@v4hn, @davetcoleman: Would be great to get feedback on the proposed approach. I we agree, it remains to tackle the other plugins (lma, srv, ikfast) in a similar fashion as I already did for kdl.

@rhaschke
Copy link
Contributor Author

Rebased to resolve conflicts.

Copy link
Member

@davetcoleman davetcoleman left a 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.
Copy link
Member

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

Copy link
Contributor Author

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)
Copy link
Member

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

Copy link
Contributor Author

@rhaschke rhaschke Nov 21, 2018

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.

Copy link
Member

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

Copy link
Contributor Author

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.

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
Copy link
Member

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,
Copy link
Member

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?

Copy link
Contributor Author

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*)
Copy link
Member

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);
Copy link
Member

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. "
Copy link
Member

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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

rename to: initializeImpl

@rhaschke
Copy link
Contributor Author

@davetcoleman I addressed your formal comments. Do you have any content-wise objections?
Otherwise I will proceed and adapt the remaining kinematics solvers.

@rhaschke
Copy link
Contributor Author

@mlautman Could you take care of migrating the IKFast template to the new API please, i.e. passing the RobotModel?

@simonschmeisser
Copy link
Contributor

Do we really need to migrate all IK plugins at once? as @davetcoleman said this is already quite big

@rhaschke
Copy link
Contributor Author

Do we really need to migrate all IK plugins at once?

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!

@rhaschke rhaschke changed the title WIP: Pass RobotModel to kinematics plugins Pass RobotModel to kinematics plugins Nov 21, 2018
@davetcoleman
Copy link
Member

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.

@davetcoleman
Copy link
Member

Do you have any content-wise objections? Otherwise I will proceed and adapt the remaining kinematics solvers.

It all seems reasonable enough

@rhaschke rhaschke merged commit 6966e7d into moveit:melodic-devel Nov 22, 2018
rhaschke added a commit that referenced this pull request Nov 22, 2018
@rhaschke rhaschke deleted the pass-robot-model-to-ik branch November 22, 2018 09:05

protected:
moveit::core::RobotModelConstPtr robot_model_;
Copy link
Contributor

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?

Copy link
Contributor Author

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.

Copy link
Contributor

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

@jonbinney
Copy link
Contributor

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?

@rhaschke
Copy link
Contributor Author

No, that's not expected. I tested with IKFast only. Can you provide more details / a moveit_config to reproduce this?

@rhaschke
Copy link
Contributor Author

I found an TracIK setup and can reproduce the error. Will have a look.

@jonbinney
Copy link
Contributor

Thanks! I did just test with trac_ik from source, and no segfault. So that's good at least.

@rhaschke
Copy link
Contributor Author

I'm afraid that a rebuild is required as KinematicsBase has ABI-incompatible changes.


protected:
moveit::core::RobotModelConstPtr robot_model_;
Copy link
Contributor

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?

Copy link
Contributor

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.

Copy link
Contributor

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.

@jonbinney
Copy link
Contributor

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?

@rhaschke
Copy link
Contributor Author

I'm planning to re-release as soon as #1096 is merged - after the planned sync on this weekend...

@jonbinney
Copy link
Contributor

Thanks @rhaschke . I appreciate your work on this!

@130s 130s mentioned this pull request Nov 25, 2018
12 tasks
Copy link
Contributor

@v4hn v4hn left a 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_;
Copy link
Contributor

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_;
Copy link
Contributor

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*)
Copy link
Contributor

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

@v4hn
Copy link
Contributor

v4hn commented Nov 26, 2018

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.

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.
Thus I don't see the need to release immediately.
Given we just merged major changes to the code base I would prefer to hold of the release at least until December 7th (end of next week) to check for potential problems.

@jonbinney
Copy link
Contributor

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.

@rhaschke
Copy link
Contributor Author

I still consider the nodeleter very ugly.

That's a perfectly valid solution. Not breaking API everywhere could be even seen as beautyful ;-)

Do the plugin maintainers have to re-release themselves?

No, all downstream packages are automatically rebuild and re-released (if they have modeled their dependencies correctly, of course).

@v4hn
Copy link
Contributor

v4hn commented Nov 27, 2018 via email

@rhaschke
Copy link
Contributor Author

Ok. I guess, we have different opinions on this...

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants