5DOF arm cannot load IK libraries

[Lance]

Hello,

I am getting a few errors I cannot figure out from the rest of the forum when trying to load the IK solver for my 5DOF robot, using OpenRave 0.9.0 on ubuntu 12.04.  I tried following the tutorial for ik5, but the following line:
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=IkParameterization.Type.TranslationDirection5D)
does not create a correct model, since openrave attempts to autogenerate one afterwards.  That is where my errors occur.  I am not sure if I made a mistake in the xml file (has 6 links and 5 joints), or somewhere else?

Error output:
openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip arm: TranslationDirection5D [0, 1, 2, 3, 4] (this might take up to 10 min)
openravepy.databases.inversekinematics: generate, creating ik file /home/boose/.openrave/kinematics.67ec4fe8d9a8559dd78ba0a103495c88/ikfast69.TranslationDirection5D.0_1_2_3_4.cpp
/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py:1292: RuntimeWarning: invalid value encountered in divide
  axisangle /= angle
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 229/1000] to right end
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 1/40] to left end
openravepy.ikfast: forwardKinematicsChain, moved translation on intersecting axis [0, 0, 17/250] to left
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 1, 0, 0],[0, 0, 1, 1/40]]
openravepy.ikfast: generateIkSolver, [[cos(j0), -sin(j0), 0, 0],[sin(j0), cos(j0), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, -3/200],[0, 1, 0, 0],[-1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j1), -sin(j1), 0, 0],[sin(j1), cos(j1), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, -19/200],[0, 1, 0, 0],[0, 0, 1, 37/500]]
openravepy.ikfast: generateIkSolver, [[cos(j2), -sin(j2), 0, 0],[sin(j2), cos(j2), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, -229/1000],[0, 1, 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j3), -sin(j3), 0, 0],[sin(j3), cos(j3), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, -17/250]]
openravepy.ikfast: generateIkSolver, [[cos(j4), -sin(j4), 0, 0],[sin(j4), cos(j4), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 1, 0, 0],[0, 0, 1, 299/1000]]
openravepy.ikfast: solveFullIK_TranslationDirection5D, ikfast translation direction 5d: [j0, j1, j2, j3, j4]
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller general ik method
openravepy.ikfast: solveFullIK_TranslationDirection5D, 'Li/Woernle/Hiller method requires 3 unknown variables, has 2'
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller general ik method
openravepy.ikfast: solveLiWoernleHiller, could not find any variable where number of equations is exacty 8, trying all possibilities
openravepy.ikfast: solveLiWoernleHiller, allowedindex 0 found 0 equations where coefficients of equations match
openravepy.ikfast: solveFullIK_TranslationDirection5D, 'new monoms is 34>20'
openravepy.ikfast: solveKohliOsvatic, attempting kohli/osvatic general ik method
openravepy.ikfast: solveFullIK_TranslationDirection5D, 'Kohli/Osvatic method requires 3 unknown variables'
openravepy.ikfast: solveKohliOsvatic, attempting kohli/osvatic general ik method
openravepy.ikfast: solveFullIK_TranslationDirection5D, 'need 8 or less equations of one variable, currently have 12'
Traceback (most recent call last):
  File "test_ik5.py", line 19, in <module>
    ikmodel.autogenerate()
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/inversekinematics.py", line 624, in autogenerate
    self.generate(iktype=iktype,freejoints=freejoints,precision=precision,forceikbuild=forceikbuild,outputlang=outputlang,ipython=ipython)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/inversekinematics.py", line 846, in generate
    chaintree = solver.generateIkSolver(baselink=baselink,eelink=eelink,freeindices=self.freeindices,solvefn=solvefn)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 1836, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/inversekinematics.py", line 678, in solveFullIK_TranslationDirection5D
    return self.ikfast.IKFastSolver.solveFullIK_TranslationDirection5D(*args,**kwargs)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2239, in solveFullIK_TranslationDirection5D
    raise self.CannotSolveError('raghavan roth equations too complex')
