Improve KinematicsBase API

[davetcoleman]

The KinematicsBase API plugin interface is in need for a complete overhaul in order to improve performance and reduce function heading duplication. The changes I've made in #149 have made this very apparent, and I've heard some others with similar thoughts. I think we could still do this while maintaining backwards compatibility.

Improvements

• Pass a read-only reference to an already-loaded RobotModel to significantly reduce loading time. Currently the URDF is generally loaded for each planning group that has an IK solver, as well as many other places throughout MoveIt!
• In the initialize() function, also pass references to the planning group instead of a string
• Fix the IKCallbackFn so that it actually works correctly with MoveIt instead of needing a function adapter in ikCallbackFnAdapter
• Depreciate all the older functions, reducing to just one getPositionIK heading as discussed here
• In this new unified IK function, pass a robot_state pointer instead of a geometry_msgs::Pose and std::vector<double> ik_seed_state. moveit_core is suppose to be ROS-agnostic anyway.
• Other improvement ideas?

I do realize, however, this might cause some dependency issues. This issue is not an immediate concern but should be addressed when we can.

[hersh]

I'm looking into this now.

I have a question for @isucan and any others who mess with IK solvers a bunch. kinematics_plugin_loader.cpp seems to support having a list of solver classes for each group. Why? Is that important? Looking at the code, it just uses the first one which loads successfully.

kinematics_plugin_loader.cpp also has a constructor that takes a solver plugin class name as an argument. RobotModelLoader exposes this by letting the default solver loading be disabled and then having a public function loadKinematicsSolvers() which takes a KinematicsPluginLoader. This might be useful for testing, but I don't see anywhere it is used.

[isucan]

Neither of these bits of functionality are essential. Especially the multiple IK solvers for a group, we never used that. May assumption was that we might have different solvers with different abilities. For example, some that solve only for x,y,z, some that solve for the full pose. While this would be perhaps useful at some point, I think we can remove this functionality until we reach that point.

[acornacorn]

I think if we need multiple solvers for a particular group we can just create multiple groups with the same joints (but different solvers). So eliminating multiple solvers per group should be fine.

I agree having the kinematics solver loading be optional is not useful and having it be optional has cause problems for me. For example I generally want to share a single (const) RobotModel instance as widely as possible, but if one instance of RobotModel may not have kinematics loaded (or may have different kinematics loaded than I expect) then it makes sharing more difficult. So I would like to see that option disappear (so it always loads the same kinematics).

+1 for the other suggestions.

I would like to see RobotModelLoader work like a cache, so if the model is already loaded a reference to the existing RobotModel is returned. I tried to quickly implement this but ran into problems because of the kinematics thing and because RobotModel is not always const (I'm not clear what might change in it after it is loaded).

[davetcoleman]

I actually tried using the RobotModelLoader last week with the option of not loading the KinematicSolvers since I did not need them for a particular component and because, when you do load them, a URDF is parsed for every joint model group that uses an ik solver. This is a lot of overhead if you have ~5 joint model groups and many different components that need the RobotModel but that can't share the same loader.

I ended up not being able to use this feature, however, because when you skip loading the KinematicSolvers it also seems to skip loading the planning groups, meaning you can't use RobotModel or RobotState's notion of planning groups. I may have missed something in this regard or been mistaken, but it seemed to me the planning groups functionality is too intimately tied with the ik solver functionality.

So in this case, it would be useful. However, I agree it could be removed if we addressed the bigger problem of IK solvers not being passing a pointer to a RobotModel instance during initialization.

+1 for removing multiple IK solvers per planning group, this extra unused functionality only further complicates the code.

[mikeferguson]

@davetcoleman -- I think some of this made it into moveit -- if there are additional things to do, please open a new ticket against the new repo