Elbow joint self-collisions break path planning when full joint space is used on UR5

[scottpaulin]

When the elbow joint is close to Pi or -Pi the UR5 will be in self-collision, no matter what positions the other joints are in:

Fig. 1: Self collision when elbow joint is near -Pi or Pi. Moving the other joints will not bring the robot out of self-collision.

This results in the collision free part of the UR5's configuration space to be disjoint:

Fig 2: Collision free part of the UR5's configuration space (CFree) when only considering self-collisions. The green circles show where CFree becomes discontinuous because of the elbow joint self collisions shown in Fig. 1. CFree is made up of 3 disjoint sets A, B, and C depending on the position of the elbow joint.

The 'disjoint-ness' shown in Fig. 2 means that it is impossible to plan a path where the elbow joint position starts in one of the sets A, B, or C and finishes in another. I think this is why plans fail reasonably often when using the full joint limits of the UR5.

This could be fixed by limiting the elbow joint to [-Pi, Pi) when full joint limits are used. The other joints will be fine to use their full [-2Pi, 2Pi) range of motion.

I have not tested with the other UR robots, but they may have a similar issue.

[gavanderhoorn]

Trying to understand this better: MoveIt will refuse/fail to plan whenever the start state puts the robot in a (self) collision. Isn't that what is happening here?

Not saying this isn't a problem, jsut trying to figure out "whose" problem it is.

[ThomasTimm]

