Quantcast

Ik fast moveIt transformation6D

classic Classic list List threaded Threaded
3 messages Options
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Ik fast moveIt transformation6D

Chickenman
 Hi

I am creating Ikfast plugin for my robotic manupulator. I am following this tutorial http://docs.ros.org/indigo/api/moveit_ikfast/html/doc/ikfast_tutorial.html . Here is my moveit demo launch everything should be  correct.



KDL work fine but I needed to setup smaller resolutions and I am not satisfied with it
I used collada to change urdf to dae file. Collada generated dae file without any errors or warnings but wen I openrave name.dae
The model looks horrible



But it was not problem for me because i wanted just IK c++ file created so I typed
openrave-robot.py kv01.dae --info links
and it wrote correctly

base_link       0                  
    wheel_frame_l   1     base_link    
    wheel_lp        2     wheel_frame_l
    wheel_lz        3     wheel_frame_l
    wheel_frame_r   4     base_link    
    wheel_pp        5     wheel_frame_r
    wheel_pz        6     wheel_frame_r
    scan            7     base_link    
    veza            8     scan         
    camera_l        9     veza         
    camera_r        10    veza         
    camera          11    camera_r     
    model_klb1      12    base_link    
    model_klb2      13    model_klb1   
    model_klb3      14    model_klb2   
    model_klb4      15    model_klb3   
    model_klb5      16    model_klb4   
    model_klb6      17    model_klb5   
    uchopenie_lavo  18    model_klb6   
    uchopenie_pravo 19    model_klb6   
    menic5_6        20    model_klb5   
    link_4_5        21    model_klb4   
    link_4_5x       22    model_klb4   
    link_3_4        23    model_klb3   
    link_2_3        24    model_klb2   
    menic2_3        25    model_klb2   

The informations are correct and my arm is made from Baselink -> model_klb1- model_klb6
so I typed to create cpp file:

