Skip to content
This repository has been archived by the owner on Nov 13, 2017. It is now read-only.

KDLKinematicsPlugin ignores joint_limits parameter #731

Open
jonbinney opened this issue Jul 28, 2016 · 11 comments
Open

KDLKinematicsPlugin ignores joint_limits parameter #731

jonbinney opened this issue Jul 28, 2016 · 11 comments

Comments

@jonbinney
Copy link
Contributor

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 #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.

@jonbinney jonbinney changed the title KDLKinematicsPlugin ignores joint_limits set parameter KDLKinematicsPlugin ignores joint_limits parameter Jul 28, 2016
@rhaschke
Copy link
Contributor

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

@v4hn
Copy link
Contributor

v4hn commented Jul 28, 2016

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
Copy link
Contributor Author

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
Copy link
Contributor Author

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
Copy link
Contributor

v4hn commented Jul 28, 2016 via email

@rhaschke
Copy link
Contributor

+1

@jonbinney
Copy link
Contributor Author

@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
Copy link
Contributor Author

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
Copy link
Member

+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
Copy link
Contributor Author

@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
Copy link
Contributor Author

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.

Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.
Labels
None yet
Projects
None yet
Development

No branches or pull requests

4 participants