Handle consistency limits in MoveIt! plugin
MoveIt! plugin can get consistency limits that keep the answer close to the seed. SHould add support for this.
Comments (11)
-
-
Account Deactivated reporter @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)
-
@patrickbeeson It seems to work now with the distance mode on. Thank you for your help and this great package !
-
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. -
Account Deactivated reporter 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?
-
Account Deactivated reporter Bump. I'm willing to help you diagnose this issue in my spare time.
-
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 :)
-
Account Deactivated reporter OK. I need a bit more info to successfully reproduce this:
- What time out are you using?
- What joint are you commanding to the Cartesian goal?
- What two Cartesian goals were you using to make the graph above?
-
@pbeeson Sorry for the late reply.
- 0.01 for the kinematics_solver_timeout
- link_7
- 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!
-
Account Deactivated reporter - changed status to closed
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. -
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 - Log in to comment
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