It is not the start state that is in a self collision, it is part of the path.
If the robot needs to move the elbow joint from 2.5rad to 0.36rad, the same configuration would be to have the arm at 3.5rad, so the inverse kinematics solver chooses that solution. But that requires the arm to move through itself and thus the planning fails (as I've understood it).
Although only giving it a few minutes of thoughts, I think it is a reasonable solution. It kind of makes me wonder why the elbow joint has such a large motion range, in reality it can never move outside [pi;-pi)..?

[gavanderhoorn]

Ah, makes sense. Unconsciously I had added "at the start of planning" everywhere. Not sure why.

The large joint ranges are a characteristic of the robot and its controllers. Personally I'd like some really strong arguments for why we should not use them (or configure our packages with non-default limits). I know we currently have the joint limited versions of everything, but I'd rather we do away with those as well, as soon as someone figures out why those are needed anyway (moveit/moveit#276).

[scottpaulin]

just trying to figure out "whose" problem it is.

Universal robot (the massive company, not this repo) should mention this problem in their documentation for the UR5. I think the only way to get full [-2PI, 2PI) range of motion with the elbow joint is if the UR5 was redesigned to avoid the self-collisions in Fig 1.

Edit: because my browser refreshed and I saw @ThomasTimm answer

[scottpaulin]

It kind of makes me wonder why the elbow joint has such a large motion range, in reality it can never move outside [pi;-pi)..?

It might be cheaper to only manufacture joints that all have the same limits?

[gavanderhoorn]

@scottpaulin wrote:

It might be cheaper to only manufacture joints that all have the same limits?

I would be surprised if cost has anything to do with it: the limits are software defined.

I could see it being easier though: instead of having to look up the specs for the limits (and have a table with limits per joint, as other mfgs have), users can just assume the 2pi range for all joints.

[gavanderhoorn]

But this is all speculation.

@scottpaulin: if I understand things correctly, this issue is more of a feature request (ie: perhaps these packages should restrict the elbow to never be able to collide with itself) instead of a bug report?

[scottpaulin]

Its a bug report. Limiting the elbow joint range of motion should fix the planning in Moveit

[gavanderhoorn]

Its a bug report.

<pedantic>A bug report would be reporting something that is wrong, or deviates from the expected or specced behaviour. How can setting the joint limits to what the mfg specs be a bug?</pedantic>

Limiting the elbow joint range of motion should fix the planning in Moveit

Could you perhaps comment on moveit/moveit#276? I'd like to include the OP there in this discussion, as he must have had a reason for implementing a fair bit of infrastructure instead of the seemingly 'trivial' work around of limiting the range of the elbow.

@achim-k?

[scottpaulin]

Personally I'd like some really strong arguments for why we should not use them (or configure our packages with non-default limits)

For someone using the Moveit interface the joint limited version works best. If you are careful with what IK solution is used then the non-joint-limited version works best. I am currently testing and writing a short paper about how to get planners to work well with the full joint space and handle OP's problem.

[gavanderhoorn]

Great. Looking forward to read it.

If you are careful with what IK solution is used then the non-joint-limited version works best.

your example of going from -0.5pi to -1.5pi would seem to only occur when using joint space targets, correct? For pose targets, if the IK solution does not fall in the 'unreachable' space, everything should be fine. But I'm guessing that is what you are saying in your above comment?

[scottpaulin]

For pose targets, if the IK solution does not fall in the 'unreachable' space, everything should be fine

Yip! This is why the planning works sometimes. It fails if the IK solution is in the 'unreachable' space.

[gavanderhoorn]

Ok. So would your solution / work-around be 'just' to limit the elbow ranges, or do you have something more advanced up your sleeve?

Or should I just wait for your paper? :)

[scottpaulin]

Ok. So would your solution / work-around be just to limit the elbow ranges, or do you have something more advanced up your sleeve?

Its on the motion planning side, so doesn't belong here. The elbow joint limits thing is just a bug (either hardware or software). I would have finished my PhD a loooooong time ago if I could publish about software bugs (especially the ones I have written).

[gavanderhoorn]

I would have finished my PhD a loooooong time ago if I could publish about software bugs (especially the ones I have written).

Ha, welcome to the real world, with software interacting with real hw 🍻.

[v4hn]

This is a great analysis!

Its a bug report. Limiting the elbow joint range of motion should fix the planning in Moveit

Did you actually test this hypothesis? Could you provide a pull-request for this?
I'm eager to see this tested on our setup too.

I would have finished my PhD a loooooong time ago if I could publish about software bugs (especially the ones I have written).

I hear you...

[gavanderhoorn]

@scottpaulin wrote:

Its on the motion planning side, so doesn't belong here. The elbow joint limits thing is just a bug (either hardware or software).

So would the following then work:

1. leave the limits in the urdfs as they are (perhaps even remove the joint limited versions)
2. update the moveit pkgs to limit the elbow to +-PI
3. make sure all IK solvers use those limits, instead of hard-coded ones (KDLKinematicsPlugin ignores joint_limits parameter moveit/moveit_ros#731 and Pass RobotModel to kinematics plugins moveit/moveit#150)

And finally: document this all properly.

[scottpaulin]

Did you actually test this hypothesis? Could you provide a pull-request for this?
I'm eager to see this tested on our setup too.

I use OMPL directly, and have tested it through this. I have not tested by changing the joint limits in this package and then using the Movegroup interface.

Edit: for clarity

[scottpaulin]

So would the following then work:

I am not super familiar with URDF's and Moveit (because I use this package :) ), but anything that limits the joint range for the elbow in Moveit should work.

leave the limits in the urdfs as they are (perhaps even remove the joint limited versions)

The joint limited version is easier to use. The non-limited joint version is only better if you are careful with the IK solutions used in planning. With the non-joint limited version IK solvers can produce targets that are a long way from the start state.

[scottpaulin]

Short answer:
There are two things to change (bugs to fix) 1) the longest_valid_segment_fraction (from ompl_planning.yaml) is about 10x too large. Change it to 0.005. 2) In ur5.urdf.xacro change the upper limit of elbow_joint to pi and lower limit to -pi when not using the joint limited UR5 as we have discussed.

Long answer:
I booted up the UR5 and started planning to random valid targets through RVIZ. The planners would always returning quickly, but sometimes they would fail with this message:

[ WARN] [1476187001.882120692]: Fail: ABORTED: Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.

This basically means that the planner (or path simplifier) thinks it has a collision-free solution, but that solution is actually in collision. This can happen when the collision detection discretization is too large (i.e. the longest_valid_segment parameter in ompl_planning.yaml is too large). In OMPL longest_valid_segment would be a fraction, but I am not sure how Moveit handles it. If it is a fraction, the absolute length works out to be about 1.43 radians which is very large. Changing the longest_valid_segment parameter in ompl_planning.yaml to 0.005 stopped the error message, meaning that actual valid paths were being computed.

