Implemented new getPositionIK() from Kinematics Base

[Jmeyer1292]

See the new overload for getPositionIK for documentation on the new function.

We over in the Descartes project require "all" of the solutions that an analytical inverse kinematics routine can return (not just the closest to a seed). We got a few pull requests in that add such a function to moveit's KinematicsBase. Here I present an implementation of this function for this special model.

There's a little more duplication with existing code than I like, but I moved out some of the most obviously separable logic to a new helper function.

Let me know what you think; I'm eager to make the Univeral Robots trivially inter-operable with Descartes.

[shaun-edwards]

@Jmeyer1292, thanks for the contribution. Sorry for taking so long to review.

@davetcoleman have you had a chance to use this?

[Jmeyer1292]

@shaun-edwards This is not what I prepared for Dave, but rather what part of my testing for the IKFast Interface I added to Descartes a while back.

For work akin to dave's to be generally usable, we'll need something like this and #248 (and maybe more). I took the easy route and fixed things in our special plugin.

[gavanderhoorn]

@Jmeyer1292: why would #248 be needed for this?

[Jmeyer1292]

@gavanderhoorn I meant for @davetcoleman and his work on hilgendorf. Two UR robot arms and all.

[gavanderhoorn]

Ah, right. I still feel that is more of a MoveIt problem than something we need to work-around on our end though. But see #248.

[TheDash]

@gavanderhoorn It's mostly a plugin problem. Most other pugins (PR2, KDL) use the method of using the joint names passed in via the group to determine where the prefix is to be added and to determine what joints are in the kin chain.

I got the idea for the fix by looking at how the PR2 grabs joints from its plugin. Fun fact: as long as you have 7 joints in your kin chain the PR2 plugin will work. It wont be optimized unless its the PR2 arm, but it will load the plugin.

That's kinda the scenario we're facing now.

[gavanderhoorn]

@TheDash wrote:

@gavanderhoorn It's mostly a plugin problem. Most other pugins (PR2, KDL) use the method of using the joint names passed in via the group to determine where the prefix is to be added and to determine what joints are in the kin chain.

yes, and that works for those plugins because KDL (and trac_ik as well) dynamically setup their internal solver structures (ie: all their internal parameters are configured at runtime, based on the kinematic structure described in the URDF). pr2_arm_kinematics is essentially a KDL plugin. It just adds some niceties to it.

IKFast and ur_kinematics don't do that: IKFast plugins are generated off-line, with all parameters hard-coded into the source. ur_kinematics wasn't generated, but hand-coded based on @kphawkins calculations.

You simply cannot (re-)use IKFast or ur_kinematics for anything but the kinematic configurations they were generated for.

[TheDash]

You simply cannot (re-)use IKFast or ur_kinematics for anything but the kinematic configurations they were generated for.

Which is why we should also put a check in the code to see if it has the needed joints for the UR arms, but to ignore prefixes. That gets the best of both worlds.

I will continue that discussion on #248 as it is kinda side tracking this thread

[davetcoleman]

@shaun-edwards no I have not used this. I'm using the ur5_robot_model included in roscon_2015 demo. This looks like it would be a nice cleanup though

[gavanderhoorn]

This is probably something that should / can be reviewed in combination with #316 and #305.

I won't be able to do this before ROSCon however, and I would greatly appreciate some help.

[gavanderhoorn]

Tagging this PR with wrid18 as this is a PR that would be nice to review.

#305 and #316 should be taken into account / considered as well.

[mxgrey]

If these changes are not strictly needed in indigo-devel, I would suggest closing this PR in favor of #358 which combines these changes with #305 and targets kinetic-devel.

Alternatively, we could create a PR for the changes in #358 that targets indigo-devel if there are still indigo users that want them.