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

Removed prefix code which prevented the plugin from being used for mu… #248

Conversation

UltronDestroyer
Copy link

…ltiple arms on one robot. The prefix code does nothing anyway, all it does is check/confirm that the chain contains links for a UR robot, which one can assume outside of this code that it is being used for a UR robot

…ltiple arms on one robot. The prefix code does nothing anyway, all it does is check/confirm that the chain contains links for a UR robot, which one can assume outside of this code that it is being used for a UR robot
@UltronDestroyer
Copy link
Author

An alternative suggestion is to instead remove the "return false" from the loops that check the joints which cause the plugin to falsely fail. The plugin still works successfully by reading the joints from the robot model, and using them in the kdl chain. This redundant checking causes unnecessary failure when using 2+ arms, or by failing to set a prefix (which isn't always 100% necessary)

@UltronDestroyer
Copy link
Author

UltronDestroyer commented Jun 21, 2016

@UltronDestroyer
Copy link
Author

Edit: I've updated it because the end effector orbs weren't working with the first commit of this pull request. now, it uses the links that are provided by the robot models' kinematic chains rather than pre-defined prefix + hard coded names. This means that you can now set a robot up to use multiple UR-X arms with moveit on a robot.

Before, the plugins would be loaded and limited by a defined arm_prefix parameter, which can only be set to one prefix obviously.

I had it so that i had a left_ur5_arm_ as one prefix for one arm, and right_ur5_arm_ as the prefix for the other, but since the prefix could only be set once before loading a single move_group, it would mean that one planning group could not solve for the other arm, e.g only the left arm in moveit + rviz would be setup correctly, whereas the other would not be found due to the prefix looking for the arm_prefix of the other arm.

@miguelprada
Copy link
Member

Out of curiosity, by checking the moveit configuration packages for the three UR models, all of them use the KDL kinematics plugin by default, which should have no issue with the prefix stuff at all. Why are you using the custom plugin?

@UltronDestroyer
Copy link
Author

You're right, it doesn't. But the custom plugin is way more responsive.

@UltronDestroyer
Copy link
Author

FYI, managed to get this working https://twitter.com/clearpathrobots/status/746058485327085568

Check it out

@shaun-edwards
Copy link
Member

@TheDash, thanks for the PR. I'm a little over my head on this, but it looks good. Does this have to be an indigo change or could it be a later release. I'm trying to keep indigo stable.

ur_joint_names_ = joint_model_group->getJointModelNames();
ur_link_names_ = joint_model_group->getLinkModelNames();

/*
Copy link
Member

Choose a reason for hiding this comment

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

Can the commented out code be removed? FYI...I prefer // instead of /*, since it makes the diff more representative of what was changed/commented out (even though I don't like commented out code ;).

Copy link
Author

Choose a reason for hiding this comment

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

@shaun-edwards

I removed the code that is commented out :)

@UltronDestroyer
Copy link
Author

It's stable on my end and we're going to ship a robot with this code in it. Basically this code pulls the joint names defined by the planning group, which is different if you load a plugin for each arm.

Before, it would use prefix + hard coded UR names, and if you had more than one arm prefix you couldn't use the plugins. When using a left and right arm, you need two prefixes obviously, (left_, right_) and so one of the arms was left to be unable to plan.

@@ -362,10 +349,13 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description,
}
last_ur_joint_ind = cur_ur_joint_ind;
}
// if successful, the kinematic chain includes a serial chain of the UR joints
// if successful, the kinematic chain includes a serial chain of the UR joints */
Copy link
Member

Choose a reason for hiding this comment

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

Dangling */?

Copy link
Author

Choose a reason for hiding this comment

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

It should be unaffected since // is there, but I've removed it.

@gavanderhoorn
Copy link
Member

gavanderhoorn commented Jun 24, 2016

While I understand the reason for the change you propose, I'm not sure removing the check is something we necessarily want to do.

An alternative suggestion is to instead remove the "return false" from the loops that check the joints which cause the plugin to falsely fail. The plugin still works successfully by reading the joints from the robot model, and using them in the kdl chain. This redundant checking causes unnecessary failure when using 2+ arms, or by failing to set a prefix (which isn't always 100% necessary)

The checking may seem redundant, but in reality it tries to make sure that users don't configure it for use with chains that don't contain the expected joints and links. The rest of the code assumes that whatever it receives is a (joint space) pose that it can return sensible answers for. By removing the check and relying on the correctness of the configuration of the chains, we open up the possibility for end-users to configure the use of the plugin for any set of joints and links through the MoveIt Setup Assistant.

(IKFast plugins suffer from the same problem: they are generated for a particular chain, but nothing prevents you from configuring them for an entirely different chain (and not only joints and links, but also their orientation, which can lead to really dangerous situations). The plugin will just assume everything is ok and will return non-sense solutions - which is not immediately obvious.)

Of course this is not completely fool/tamper-proof: any urdf with the same joint & link names will be accepted as ok, even though that is no guarantee that lengths and other properties correspond to what the plugin assumes. There is currently no way for a plugin to communicate for which particular set of joints and links it's capable of solving IK/FK, so it requires knowledge of either the plugin's internals or whatever is the set of joints and links that make up the 'normal' chain the plugin was created for.

I guess ultimately, whether we want to remove this check or not comes down to how defensive we want to be: do we place the responsibility for proper configuration entirely on the user? Or do we prefer to fail early and warn about misconfiguration?

@gavanderhoorn
Copy link
Member

I think either @jrgnicho or @JeremyZoss ran into the IKFast-solving-for-whatever-you-give-it issue some time ago. Not sure what the context was anymore.

@gavanderhoorn
Copy link
Member

@TheDash wrote:

Before, the plugins would be loaded and limited by a defined arm_prefix parameter, which can only be set to one prefix obviously.

The real problem is this: iirc, MoveIt either doesn't actually load multiple instances of the same plugin, or it replaces all references to all instances with the last one loaded. For multiple instances of the ur_kinematics plugin to work, each should get their own prefix, which apparently (still) isn't working.

There are quite some posts on moveit-users and ROS Answers about this:

Interesting quote from the moveit-users post:

Just as a notice, when I use a IKFast plugin for one group and the KDLKinematicsPlugin for the other, moveit uses the correct solvers for each group.

I seem to remember seeing more posts, and that at least one actually found a solution/work-around. I'll see if I can find it again.

So in my opinion - but it really is just my opinion, I'm not even a maintainer here - this PR introduces a potential issue (using it with a chain that does not correspond to the implicitly assumed correct UR configuration) and does not solve the real problem.

@gavanderhoorn
Copy link
Member

@gavanderhoorn wrote:

I think either @jrgnicho or @JeremyZoss ran into the IKFast-solving-for-whatever-you-give-it issue some time ago. Not sure what the context was anymore.

Apparently that was @Jmeyer1292, he even opened an issue to discuss this over at moveit/moveit_ikfast#60.

@UltronDestroyer
Copy link
Author

@gavanderhoorn

I can check that the group of joints contains all of the arm strings, which should fix all issues here. I just dont want it to check for prefixes, which is unnecessary.

@gavanderhoorn
Copy link
Member

I can check that the group of joints contains all of the arm strings, which should fix all issues here. I just dont want it to check for prefixes, which is unnecessary.

Not sure if this ever occurs, but what if a user defines different solver configurations for specific groups/chains? The prefix would be the only way to know for sure that those configurations end up with the correct solver, right?

@UltronDestroyer
Copy link
Author

Not sure if this ever occurs, but what if a user defines different solver configurations for specific groups/chains? The prefix would be the only way to know for sure that those configurations end up with the correct solver, right?

Right now it pulls all of the joints defined in the group. We can add a check which checks if the subset of these strings contain the valid joints (which would mean every part of the string, nulling the prefix) And then it could accept any prefix, but still hard-define this plugin to make sure it has only the UR joints.

@UltronDestroyer
Copy link
Author

FYI - the referenced issue above is a use case of one of our customers having difficulty with the plugin - not because it is not a UR arm but because the prefix configuration isn't correctly set up. This caused him to use a TRAC IK plugin instead of the work done inside the UR kinematics package - not sure if that is intended.

With this PR, to re-iterate, it would help users just put the ur_kinematics plugin in their .yaml file and then be on their way. As long as they have the 6 UR links/joints defined in their .yaml file and are using the plugin, it'll work with no issues.

@UltronDestroyer
Copy link
Author

@shaun-edwards @gavanderhoorn any thoughts on getting this merged?

--- FYI we are most likely going to start a new process at CPR for using external repositories;

we will begin by forking a repository (like this one) we wish to modify for our products, make changes, and then release it on our internal ROS distro/system. This way, we can distribute the necessary debian packages and move forward with our lives quickly.

We will then make a PR of the changes we've made - but with minor rush/risk so we can be precise and accurate that the proper changes can be merged into public repositories.

In any case the merge is refused - we will go ahead with our forked branch/release and merge in the features developed here as necessary.

I believe this should help us both in making sure quality code is added - and that we aren't held up by waiting for commits/releases as it is a timely process

@gavanderhoorn
Copy link
Member

I'm really not happy about removing the prefix just because it's harder to get the configuration right. But:

  1. I'm not a maintainer here, so ultimately it's up to @ipa-fxm and maybe @shaun-edwards

  2. if you do this:

    I can check that the group of joints contains all of the arm strings, which should fix all issues here.

    it's probably enough to guard against user configuration error

Reason I don't like removing the prefix is because I feel there are valid use cases for it, and I've never really had too much difficulty getting things right. However, I've learnt to not take my own experience with things as leading in these things, and apparently there is a real issue - or at least enough for you to make the effort to change things.

So if you could add some defensive checking (all joints accounted for, proper order, maybe even link length (but that may be too much)) it would seem to cover all my (our?) concerns and improve the situation as encountered by your customer. So please go ahead.

@gavanderhoorn
Copy link
Member

@TheDash wrote:

[..] all it does is check/confirm that the chain contains links for a UR robot, which one can assume outside of this code that it is being used for a UR robot

It's exactly this: assume. Isn't there an American colloquialism about assumptions?

@UltronDestroyer
Copy link
Author

@gavanderhoorn

Have you tried using multiple functional UR arms on a platform? This is where the issue stems from - since multiple loading of the move_group in the RViz node or namespacing move_group causes major issues within moveit

Ok - general concensus seems to be to add some kind of checking/warning to the code while keeping it generic

@gavanderhoorn
Copy link
Member

Ok - general concensus seems to be to add some kind of checking/warning to the code while keeping it generic

that would be ideal, yes. But again, I'm just one voice here.

@gavanderhoorn
Copy link
Member

Have you tried using multiple functional UR arms on a platform? This is where the issue stems from - since multiple loading of the move_group in the RViz node or namespacing move_group causes major issues within moveit

Yep.

But the real issue seems to be with MoveIt. Probably unrealistic of me, but perhaps those issues should be fixed, instead of working around it in every (kinematics) plugin.

@UltronDestroyer
Copy link
Author

But the real issue seems to be with MoveIt. Probably unrealistic of me, but perhaps those issues should be fixed, instead of working around it in every (kinematics) plugin.

Yeah.. I found the weakest link I could tackle to make things happen. Re-arranging all the bugs in Moveit is a tiresome affair and, I played around with it a bit. 2-3 days of re-organizng namespaces ran into no solutions that worked since some things are hard coded in MoveIt and would require many c++ files to be touched there.

However - lets say I modified MoveIt so that I can run the plugin in 2 different namespaces - I would then have two different move_groups running, rather than one move_group with 2 plugins. With this setup, we only have one move_group, with two plugins and the only difference between the plugins is the name of the joints that are passed in.

Engineering wise - this solution makes total sense. Even configuration wise, this makes sense. otherwise in the moveit_config for each robot with 2+X arms you have to scale the package size relative to the number of UR-X arms that are on the robot, or do a major refactoring of how moveit is setup.

@gavanderhoorn
Copy link
Member

gavanderhoorn commented Jul 12, 2016

First: I agree that this is probably the easiest work-around, taking the effort required into account (but that doesn't mean I need to like it :) ).

But:

However - lets say I modified MoveIt so that I can run the plugin in 2 different namespaces - I would then have two different move_groups running, rather than one move_group with 2 plugins. With this setup, we only have one move_group, with two plugins and the only difference between the plugins is the name of the joints that are passed in.

last time I attempted something like this, the only thing not working was getting MoveIt to use two instances of the same plugin with different parameters. IK plugins are configured per chain, and should read their parameters from a namespace based on the chain name. Something in that piece of the infrastructure isn't working correctly, which is what the issues I linked to earlier in this thread also reported. It makes it difficult to get this to work. But it isn't impossible.

But let's stop arguing about this.

The only thing I'd like to see is a mechanism to prevent user configuration errors from having actual, real world consequences because the IK plugin is happily returning solutions for chains it doesn't know anything about, but is configured to solve for.

@gavanderhoorn
Copy link
Member

O and since the plugin has been released with support for the prefix, and is checking for it in LTS releases, a warning about it being ignored in the new version would be a good thing, I think.

@Jmeyer1292
Copy link

The only thing I'd like to see is a mechanism to prevent user configuration errors from having actualy, real world consequences because the IK plugin is happily returning solutions for chains it doesn't know anything about, but is configured to solve for.

Which is exactly what every other IKFast plugin but this particular one does. The only problem I see with this PR is that it will bring this plugins behaviour inline with all of the rest and thus change the IK solutions given to existing programs. But maybe I misunderstand.

If I do understand, then how about looping over the looping over the link names from the move-group and finding the index of the first element in the chain of links that ends with the hard-coded names.

This ensures that:

  1. We do some kind of check for the appropriate joints
  2. We don't change the behaviour (as ur_joint_inds_start_ will still be set)
  3. and we don't have some stupid magical prefix parameter but rather infer it from the model

@gavanderhoorn
Copy link
Member

gavanderhoorn commented Jul 12, 2016

@Jmeyer1292 wrote:

Which is exactly what every other IKFast plugin but this particular one does. The only problem I see with this PR is that it will bring this plugins behaviour inline with all of the rest and thus change the IK solutions given to existing programs. But maybe I misunderstand.

Are you saying that you feel it's better to remove the check to make this plugin act as other IKFast plugins (which ur_kinematics isn't btw)? I'm all for consistency, but in this case I think it's not something that should be the goal.

And how will changing / removing the check as proposed by @TheDash change the solutions?

If I do understand, then how about looping over the looping over the link names from the move-group and finding the index of the first element in the chain of links that ends with the hard-coded names.

This ensures that:

  1. We do some kind of check for the appropriate joints
  2. We don't change the behaviour (as ur_joint_inds_start_ will still be set)

Isn't that basically what was implied in the above discussion?

I wouldn't rely on indices and the assumption that joints form a consecutive list, but other than that: yes, that was the idea.

3 . and we don't have some stupid magical prefix parameter but rather infer it from the model

The prefix was added in analogy to tf_prefix, the xacro prefix and the namespacing that is normally used in launch files / parameters. I don't think it's that 'stupid magical' at all.

@UltronDestroyer
Copy link
Author

If I do understand, then how about looping over the looping over the link names from the move-group and finding the index of the first element in the chain of links that ends with the hard-coded names.

This ensures that:

We do some kind of check for the appropriate joints
We don't change the behaviour (as ur_joint_inds_start_ will still be set)
and we don't have some stupid magical prefix parameter but rather infer it from the mo

yes - this logic is what i propose - we are on the same page. @Jmeyer1292

O and since the plugin has been released with support for the prefix, and is checking for it in LTS releases, a warning about it being ignored in the new version would be a good thing, I think.

Using the solution I proposed, and reiterated above, it is a change the users won't see. They still pass in the prefix, but in the back-end it won't end up giving an error unless the joints aren't UR joints.

last time I attempted something like this, the only thing not working was getting MoveIt to use two instances of the same plugin with different parameters

Yeah, I seen the same thing. As it is now, we only load 1 plugin instance but when you change the planning group it changes the joints passed into the plugin instead of swapping to a new plugin since there is an instance that exists already

The only thing I'd like to see is a mechanism to prevent user configuration errors from having actualy, real world consequences because the IK plugin is happily returning solutions for chains it doesn't know anything about, but is configured to solve for.

No worries - I will take that into account with the above soln.

I appreciate all input so far.

ur_link_names_.push_back(arm_prefix_ + "ee_link"); // 8
// Load the joint names from the group model
ur_joint_names_ = joint_model_group->getJointModelNames();
ur_link_names_ = joint_model_group->getLinkModelNames();

ur_joint_inds_start_ = getJointIndex(ur_joint_names_[0]);
Copy link

@Jmeyer1292 Jmeyer1292 Jul 12, 2016

Choose a reason for hiding this comment

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

Nm

@Jmeyer1292
Copy link

The prefix was added in analogy to tf_prefix, the xacro prefix and the namespacing that is normally used in launch files / parameters. I don't think it's that 'stupid magical' at all.

That's fair, I'm being far too harsh. All I meant to say with this is that its not part of the solver interface, so it will bite anyone who uses a prefix the first time they use it.

@wxmerkt
Copy link

wxmerkt commented Nov 5, 2016

What's currently blocking this PR from being merged?

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.

7 participants