Generating IK solution for use in MoveIT for 5 DOF arm

edit

I am working on a custom 5 DOF arm(image at the bottom). I have generated the URDF, the MoveIt configs etc for it.

I want to have the basic IK, planning and collision detection features of MoveIt. Before programming my application, I tried running the simple planning in RViz, using the interactive marker etc. Now, since it is a 5 dof system, I had to enable "Allow Approximate IK solutions", for it to let me use the interactive markers. It was using OMPL by default. I tried planning for some random-valid goal states. The problem is that the planning success rate was very very low. 4 out of 5 times the planning failed. I also tried changing the planners from the dropdowns, but none of them were consistent in planning succesfully, all of them were failing quite frequently.

After looking up this problem here on Ros-Answers and many other places, we found that IKFast would be what we should use, as it works reliably for 5 dof systems.

After installing openrave and ikfast via source, we ran the commands to generate a IKfast cpp file(using this tutorial) for our robot model. We tried the TranslationDirection5D ik type. Now here we are getting a tuple index out of range error which is something that others have faced before and have not been able to solve. See this and this. Basically, we've now hit a wall trying to get the IK on my 5 dof robot to work properly. Openrave is not even able to generate an Ikfast cpp.

Here'ss the exact error i get when i try to generate an ikfast cpp,

Traceback (most recent call last):
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 9521, in <module>
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2281, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2796, in solveFullIK_TranslationDirection5D
    coupledsolutions,usedvars = solvemethod(rawpolyeqs2[index],newsolvejointvars,endbranchtree=[AST.SolverSequence([endbranchtree2])], AllEquationsExtra=AllEquations)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 5226, in solveLiWoernleHiller
    hassinglevariable |= any([all([__builtin__.sum(monom)==monom[i] for monom in newpeq.monoms()]) for i in range(3)])
IndexError: tuple index out of range

Is there a recommended way to get IKfast to generate a cpp or maybe even get the normal KDL to work reliably for my 5 dof system? I would really appreciate any kind of help to get this to work, thanks

Here's the picture of the bot from RViz