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), movegroup will send several sampleposes within the GTOL to ik; ik finds a solution of the samplepose within IKtolerance(IKTOL); the solutions are received by movegroup and checked whether they are with GTOL of TP.

By default, the tolerance of KDLik and tracik 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, KDLik and tracik may fail since the samplepose has some bias. For 100 solvable TP, the success rates of KDLik and tracik 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..