openravepy.ikfast.CannotSolveError: 'raghavan roth equations too complex'
[qtcoinrave.cpp:80] SoQt releasing all memory
Coin debug: socontexthandler_cleanup(): 35 context-bound resources not free'd before exit.


Python code:
from openravepy import *
import numpy
from openravepy.misc import InitOpenRAVELogging
InitOpenRAVELogging()

env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
robot = env.ReadRobotXMLFile('data/d.xml')
env.Add(robot, True)
robot.SetActiveManipulator('arm')

print "generate the ik solver"
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=IkParameterization.Type.TranslationDirection5D)
print ikmodel
if not ikmodel.load():
    print "autogenerating..."
    ikmodel.autogenerate()


And here is my custom xml file:
<?xml version="1.0" encoding="UTF-8"?>
<Robot name="d">

 <KinBody>
   
   <Body name="lower_base" type="dynamic">
     <Translation>0 0 0</Translation>
     <Geom type="box">
       <Translation>0 0 0.0125</Translation>
       <Extents>0.1 0.09 0.0125</Extents>
     </Geom>
   </Body>

   
   <Body name="upper_base" type="dynamic">
     <offsetfrom>lower_base</offsetfrom>
     <Translation>-0.015 0 0.025</Translation>
     <Geom type="box">
       <Translation>0 0 0.0475</Translation>
       <Extents>0.075 0.039 0.0475</Extents>
     </Geom>
   </Body>

   
   <Body name="upper_arm" type="dynamic">
     <offsetfrom>upper_base</offsetfrom>
     <Translation>0.006 0 0.095</Translation>
     <Geom type="box">
       <Translation>0 0 0.1485</Translation>
       <Extents>0.034 0.034 0.1485</Extents>
     </Geom>
   </Body>

   
   <Body name="forearm" type="dynamic">
     <offsetfrom>upper_arm</offsetfrom>
     <Translation>0.068 0 0.229</Translation>
     <Geom type="box">
       <Translation>0 0 0.1485</Translation>
       <Extents>0.034 0.034 0.1485</Extents>
     </Geom>
   </Body>

   
   <Body name="wrist" type="dynamic">
     <offsetfrom>forearm</offsetfrom>
     <Translation>-0.068 0 0.229</Translation>
     <Geom type="box">
       <Translation>0 0 0.034</Translation>
       <Extents>0.034 0.034 0.034</Extents>
     </Geom>
   </Body>

   
   <Body name="hand" type="dynamic">
     <offsetfrom>wrist</offsetfrom>
     <Translation>0 0 0.07</Translation>
     <Geom type="cylinder">
       <rotationaxis>1 0 0 90</rotationaxis>
       <radius>0.023</radius>
       <height>0.0075</height>
       <diffuseColor>0.05 0.05 0.05</diffuseColor>
     </Geom>
   </Body>



   
   <Joint circular="true" name="base_joint" type="hinge">
     <Body>lower_base</Body>
     <Body>upper_base</Body>
     <offsetfrom>lower_base</offsetfrom>
     <limitsdeg>-180 180</limitsdeg>
     <axis>0 0 1</axis>
     
     <weight>4</weight>
     <maxvel>5</maxvel>
     <resolution>1</resolution>
   </Joint>

   
   <Joint circular="false" name="shoulder" type="hinge">
     <Body>upper_base</Body>
     <Body>upper_arm</Body>
     <offsetfrom>upper_base</offsetfrom>
     <limitsdeg>-90 90</limitsdeg>
     <axis>1 0 0</axis>
     <maxvel>1000</maxvel>
     <maxtorque>1000</maxtorque> >
     <weight>4</weight>
     <resolution>1</resolution>
   </Joint>

   
   <Joint circular="false" name="elbow" type="hinge">
     <Body>upper_arm</Body>
     <Body>forearm</Body>
     <offsetfrom>upper_arm</offsetfrom>
     <limitsdeg>-180 180</limitsdeg>
     <axis>1 0 0</axis>
     <maxvel>1000</maxvel>
     <maxtorque>1000</maxtorque>
     
     <weight>4</weight>
     <resolution>1</resolution>
   </Joint>

   
   <Joint circular="false" name="wrist1" type="hinge">
     <Body>forearm</Body>
     <Body>wrist</Body>
     <offsetfrom>forearm</offsetfrom>
     <limitsdeg>-180 180</limitsdeg>
     <axis>1 0 0</axis>
     <maxvel>1000</maxvel>
     <maxtorque>1000</maxtorque>

     
     <weight>4</weight>
     <resolution>1</resolution>
   </Joint>


   
   <Joint circular="true" name="wrist2" type="hinge">
     <Body>wrist</Body>
     <Body>hand</Body>
     <offsetfrom>wrist</offsetfrom>
     <limitsdeg>-180 180</limitsdeg>
     <axis>0 0 1</axis>
     
     <maxvel>1000</maxvel>
     <maxtorque>1000</maxtorque>
     <weight>4</weight>
     <resolution>1</resolution>
   </Joint>

   <transparency>0.1</transparency>
 </KinBody>

 <Manipulator name="arm">
   <effector>hand</effector>
   <base>lower_base</base>
 </Manipulator>

