Harmonize Not Using Harmonized Values

[Jmeyer1292]

The harmonize function appears to adjust joint values to be inside the -2Pi to 2Pi range. To this end, it copies the passed in ik_seed_state and computes an adjusted version of it ss. This variable is currently never used and in calculating the squared distance, the original is used instead.

I assume this is a bug, so here's the fix. I've tested it locally and the plugin seems to work but I'd be curious for the input of others.

Also, should the joint range be [-2 Pi, 2 Pi] or [- Pi, Pi ]?

[Jmeyer1292]

ping

[Jmeyer1292]

ping

[Jmeyer1292]

Updated the checks for less than -2PI.

[gavanderhoorn]

In principle I like this PR -- especially since the last addition -- as it seems to fix 2 real bugs which I think I've even ran into myself.

The issue is that without unit-tests I don't feel qualified to make the decision here to either merge or reject, as I can't say anything about regressions or unintended side-effects.

Do the recently introduced tests in moveit_kinematics_tests cover these lines?

@davetcoleman: what is your opinion?

[jrgnicho]

@gavanderhoorn the kinematics unit tests don't test this particular part of the code directly.

[jrgnicho]

@Jmeyer1292 it seems that I left this issue unattended for quite a while, I apologize for that. I believe @gavanderhoorn point about needing a unit test to verify this change is very valid, would it be too difficult to set that up?

[gavanderhoorn]

Related (from Using IkFast on a UsarsimROS Robot):

harmonize()

Current behavior

The harmonize() function in the ikfast plugin forces solution joint angles into the interval (-2π, 2π). However, potential solutions are not harmonized when they are checked to see if they are within joint limits, so solutions may be discarded even if they are valid.

Patch

The patched harmonize() function forces angles into the interval (-π, π), since USARSim has poor handling of joint limits outside of this range. This also ensures that the getClosestSolution() function performs properly, since joint angles are represented unambiguously. Also, potential solutions are harmonized before checking joint limits.

[davetcoleman]

I don't understand why it was ever -2_pi to 2_pi, seems like -pi to pi makes way more sense. Do you know why someone would have wanted it that way?

I'd feel much better merging this into Jade

[gavanderhoorn]

@davetcoleman wrote:

I don't understand why it was ever -2pi to 2pi, seems like -pi to pi makes way more sense. Do you know why someone would have wanted it that way?

I agree and I don't know. Afaict this has always been this way: at least since 370a7331.

I'd feel much better merging this into Jade

Seeing as this is (potentially) a behaviour change I'd be inclined to agree.

[gavanderhoorn]

@Jmeyer1292: it would still be really nice to have some kind of test covering this, but in any case: could you reopen against jade-devel?

[Jmeyer1292]

@gavanderhoorn @davetcoleman I can re-open it against Jade, and I can change the harmonization to work between -PI to -PI if you want.

You have to decide on the behaviour you want out of this function before I'll write any unit tests. Even with that, I'm not sure what the proper framework is for "unit testing" this one, small, private helper function. So your guidance will be required there.

[jrgnicho]

Would the right behavior here be to attempt setting the joint value within its allowed range? Some joints may have ranges from 0 to 2PI e.g., in which case harmonizing from -PI to PI might put you outside of the allowed range for that joint. The unit test could be as simple as checking that the solutions are withinthe allowed joint ranges.

[davetcoleman]

Ah you're suggesting we take the joint bounds from the URDF and harmonize it in a custom way for each joint's limits. Now that you say it like that, I see that the current implementation is not very robot-agnostic but was probably implemented for a specific platform. I'm not very experiences in how to harmonize continuous joints, I'm hoping someone else can take the lead on this

[jrgnicho]

@davetcoleman Yes, that's right, we'd use the joint limits as defined in the urdf. I think that the implementation for continuous joints would look very similar to the method RevoluteJointModel::enforcePositionBounds()

[davetcoleman]

+1

[simonschmeisser]

+1 on using joint limits from urdf, we have one robot here with asymmetric limits on j6 due to cables just to give an example

[Jmeyer1292]

So the harmonize function is only called in getClosestSolution where it's primarily used to score the "distance" between a seed state & a candidate solution. This function is never called anywhere else, so it's basically dead code.

Furthermore, even if you did call it this function then you would run into issues with the fact that it does not do joint limit testing.

That said, if we used existing joint limits then this function will have to be able to fail. We will essentially ask for every joint: If you aren't continuous, and you're out of bounds, is it possible to produce an equivalent joint position that is in bounds?. You can image for a user who sets the magnitude of joint limits to values very near zero (or any narrow range), there will often be no way to harmonize a joint value.

Is that the behaviour we want?

[davetcoleman]

Should we just remove all the dead code then? It seems redundant since we have the OPTIMIZE_MAX_JOINT functionality now. Are you guys using it for an application?