openrave fails to generate IK solver
I have been following the instructions here: http://moveit.ros.org/wiki/Kinematics... for generating an ikfast moveit plugin for a ur5. I had to use a pre-made collada file from here: https://github.com/rdiankov/collada_r... and renamed it to the appropriate filename. I got to the step where i attempt to use this command:
python `openrave0.9-config --python-dir`/openravepy/_openravepy_0_9/ikfast.py --robot=ur5round.dae --iktype=transform6d --baselink=1 --eelink=6 --savefile=ikfast61_manipulator.cpp
it appears as though it begins to work, but it fails shortly after. Here is the terminal output:
INFO: moved translation Matrix([[0, 0, 19/400]]) to right end
INFO: moved translation Matrix([[0, 0, 1197/10000]]) to left end
INFO: moved translation on intersecting axis Matrix([[0, 0, 0]]) to left
INFO: [[1, 0, 0, 0],[0, 0, 1, -2/125],[0, -1, 0, 0]]
INFO: [[cos(j1), -sin(j1), 0, 0],[sin(j1), cos(j1), 0, 0],[0, 0, 1, 0]]
INFO: [[-1, 0, 0, 0],[0, -1, 0, 17/40],[0, 0, 1, 0]]
INFO: [[cos(j2), -sin(j2), 0, 0],[sin(j2), cos(j2), 0, 0],[0, 0, 1, 0]]
INFO: [[0, -1, 0, 0],[1, 0, 0, -39243/100000],[0, 0, 1, -93/1000]]
INFO: [[cos(j3), -sin(j3), 0, 0],[sin(j3), cos(j3), 0, 0],[0, 0, 1, 0]]
INFO: [[0, -1, 0, 0],[0, 0, -1, 93/1000],[1, 0, 0, 0]]
INFO: [[cos(j4), -sin(j4), 0, 0],[sin(j4), cos(j4), 0, 0],[0, 0, 1, 0]]
INFO: [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 0]]
INFO: [[cos(j5), -sin(j5), 0, 0],[sin(j5), cos(j5), 0, 0],[0, 0, 1, 0]]
INFO: [[0, -1, 0, 0],[0, 0, -1, 0],[1, 0, 0, 19/400]]
Traceback (most recent call last):
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 7478, in <module>
chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 1836, in generateIkSolver
chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 2306, in solveFullIK_6D
self.testconsistentvalues = self.ComputeConsistentValues(jointvars,self.Tfinal,numsolutions=4)
File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_9/ikfast.py", line 1885, in ComputeConsistentValues
psubs.append((s,v.subs(psubs)))
File "/usr/local/lib/python2.7/dist-packages/sympy/core/basic.py", line 976, in subs
rv = rv._subs(old, new, **kwargs)
File "/usr/local/lib/python2.7/dist-packages/sympy/core/cache.py", line 93, in wrapper
r = func(*args, **kw_args)
File "/usr/local/lib/python2.7/dist-packages/sympy/core/basic.py", line 1090, in _subs
rv = fallback(self, old, new)
File "/usr/local/lib/python2.7/dist-packages/sympy/core/basic.py", line 1062, in fallback
arg = arg._subs(old, new, **hints)
File "/usr/local/lib/python2.7/dist-packages/sympy/core/cache.py", line 93, in wrapper
r = func(*args, **kw_args)
File "/usr/local/lib/python2.7/dist-packages/sympy/core/basic ...