Removed prefix code which prevented the plugin from being used for mu… #248
Conversation
…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
|
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) |
|
@ipa-fxm @gavanderhoorn @shaun-edwards @abubeck @kphawkins |
…regardless of prefixes
|
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. |
|
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? |
|
You're right, it doesn't. But the custom plugin is way more responsive. |
|
FYI, managed to get this working https://twitter.com/clearpathrobots/status/746058485327085568 Check it out |
|
@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(); | ||
|
|
||
| /* |
There was a problem hiding this comment.
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 ;).
There was a problem hiding this comment.
I removed the code that is commented out :)
|
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 */ | |||
There was a problem hiding this comment.
It should be unaffected since // is there, but I've removed it.
|
While I understand the reason for the change you propose, I'm not sure removing the check is something we necessarily want to do.
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? |
|
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. |
|
@TheDash wrote:
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 There are quite some posts on Interesting quote from the
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 wrote:
Apparently that was @Jmeyer1292, he even opened an issue to discuss this over at moveit/moveit_ikfast#60. |
|
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? |
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. |
|
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. |
|
@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 |
|
I'm really not happy about removing the prefix just because it's harder to get the configuration right. But:
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. |
|
@TheDash wrote:
It's exactly this: assume. Isn't there an American colloquialism about assumptions? |
|
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 |
that would be ideal, yes. But again, I'm just one voice here. |
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. |
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. |
|
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:
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. |
|
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. |
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:
|
|
@Jmeyer1292 wrote:
Are you saying that you feel it's better to remove the check to make this plugin act as other IKFast plugins (which And how will changing / removing the check as proposed by @TheDash change the solutions?
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.
The |
yes - this logic is what i propose - we are on the same page. @Jmeyer1292
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.
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
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]); |
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. |
|
What's currently blocking this PR from being merged? |
…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