</Robot>


I would really appreciate any help with this!  Thank you very much for your time!

Best,
Lance

[Lance]

Just an update on this.  With the following xml file I can successfully (it appears) use IK Translation3D, but when attempting TranslationDirection5D, it attempts to autogenerate the IK and I get the below error.  Any ideas on how to fix this?

/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py:1292: RuntimeWarning: invalid value encountered in divide
  axisangle /= angle
No handlers could be found for logger "openravepy.ikfast"
Traceback (most recent call last):
  File "tridof_test.py", line 50, in <module>
    ikmodel.autogenerate()
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/inversekinematics.py", line 624, in autogenerate
    self.generate(iktype=iktype,freejoints=freejoints,precision=precision,forceikbuild=forceikbuild,outputlang=outputlang,ipython=ipython)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/inversekinematics.py", line 846, in generate
    chaintree = solver.generateIkSolver(baselink=baselink,eelink=eelink,freeindices=self.freeindices,solvefn=solvefn)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 1836, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/inversekinematics.py", line 678, in solveFullIK_TranslationDirection5D
    return self.ikfast.IKFastSolver.solveFullIK_TranslationDirection5D(*args,**kwargs)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2239, in solveFullIK_TranslationDirection5D
    raise self.CannotSolveError('raghavan roth equations too complex')
openravepy.ikfast.CannotSolveError: 'raghavan roth equations too complex'
[qtcoinrave.cpp:80] SoQt releasing all memory
Coin debug: socontexthandler_cleanup(): 35 context-bound resources not free'd before exit.


Here is the updated XML file:
<Environment>