Now when the robot is tested (without limiting the elbow joint) it times out (fails) on many queries, this will be because of the elbow self collisions. The planners then work when the elbow is limited to -pi, pi.

Sorry if some of the above is not well written. It's 1:30 am here.

[gavanderhoorn]

@scottpaulin wrote:

leave the limits in the urdfs as they are (perhaps even remove the joint limited versions)

The joint limited version is easier to use. The non-limited joint version is only better if you are careful with the IK solutions used in planning. With the non-joint limited version IK solvers can produce targets that are a long way from the start state.

imo IK solutions being "far away from the start state" is an orthogonal problem to what you opened the issue for. Properly configured IK solvers (such as trac_ik with the distance metric configured fi) can be used to solve that. We'd have to check whether ur_kinematics does the same.

If restricting the elbow to +-pi resolves the planning issue, I'd rather ditch the joint limited versions of the urdfs: they confuse new users, duplicate stuff, require maintenance, etc.

Until we can find out why having +-2pi on the elbow would be better, having just that joint restricted in the way we discussed makes sense to me. The rest would not seem to suffer from this discontinuity in the planning space.

[scottpaulin]

Yea maintaining the joint limited version sounds like a pain. I was just worried that it might increase the barrier to entry for new users if they have to think about the IK solution they are using.

[rbbg]

Very interesting stuff! I spent some time thinking about this problem after the MoveIt! issue and I suspected the same thing regarding the longest_valid_segment parameter. Digging a little bit:

According to Ioan Sucan (here) the setStateValidityCheckingResolution(..) of the SpaceInformation class is used to set this parameter, in the API we find:

_void setStateValidityCheckingResolution (double resolution)
Set the resolution at which state validity needs to be verified in order for a motion between two states to be considered valid. This value is specified as a fraction of the space's extent. This call is only applicable if a ompl::base::DiscreteMotionValidator is used. See State Validity Checking._

Regarding the disjointed space, that's very odd. +1 for just limiting the elbow and leaving the rest as is. Hopefully that fixes it.

[v4hn]

Yes longest_valid_segment_fraction limits the number of collision checks as a fraction of the state space, and just forwards the parameter to OMPL.
@davetcoleman chose to set this to 0.05 by default in MoveIt, OMPL currently uses a default of 0.01.
I keep thinking about whether it would make sense to set MoveIt's default to that too, but that's a different story. If you think so too, feel free to file a pull-request against moveit.
For the not-limited version of the UR5 urdf both values are definitely way to high.

+1 to restrict the elbow joint in the not-limited URDF in this repo. It does not make much sense to specify the full joint space of the individual joint if the whole arm makes it impossible to reach it.

However, about everyone using the UR's with MoveIt uses the joint-limited version right now and the non-limited file makes it more complex to generate reasonable IK solutions. I would therefore like to keep the limited urdf in the repo too.

[achim-k]

Could you perhaps comment on moveit/moveit#276? I'd like to include the OP there in this discussion, as he must have had a reason for implementing a fair bit of infrastructure instead of the seemingly 'trivial' work around of limiting the range of the elbow.

I was under the impression that this was a moveit limitation. But yes, this seems much better than my solution 😄

+1 to restrict the elbow joint in the not-limited URDF in this repo. It does not make much sense to specify the full joint space of the individual joint if the whole arm makes it impossible to reach it.

However, about everyone using the UR's with MoveIt uses the joint limited version right now and the non-limited file makes it more complex to generate reasonable IK solutions. I would therefore like to keep the limited urdf in the repo too.

I think so too

[gavanderhoorn]

@v4hn wrote:

However, about everyone using the UR's with MoveIt uses the joint-limited version right now and the non-limited file makes it more complex to generate reasonable IK solutions. I would therefore like to keep the limited urdf in the repo too.

