Handle consistency limits in MoveIt! plugin

[Patrick Beeson]

MoveIt! plugin can get consistency limits that keep the answer close to the seed. SHould add support for this.

[Jimmy Da Silva]

Hello Patrick!

Nice job on this solver, it seems to work like a charm. Unfortunately this issue prevents me from using your solver with MoveIt properly (Calling the IK service to get a joint position doesn't give the closest target, BUT IK is fast and uses the seed when setting path constraints)

Do you know where the problem is exactly? I looked quickly at the code and saw that you are using the seed state everywhere so I don't know what is wrong here so far.

Looking forward to seeing this nice solver working alongside MoveIt,

Jimmy Da Silva

[Patrick Beeson]

@JimmyDaSilva If you use the Distance mode, it will return the closest IK it finds in the max time you allow it to run for (I'd suggest at least 5-10 ms depending on your chain)

[Jimmy Da Silva]

@patrickbeeson It seems to work now with the distance mode on. Thank you for your help and this great package !

[Jimmy Da Silva]

Though by repeating the same 2 IK calls over and over I sometimes (quite rare now) get an IK quite far from the current state. I suggest it's caused by an internal problem of MoveIt, or maybe the kinematics_solver_timeout param is a little bit to small.

[Patrick Beeson]

If you send me your URDF, your timeout, and the two IK call poses (with seed values if it isn't just from the two solutions), I can verify if this is a problem with my solver. I'd doubt it, but I'd like to make sure.

Prior to that, I guess I should ask if increasing the timeout helped?

[Patrick Beeson]

Bump. I'm willing to help you diagnose this issue in my spare time.

[Jimmy Da Silva]

Hi @pbeeson, sorry I have been quite busy these days working on something else. My robot is the Kuka LWR 4+ with some additional tools. My URDF can be found here : https://github.com/kuka-isir/rtt_lwr/tree/rtt_lwr-2.0/lwr_description

Thanks a lot for your help :)

[Patrick Beeson]

OK. I need a bit more info to successfully reproduce this:

1. What time out are you using?
2. What joint are you commanding to the Cartesian goal?
3. What two Cartesian goals were you using to make the graph above?

[Jimmy Da Silva]

@pbeeson Sorry for the late reply.

1. 0.01 for the kinematics_solver_timeout
2. link_7
3. I don't have the exact values anymore. But it was something like (0.5, 0.5, 0.5) <-> (-0.5, 0.5, 0.5) for the position and RPY = (-pi, 0.0, 0.0) for the orientation

Thanks!

[Patrick Beeson]

After looking into this, I'm pretty convinced this error is NOT due to trac-ik. TRAC-IK runs all solutions for Distance through a function (normalize_seed() in track_ik.cpp) that looks at each joint in the solution and if it is possible to add/subtract 2PI and be closer to the seed value for that joint, it does so. The graph you show where joint6 goes REALLY high to +3, would be detected by this function as > PI from the seed (current position) near -1.6 and would have changed that joint to be closer to -3 instead of +3. My guess is that something (MoveIt!) is calling IK with a bad seed. IF you could nail down exactly the code used that calls moveit it repeatedly between two poses that exhibits this behavior, I could reproduce on my machine, I could determine if the seeds are bad, or if indeed there is a rare bug that I cannot reproduce with my current testing suite.

[Jimmy Da Silva]

That's what I thought. It's not the first time I see MoveIt giving bad seed states. Especially if the time between two plans is smaller than 1s it seems that the start state is not updated correctly.

@pbeeson Thank you so much for looking into this and making this amazing repo.

Looking forward to seeing trac_ik release soon with ros and MoveIt