Nice work with the katana drivers.
I am working on a katana_400_6m180. Currently, it works on moveit with the default KLD solver. When using rviz, it responds quite ok to random valid poses but (due to the limitations of 5d) the interactive marker does not allow for a freely selection of poses.
Nevertheless, I would like to use IKfast for a better performance and for its use with moveit_commander. I saw that you provide an ikfast plugin for the 450 model. I want to build one for our katana but I cannot pass the openrave cpp generation command.
I converted the .urdf.xacro from the description folder (katana_description/urdf) to a urdf file and then to the desired .dae file.
However, when I try to generate the cpp, this error appears and I dot have any idea if it comes from the urdf description or from other source.
2017-02-06 17:13:25,945 openrave [WARN] [plugindatabase.h:577 InterfaceBasePtr OpenRAVE::RaveDatabase::Create] Failed to create name fcl_, interface collisionchecker
INFO: moved translation Matrix([[0, 0, -1523/10000]]) to right end
INFO: moved translation Matrix([[0, 0, 0]]) to left end
INFO: moved translation on intersecting axis Matrix([[0, 0, 0]]) to left
INFO: [[1, 0, 0, 0],[0, 1, 0, 0],[0, 0, 1, 0]]
INFO: [[cos(j0), -sin(j0), 0, 0],[sin(j0), cos(j0), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 0],[0, 0, -1, 0],[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, 19/100],[0, 1, 0, 0],[0, 0, 1, 0]]
INFO: [[cos(j2), -sin(j2), 0, 0],[sin(j2), cos(j2), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 139/1000],[0, -1, 0, 0],[0, 0, -1, 0]]
INFO: [[cos(j3), -sin(j3), 0, 0],[sin(j3), cos(j3), 0, 0],[0, 0, 1, 0]]
INFO: [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 0]]
INFO: [[cos(j4), -sin(j4), 0, 0],[sin(j4), cos(j4), 0, 0],[0, 0, 1, 0]]
INFO: [[0, 1, 0, 0],[0, 0, -1, 0],[-1, 0, 0, -757/2500]]
INFO: ikfast translation direction 5d: [j0, j1, j2, j3, j4], direction=Matrix([
[0],
[0],
[1]])
MORE LOG OUTOUT
INFO: depth=0, c=2, iter=0/1, starting newcases: set([Abs(px) + Abs(py)])
INFO: depth=1 c=3, [] [j0]: cases=set([Abs(px) + Abs(py)])
WARNING: converting polyeq "Poly(0, htj1, htj2, domain='ZZ')" to "Poly(0, htj1, htj2, domain='ZZ')"
Traceback (most recent call last):
File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 9521, in <module>
chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 2281, in generateIkSolver
chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 2796, in solveFullIK_TranslationDirection5D
coupledsolutions,usedvars = solvemethod(rawpolyeqs2[index],newsolvejointvars,endbranchtree=[AST.SolverSequence([endbranchtree2])], AllEquationsExtra=AllEquations)
File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 5054, in solveLiWoernleHiller
halfanglesolution=self.SolvePairVariablesHalfAngle(raweqns=[eq.subs(curvarsubs) for eq in AllEquations],var0=unknownvars[0],var1=unknownvars[1],othersolvedvars=self.freejointvars+[curvar])[0]
File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 7706, in SolvePairVariablesHalfAngle
for degree in range(mindegree,maxdegree+1):
File "/usr/lib/python2.7/dist-packages/sympy/core/expr.py", line 178, in __int__
r = self.round(2)
File "/usr/lib/python2.7/dist-packages/sympy/core/expr.py", line 3051, in round
mag_first_dig = _mag(x)
File "/usr/lib/python2.7/dist-packages/sympy/core/expr.py", line 3123, in _mag
mag_first_dig = int(ceil(C.Float(mpf_log(xpos._mpf_, 53))/log(10)))
OverflowError: cannot convert float infinity to integer
I have tried rounding the floating point values, using the dae file you already provide in the collada folder but the error persists.
I saw that in the indigo_catkin branch of the driver there is a small tutorial in the collada folder. I could not replicate it due to the lack of orrosplaning package in my system. Is this tutorial similar to the one provided by moveit.
Sorry for the extensive post but this is my first time using ikfast and I have not reached the point when I can really use it for robot manipulation.
I would appreciate if you can give me some pointers in how to build this plugin.