andyzeng/ikfastpy

NoConvergence: Didn't converge in maxsteps=50 steps, when regenerating ikfast61.cpp

kracon7 opened this issue · 1 comments

Hi @andyzeng,
Thanks for sharing this awesome work!
I was trying to modify the robot configuration and regenerate the ikfast61.cpp file following the instructions you gave. I got a NoConvergence error and it remains even when I test it without modifying the ur.robot.xml. Any suggestions on how to debug this issue? Appreciate it!

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 2936, in solveFullIK_6D
    tree = self.solveFullIK_6DGeneral(T0links, T1links, solvejointvars, endbranchtree, usesolvers=1)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 3271, in solveFullIK_6DGeneral
    leftovertree = self.SolveAllEquations(AllEquationsExtra,curvars=curvars,othersolvedvars = self.freejointvars+usedvars,solsubs = solsubs,endbranchtree=origendbranchtree)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 6848, in SolveAllEquations
    return self.AddSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 7511, in AddSolution
    newtree = self.SolveAllEquations(NewEquationsClean,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=newcases, currentcasesubs=newcasesubs, unknownvars=unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 6848, in SolveAllEquations
    return self.AddSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 7213, in AddSolution
    nextsolutions[var] = self.SolveAllEquations(AllEquations,curvars=newvars,othersolvedvars=othersolvedvars+[var],solsubs=solsubs+self.Variable(var).subs,endbranchtree=endbranchtree,currentcases=currentcases, currentcasesubs=currentcasesubs, unknownvars=unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 6828, in SolveAllEquations
    rawsolutions=self.SolvePairVariables(raweqns,var0,var1,othersolvedvars,unknownvars=curvars+unknownvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 8955, in SolvePairVariables
    return self.SolvePairVariablesHalfAngle(raweqns,var0,var1,othersolvedvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 7740, in SolvePairVariablesHalfAngle
    possiblefinaleq = self.checkFinalEquation(Poly(Malldet,leftvar),subs)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 8154, in checkFinalEquation
    roots = mpmath.polyroots(numpy.array(numpy.array(coeffs),numpy.float64))
  File "/usr/lib/python2.7/dist-packages/mpmath/calculus/polynomials.py", line 189, in polyroots
    % maxsteps)
mpmath.libmp.libhyper.NoConvergence: Didn't converge in maxsteps=50 steps.

Okay I managed to solve it. The problem comes from mpmath. SymPy requires mpmath >= 0.19. However, it also makes ikfast fail my robot file.
So the solution is to simply remove mpmath by sudo apt remove python-mpmath
After removing mpmath I was able to regenerate ikfast61.cpp and it works fine.