python `open-rave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=kv01.dae --iktype=transform6d --baselink=0 --eelink=17 --savefile=ikfast64_kv01.cpp

I got this error

  [plugindatabase.h:929 _SysLoadLibrary] /usr/local/share/openrave-0.9/plugins/openravepy_configurationcache.so: libconfigurationcache.so: cannot open shared object file: No such file or directory
    [plugindatabase.h:857 _LoadPlugin] failed to load: /usr/local/share/openrave-0.9/plugins/openravepy_configurationcache.so
    [colladareader.cpp:2130 FillGeometryColor] transparecy is 1.000000, which means the item will be rendered invisible, this must be a mistake so setting to opaque (1)
    [colladareader.cpp:2130 FillGeometryColor] transparecy is 1.000000, which means the item will be rendered invisible, this must be a mistake so setting to opaque (1)
    [colladareader.cpp:2130 FillGeometryColor] transparecy is 1.000000, which means the item will be rendered invisible, this must be a mistake so setting to opaque (1)
    /usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py:1437: RuntimeWarning: invalid value encountered in divide
      axisangle /= angle
    INFO: moved translation Matrix([[0, 0, 19/100]]) to right end
    INFO: moved translation Matrix([[0, 0, -129/500]]) to left end
    INFO: moved translation on intersecting axis Matrix([[0, 0, -31/200]]) to left
    INFO: [[1, 0, 0, 0],[0, -1, 0, 0],[0, 0, -1, 169/500]]
    INFO: [[cos(j6), -sin(j6), 0, 0],[sin(j6), cos(j6), 0, 0],[0, 0, 1, 0]]
    INFO: [[1, 0, 0, 0],[0, 0, -1, 0],[0, 1, 0, 0]]
    INFO: [[cos(j7), -sin(j7), 0, 0],[sin(j7), cos(j7), 0, 0],[0, 0, 1, 0]]
    INFO: [[1, 0, 0, 0],[0, -1, 0, -97/200],[0, 0, -1, 0]]
    INFO: [[cos(j8), -sin(j8), 0, 0],[sin(j8), cos(j8), 0, 0],[0, 0, 1, 0]]
    INFO: [[1, 0, 0, 0],[0, 0, -1, 101/200],[0, 1, 0, -1/25]]
    INFO: [[cos(j9), -sin(j9), 0, 0],[sin(j9), cos(j9), 0, 0],[0, 0, 1, 0]]
    INFO: [[1, 0, 0, 0],[0, 0, 1, -9/250],[0, -1, 0, 0]]
    INFO: [[cos(j10), -sin(j10), 0, 0],[sin(j10), cos(j10), 0, 0],[0, 0, 1, 0]]
    INFO: [[1, 0, 0, 0],[0, 0, 1, 0],[0, -1, 0, 1/40]]
    INFO: [[cos(j11), -sin(j11), 0, 0],[sin(j11), cos(j11), 0, 0],[0, 0, 1, 0]]
    INFO: [[1, 0, 0, 0],[0, 1, 0, 0],[0, 0, 1, 19/100]]
    INFO: ikfast 6d: [j6, j7, j8, j9, j10, j11]
    INFO: found 3 consecutive non-intersecting axes links[2:7], vars=[j7, j8, j9]
    INFO: found 3 consecutive non-intersecting axes links[4:9], vars=[j8, j9, j10]
    INFO: found 3 consecutive non-intersecting axes links[0:5], vars=[j6, j7, j8]
    INFO: found 3 consecutive non-intersecting axes links[6:11], vars=[j9, j10, j11]
    INFO: try group 0/4
    INFO: computed in 1.605267s
    INFO: computed in 1.254980s
    INFO: attempting li/woernle/hiller general ik method
    INFO: allowed indices: [0]
    INFO: assuming equation htj7 is non-zero
    INFO: not unique: (Poly(-cj9, cj8, sj8, cj9, sj9, htj7, domain='ZZ'), Poly(r11*cj6*cj11 + r10*cj6*sj11 - r01*sj6*cj11 - r00*sj6*sj11, cj6, sj6, cj11, sj11, htj7, domain='ZZ[r00,r01,r10,r11]'))
    INFO: assuming equation htj7 is non-zero
    INFO: factors 1 reduced not unique: -11*cj11*cj6*r11/1000 + 11*cj11*r01*sj6/1000 + cj6*py - 11*cj6*r10*sj11/1000 - px*sj6 + 11*r00*sj11*sj6/1000 + 1/25
    INFO: assuming equation htj7 is non-zero
    INFO: assuming equation htj7 is non-zero
    INFO: factors 1 reduced not unique: -cj10*cj6*r12 + cj10*r02*sj6 - cj11*cj6*r10*sj10 + cj11*r00*sj10*sj6 + cj6*r11*sj10*sj11 - r01*sj10*sj11*sj6
    INFO: assuming equation htj7 is non-zero
    INFO: not unique: (Poly(-sj9, cj8, sj8, cj9, sj9, htj7, domain='ZZ'), Poly(-cj10*r10*cj6*cj11 + cj10*r11*cj6*sj11 + r12*sj10*cj6 + cj10*r00*sj6*cj11 - cj10*r01*sj6*sj11 - r02*sj10*sj6, cj6, sj6, cj11, sj11, htj7, domain='ZZ[r00,r01,r02,r10,r11,r12,cj10,sj10]'))
    INFO: factors 0 reduced not unique: 0
    INFO: factors 1 reduced not unique: -11*cj11*cj6*r11/1000 + 11*cj11*r01*sj6/1000 + cj6*py - 11*cj6*r10*sj11/1000 - px*sj6 + 11*r00*sj11*sj6/1000 + 1/25
    INFO: assuming equation htj7 is non-zero
    INFO: factors 1 reduced not unique: -11*cj11*cj6*r11/1000 + 11*cj11*r01*sj6/1000 + cj6*py - 11*cj6*r10*sj11/1000 - px*sj6 + 11*r00*sj11*sj6/1000 + 1/25
    INFO: assuming equation htj7 is non-zero
    INFO: not unique: (Poly(-97/200*cj8*sj9, cj8, sj8, cj9, sj9, htj7, domain='QQ'), Poly((101*cj10*r10/200 - rxp1_1)*cj6*cj11 + (-101*cj10*r11/200 - rxp0_1)*cj6*sj11 - 101*r12*sj10/200*cj6 + (-101*cj10*r00/200 + rxp1_0)*sj6*cj11 + (101*cj10*r01/200 + rxp0_0)*sj6*sj11 + 101*r02*sj10/200*sj6, cj6, sj6, cj11, sj11, htj7, domain='QQ[r00,r01,r02,r10,r11,r12,cj10,rxp0_0,rxp0_1,rxp1_0,rxp1_1,sj10]'))
    INFO: assuming equation htj7 is non-zero
    INFO: not unique: (Poly(950309/160000*cj8, cj8, sj8, cj9, sj9, htj7, domain='QQ'), Poly(-1067*npy/4000*cj11 - 1067*npx/4000*sj11 + 97*pp/8 - 47697713/8000000, cj6, sj6, cj11, sj11, htj7, domain='QQ[npx,npy,pp]'))
    INFO: assuming equation htj7 is non-zero
    INFO: not unique: (Poly(-97/8*sj8*sj9, cj8, sj8, cj9, sj9, htj7, domain='QQ'), Poly(-r11*cj6*cj11 - r10*cj6*sj11 + r01*sj6*cj11 + r00*sj6*sj11 - 25*npy*cj11 - 25*npx*sj11 + 11/40, cj6, sj6, cj11, sj11, htj7, domain='QQ[r00,r01,r10,r11,npx,npy]'))
    INFO: assuming equation htj7 is non-zero
    INFO: not unique: (Poly(-950309/160000*cj8*cj9, cj8, sj8, cj9, sj9, htj7, domain='QQ'), Poly((-97*npy*py/4 + 97*pp*r11/8 - 47644363*r11/8000000)*cj6*cj11 + (-97*npx*py/4 + 97*pp*r10/8 - 47644363*r10/8000000)*cj6*sj11 - 97*py/160*cj6 + (97*npy*px/4 - 97*pp*r01/8 + 47644363*r01/8000000)*sj6*cj11 + (97*npx*px/4 - 97*pp*r00/8 + 47644363*r00/8000000)*sj6*sj11 + 97*px/160*sj6 - 97*npy/100*cj11 - 97*npx/100*sj11 - 97/4000, cj6, sj6, cj11, sj11, htj7, domain='QQ[r00,r01,r10,r11,npx,npy,pp,px,py]'))
    INFO: matrix has 0 symbols
    INFO: skipping dependent index 9
    INFO: skipping dependent index 14
    INFO: computed non-singular AU matrix
    INFO: special structure in equations detected, try to solve through elimination
    INFO: depth=0 c=1, [] [j6]: cases=None
    INFO: CannotSolveError: SolveAllEquations failed to find a variable to solve
    INFO: depth=0 c=2, [] [j11]: cases=None
    INFO: CannotSolveError: SolveAllEquations failed to find a variable to solve
    INFO: try to solve first two variables pairwise
    WARNING: converting polyeq "Poly(0, htj6, htj11, domain='ZZ')" to "Poly(0, htj6, htj11, domain='ZZ')"
    Traceback (most recent call last):
      File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 9206, 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 2257, in generateIkSolver
        chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
      File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2911, 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 3213, in solveFullIK_6DGeneral
        coupledsolutions,usedvars = solvemethod(rawpolyeqs,solvejointvars,endbranchtree=endbranchtree,AllEquationsExtra=AllEquationsExtra)
      File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 5023, in solveLiWoernleHiller
        halfanglesolution = self.SolvePairVariablesHalfAngle(raweqns=raweqns,var0=usedvars[0],var1=usedvars[1],othersolvedvars=self.freejointvars)[0]
      File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 7417, 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

is there problem with my openrave or with dae file. Can anyone please help I cannot find anything about this. I am using ubuntu 14.04 with ros Indigo and I followed this tutorial to install Openrave from source https://scaron.info/teaching/installing-openrave-on-ubuntu-14.04.html

Thank you for your time and help.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Ik fast moveIt transformation6D

Chickenman
Update: I tried updated python-sympy
 
Actual version is Name: sympy
Version: 0.7.6.1
Location: /usr/local/lib/python2.7/dist-packages
 
Now I am getting new error

    Traceback (most recent call last):
      File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 9206, 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 2257, in generateIkSolver
        chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
      File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2911, 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 3213, in solveFullIK_6DGeneral
        coupledsolutions,usedvars = solvemethod(rawpolyeqs,solvejointvars,endbranchtree=endbranchtree,AllEquationsExtra=AllEquationsExtra)
      File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 4667, in solveLiWoernleHiller
        A = zeros((len(neweqs_test),len(allmonoms)))
      File "/usr/local/lib/python2.7/dist-packages/sympy/matrices/dense.py", line 1229, in zeros
        return cls.zeros(r, c)
      File "/usr/local/lib/python2.7/dist-packages/sympy/matrices/dense.py", line 513, in zeros
        r = as_int(r)
      File "/usr/local/lib/python2.7/dist-packages/sympy/core/compatibility.py", line 389, in as_int
        raise ValueError('%s is not an integer' % n)
    TypeError: not all arguments converted during string formatting

And here is generated dae file http://hostcode.sourceforge.net/view/4370
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Ik fast moveIt transformation6D

wangchong
This post has NOT been accepted by the mailing list yet.
In reply to this post by Chickenman
Hi ,do you solve the question? I get same question like you.

Any hints would be appreciated!
Loading...