<Robot name="5DOFRobot">

 <KinBody>
   
   <Body name="lower_base" type="dynamic">
     <Translation>0 0 0</Translation>
     <Geom type="box">
       <Translation>0 0 0.0125</Translation>
       <Extents>0.1 0.09 0.0125</Extents>
     </Geom>
   </Body>


   
   <Body name="upper_base" type="dynamic">
     <offsetfrom>lower_base</offsetfrom>
     <Translation>0 0 0.025</Translation>
     <Geom type="box">
       <Translation>0 0 0.0475</Translation>
       <Extents>0.075 0.039 0.0475</Extents>
     </Geom>
   </Body>

   
   <Body name="upper_arm" type="dynamic">
     <offsetfrom>upper_base</offsetfrom>
     <Translation>0 0 0.095</Translation> 
     <Geom type="box">
       <Translation>0 0 0.1485</Translation>
       <Extents>0.034 0.034 0.1485</Extents>
     </Geom>
   </Body>


   
   <Body name="forearm" type="dynamic">
     <offsetfrom>upper_arm</offsetfrom>
     <Translation>0.068 0 0.229</Translation>
     <Geom type="box">
       <Translation>0 0 0.1485</Translation>
       <Extents>0.034 0.034 0.1485</Extents>
     </Geom>
   </Body>

   
   <Body name="wrist" type="dynamic">
     <offsetfrom>forearm</offsetfrom>
     <Translation>-0.068 0 0.229</Translation> 
     <Geom type="box">
       <Translation>0 0 0.034</Translation>
       <Extents>0.034 0.034 0.034</Extents>
     </Geom>
   </Body>

   
   <Body name="hand" type="dynamic">
     <offsetfrom>wrist</offsetfrom>
     <Translation>0 0 0.068</Translation>
     <Geom type="box">
       <Translation>0 0 0.0035</Translation>
       <Extents>0.01 0.01 0.0035</Extents>
       <diffuseColor>0.05 0.05 0.05</diffuseColor>
     </Geom>
   </Body>

   
   <Joint circular="true" name="base_joint" type="hinge">
     <Body>lower_base</Body>
     <Body>upper_base</Body>
     <offsetfrom>lower_base</offsetfrom>
     <Anchor>0 0 0.025</Anchor>
     <limitsdeg>-180 180</limitsdeg>
     <axis>0 0 1</axis>
     <weight>4</weight>
     <maxvel>5</maxvel>
     <resolution>1</resolution>
   </Joint>

   
   <Joint circular="false" name="shoulder" type="hinge">
     <Body>upper_base</Body>
     <Body>upper_arm</Body>
     <offsetfrom>upper_base</offsetfrom>
     <Anchor>0 0 0.095</Anchor>
     <limitsdeg>-90 90</limitsdeg>
     <axis>1 0 0</axis>
     <maxvel>1000</maxvel> 
     <maxtorque>1000</maxtorque> 
     <weight>4</weight>
     <resolution>1</resolution>
   </Joint>
   
   
   <Joint circular="false" name="elbow" type="hinge">
     <Body>upper_arm</Body>
     <Body>forearm</Body>
     <offsetfrom>upper_arm</offsetfrom>
     <Anchor>0.034 0 0.263</Anchor>
     <limitsdeg>-180 180</limitsdeg>
     <axis>1 0 0</axis>
     <maxvel>1000</maxvel> 
     <maxtorque>1000</maxtorque>
     <weight>4</weight>
     <resolution>1</resolution>
   </Joint>

   
   <Joint circular="false" name="wrist1" type="hinge">
     <Body>forearm</Body>
     <Body>wrist</Body>
     <offsetfrom>forearm</offsetfrom>
     <Anchor>-0.034 0 0.263</Anchor>
     <limitsdeg>-180 180</limitsdeg>
     <axis>1 0 0</axis>
     <maxvel>1000</maxvel> 
     <maxtorque>1000</maxtorque> 
     <weight>4</weight>
     <resolution>1</resolution>
   </Joint>


   
   <Joint circular="true" name="wrist2" type="hinge">
     <Body>wrist</Body>
     <Body>hand</Body>
     <offsetfrom>wrist</offsetfrom>
     <Anchor>0 0 0.068</Anchor>
     <limitsdeg>-180 180</limitsdeg>
     <axis>0 0 1</axis>
     <maxvel>1000</maxvel> 
     <maxtorque>1000</maxtorque> 
     <weight>4</weight>
     <resolution>1</resolution>
   </Joint>

   <transparency>0.1</transparency>
 </KinBody>
 
 <Manipulator name="arm">
   <effector>hand</effector>   
   <base>lower_base</base>     
    <Translation>0 0 0.007</Translation>
 </Manipulator>

</Robot>
</Environment>

Thanks!
Lance