KDLKinematicsPlugin ignores joint_limits parameter

[jonbinney]

One symptom of this is that if you have position limits set in the joint_limits parameter, then when doing a "pick" with move_group's pick and place capability, the robot may occasionally generate plans that it thinks are successful but actually go to entirely the wrong place.

After the discussion in https://github.com/ros-planning/moveit_ros/issues/23 the ability was added to specify joint min and max positions in a "joint_limits" parameter. These values then override the values from the URDF, and are read in the KinematicModelLoader. That's fine, except that the KDLKinematicsPlugin doesn't appear to use the KinematicModelLoader. Instead, it directly loads the URDF and SRDF and then creates a robot model from them here

This means that during constraint sampling, the IK will happily return joint states that satisfy the limits from the URDF, but don't satisfy the limits from the joint_limits parameter. Other parts of the code do respect the joint_limits parameter though, which leads to confusion.

For example, in the pick and place code, the ReachableAndValidPoseFilter calls (through a few layers of indirection) the IK solver to get an IK solution that puts the end effector at the pre-grasp point. (which may not satisfy joint limits from the parameter).

The plan stage tries to construct a goal constraint from the chosen joint state here . While being constructed, the kinematic constraint notices that the joints states aren't valid, and happily adjust them to be just within the bounds here . The problem is that after changing the joint state, the modified robot configuration probably doesn't put the end effector in the right pre-grasp pose anymore! The planner then computes a plan to get to the modified pre-grasp joint states, and the pick action thinks it has successfully found a plan, even though the plan doesn't get the robot end effector anywhere near the object to be grasped.

[rhaschke]

Would you like to prepare a PR to fix that issue? It seems you already well analysed the issue.

[v4hn]

Thank you for the analysis @jonbinney, this is really informative. This issue popped up somewhere else some weeks ago, I don't remember where though.. Would it be a good idea to expose the RobotModel to the IK plugins?

[jonbinney]

Yes, I think that (at least part of) the RobotModel would need to be exposed to the kinematics plugin in order to properly fix this. The current initialize function of KinematicsBase (which the kinematics plugins inherit from) is:

 virtual bool initialize(const std::string& robot_description,
                          const std::string& group_name,
                          const std::string& base_frame,
                          const std::string& tip_frame,
                          double search_discretization) = 0;

The problem is that as things have evolved, the robot_description no longer contains all of the information about the robot that the kinematics solvers need. We could pass the RobotModel to the initialize function, as @v4hn points out. That would actually simplify the kinematics plugins, since they wouldn't have to parse the URDF themselves. A few design questions:

• do kinematics plugins need the entire RobotModel? Or is the JointModel for their joints enough?
• this would be a pretty big change; in theory all other kinematics plugins that people have written for specific robots would have to be updated, right? This shouldn't be done lightly.
• should the kinematics plugin also get a ROS NodeHandle so that it can load its own, algorithm-specific parameters from within some namespace?

Basically, if the API is going to be modified, I think it's worth getting input from people on what other limitations they've found with it.

[jonbinney]

Two alternatives that wouldn't require changing the API:

• deprecate setting of min and max positions in joint_limits parameter. People would have to modify the URDF to change the joint limits, but at least behavior would be consistent.
• Have KDLKinematicsPlugin instantiate its own RobotModel, reading in the joint_model parameters in the process. This feels ugly, because it is re-doing work and because it assumes that the parameters are in the root of the node's namespace.

[v4hn]

Without looking into it too deep, I believe it makes sense to pass the RobotModel in there instead of the name of the robot_description parameter. Enforcing URDF-external limits sounds like a good reason to do that.

Just earlier today, I got confused by the IK plugins getting their own (name of the ros parameter of the) URDF for initialization, when I tried to explain the interdependencies of the major MoveIt! classes to a colleague.

I believe it is possible to additionally provide the old interface for now and just deprecate it. This could be done, for example, by calling both functions and having no-op default implementations for both, or by adding a new interface type.

Most importantly, this requires a working implementation of the KDL plugin that initializes using the RobotModel for people to see what they need to change if they want to port a plugin to the new API. @jonbinney How much work do you expect that to be? Would you be willing to provide a rough example implementation?

Lastly, assuming we want that, we should definitely do it before kinetic is out.

[rhaschke]

+1

[jonbinney]

@v4hn that sounds reasonable, and shouldn't be too much work. I'll put together the PRs for it (one to moveit_core for adding a function to KinematicBase and one to moveit_ros for updating the KDL plugin)

[jonbinney]

Hmmm... turns out my development setup is a bit precarious at the moment. I'm using kinetic+xenial, and everything works with some odd combination of repos that I found; including some of @davetcoleman's branches of some things. When I try to switch back to kinetic-devel branches of things so I can create a PR that will be useful, things don't compile because of the whole FCL/Octomap on kinetic thing.

[davetcoleman]

+1!

@jonbinney great analysis of the discontinuity in joint limits for IK solvers. I am a big proponent of passing in the RobotModel to all IK solvers because it also significantly improves MoveIt! loading time - by default an IK solver is loaded (and a URDF is parsed) for every planning group in your SRDF that has a solver specified. In addition if you are multi-threading your solving, such as in the manipulation pipeline, you have to load a solver for each thread.

In fact I created the exact PRs you mentioned for moveit_core and moveit_ros and have been using it since 2014. I'm still a bit bitter though that it was closed (not merged) after sitting unanswered for a year. @sachinchitta finally gave the following feedback:

I think this functionality is important and very significant but it feels ad-hoc and weird to have RobotModel use kinematics base as a building block and vice-versa. I would prefer it if we rethink the relationship between the two in a more fundamental way - I think robot model should offer some kinematics solvers by default internally and expose a plugin model for other types of solvers.

While I do agree that its not ideal to have "RobotModel use kinematics base as a building block and vice-versa" I think a fundamental change in this relationship is a huge undertaking that would break major API and not something we want to do at the moment.

Note also that KinematicBase already has a dependency on RobotState, and thus RobotModel, when calling searchPositionIK

I'd welcome someone cleaning up or getting inspiration from my implementation!

[jonbinney]

@davetcoleman hah - those old PRs do pretty much exactly what we were talking about. I'll revive them and make sure they still work; then we can have a discussion in the PRs about the specifics.

[jonbinney]

Quick update: I'm going to wait until after the repo merge on friday to submit the PR. I think there will be a fair amount of discussion on it; it will be a good use case for the unified repo.