We'd use the normal tick-tock approach if we'd be removing the limited urdf, so it'd probably be around in Kinetic.

We'll need to run a few tests, but I believe if we make trac_ik with the distance solve type the default IK plugin we could probably make the non-limited model (with the limited elbow) the default.

One real issue that would solve is that if the physical hw is already outside the limited limits, it's currently impossible to get it out of that pose (at least with moveit). #112 and #127 are examples of that, but I've run into it myself a few times as well.

[gavanderhoorn]

As to the longest_valid_segment_fraction and a better default value: does anyone have any figures for how this implements the average planning time? Just to get a feeling for the performance impact?

[davetcoleman]

I set the longest_valid_segment_fraction back in 2012 per the advice of Ioan - it was chosen as a balance between "enough" collision checking coverage and planning speed. I think 4 years later we all have a generation of faster computers (though mostly via multi-threading) so perhaps the 5x speed hit could still be worth the improved collision checking results. In my code I always set the collision checking resolution to 0.01 rather than the course 0.05

+1 to opening a PR to discuss this further.

[scottpaulin]

The longest_valid_segment_fraction used should possibly depend on whether the full joint, or joint-limited version is being used because the state spaces are different sizes.

One real issue that would solve is that if the physical hw is already outside the limited limits, it's currently impossible to get it out of that pose (at least with moveit)

This should only be a problem when people start up the robot. Helpful error messages e.g. "The start state is invalid" (which Moveit might already have) might be enough to get someone to put the robot within its joint limits.

@gavanderhoorn What is the status of the custom IK solver in ur_kinematics?

[v4hn]

On Tue, Oct 11, 2016 at 09:31:46PM -0700, Scott Paulin wrote:

This should only be a problem when people start up the robot. Helpful error messages e.g. "The start state is invalid" (which Moveit might already have) might be enough to get someone to put the robot within its joint limits.

It can also happen if you move the robot around by hand during operation with the ur_modern_driver and try to plan again afterwards.

And yes, MoveIt already complains about this when trying to plan. The error message is "No start state found. Joints out of bounds" or something like that.

[gavanderhoorn]

@scottpaulin wrote:

One real issue that would solve is that if the physical hw is already outside the limited limits, it's currently impossible to get it out of that pose (at least with moveit)

This should only be a problem when people start up the robot. Helpful error messages e.g. "The start state is invalid" (which Moveit might already have) might be enough to get someone to put the robot within its joint limits.

Thing is, I know of / have a lot of applications in the field where getting a "helpful error message" is nice, but not enough. The limited versions of these urdfs is a work-around, and I'd rather do away with it, if possible. It would remove the need to have special logic in applications for when the robot turns out to be outside the "limited limits".

Less special logic -> higher re-usability.

[davetcoleman]

moveit does have a planning request adapter enabled by default that will allow planning even if the start state is slightly out of bounds:

https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp#L152

[scottpaulin]

moveit does have a planning request adapter enabled by default that will allow planning even if the start state is slightly out of bounds

Ah, that's really useful!

Assuming that the robot ships with the elbow joint in region B (Fig 2), I don't think it can end up in regions A or C (unless someone is really keen and dismantles the robot, probably on April Fool's day). We just need to stop getting IK solutions in regions A and C.

[facontidavide]

Hi,

my two cents. As a new comer to the wonderful world of (UR + MoveIt), I got this issue as well.
I am happy that you started this discussion, otherwise I would have wasted a lot of time.

+1 For reducing the joint limit in the elbow to [-pi, +pi)

[gavanderhoorn]

#268 worked around the elbow joint limit issue (in the non joint-limited xacros), and #266 reduced the longest_valid_segment_fraction for all moveit configs provided in this repository.

That would seem to address the issues reported by @scottpaulin.

Because of that, I'm closing this.

I would invite users that run into issues related to the ones reported in this issue to open a new issue and to refer back to this one.

[gavanderhoorn]

Thanks @scottpaulin for investigating this, and thanks all the others for contributing to the discussion.

🍻 👍

[scottpaulin]

Awesome! Thanks.