goal tolerance of move_group and tolerance of ik

edit

Hi all,

Recently I am working on the planning of 5DOF arms but encounter some problems in finding ik solution and the motion planning afterwards. I draw a simple flow as follows:

The pipeline seems like this (if wrong please correct me): Given a targetPose(TP) and goaltolerance(GTOL), move_group will send several sampleposes within the GTOL to ik; ik finds a solution of the samplepose within IKtolerance(IKTOL); the solutions are received by move_group and checked whether they are with GTOL of TP.

By default, the tolerance of KDL_ik and trac_ik is 1e-5, and we can set GTOL to some value such as 0.02. But for 5DOF arms, the problem is that even if TP is solvable, KDL_ik and trac_ik may fail since the samplepose has some bias. For 100 solvable TP, the success rates of KDL_ik and trac_ik for my 5DOF model are about 50% and 72%.

Therefore I wrote an ik algorithm for my model and set IKTOL to some value (0.02), and set GTOL to 1e-5. In this case, the success rates of ik is 100%. However, in the ompl planning stage, move_group seems still to check whether the solution is within GTOL(1e-5) of TP. I didn't expect this... and got the error "RRTConnect: Unable to sample any valid states for goal".

If I set GTOL=0.02 and IKTOL=0.02, the success rate of ik is close to 100%, but there are still large chance of failure in the final check stage of ompl, since the bias of GTOL and IKTOL can accumulate.....

Any suggestions to achieve 100% success rate in ik and ompl stage for any pose that is within 0.02 range of a solvable pose ? Two simple ideas are: (1) set GTOL=0.02 but send the real TP to ik (IKTOL=0.02); (2) set GTOL=1e-5 and disable the check of ompl planning. But I don't know whether they are doable, I can't find the source code of passing pose to ik and final check of pose..