Quantcast

Problem generating translationdirection5d ikfast for Kuka Youbot

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

Problem generating translationdirection5d ikfast for Kuka Youbot

Rasmus
Hi,
 
I'm trying to generate a Custum MoveIT IKFast plugin for a 5DoF Kuka youbot. I'm following the guide in [1], with the addition of changing the iktype to translationdirection5d (as suggested in [2] and [3]). Therefore i use the following command:

python `openrave0.8-config --python-dir`/openravepy/_openravepy_0_8/ikfast.py --robot=youbot_ed.rounded.dae --iktype=translationdirection5d --baselink=2 --eelink=8 --savefile=ikfast61_back_arm.cpp

based on this info on the links:

name                        index parents              
---------------------------------------------------------
base_footprint              0                          
base_link                   1     base_footprint        
back_arm_link_0             2     base_link            
back_arm_link_1             3     back_arm_link_0      
back_arm_link_2             4     back_arm_link_1      
back_arm_link_3             5     back_arm_link_2      
back_arm_link_4             6     back_arm_link_3      
back_arm_link_5             7     back_arm_link_4      
gripper_back_palm_link      8     back_arm_link_5      
gripper_back_finger_link_l  9     gripper_back_palm_link
gripper_back_finger_link_r  10    gripper_back_palm_link
base_laser_front_link       11    base_link            
caster_link_bl              12    base_link            
wheel_link_bl               13    caster_link_bl        
caster_link_br              14    base_link            
wheel_link_br               15    caster_link_br        
caster_link_fl              16    base_link            
wheel_link_fl               17    caster_link_fl        
caster_link_fr              18    base_link            
wheel_link_fr               19    caster_link_fr        
front_arm_link_0            20    base_link            
front_arm_link_1            21    front_arm_link_0      
front_arm_link_2            22    front_arm_link_1      
front_arm_link_3            23    front_arm_link_2      
front_arm_link_4            24    front_arm_link_3      
front_arm_link_5            25    front_arm_link_4      
gripper_front_palm_link     26    front_arm_link_5      
gripper_front_finger_link_l 27    gripper_front_palm_link
gripper_front_finger_link_r 28    gripper_front_palm_link
---------------------------------------------------------

 
 
The problem is that I cant successfully generate the .cpp files even though i changed the iktype to translationdirection5d.

The output from the console is:

$ python `openrave0.8-config --python-dir`/openravepy/_openravepy_0_8/ikfast.py --robot=youbot_ed.rounded.dae --iktype=translationdirection5d --baselink=2 --eelink=8 --savefile=ikfast61_back_arm.cpp

/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py:1122: RuntimeWarning: invalid value encountered in divide
  axisangle /= angle
INFO: moved translation [0, 0, -13/100] to right end
INFO: moved translation [0, 0, -19/1000] to left end
INFO: moved translation on intersecting axis [0, 0, 0] to left
INFO: [[1, 0, 0, 3/125],[0, -1, 0, 0],[0, 0, -1, 23/200]]
INFO: [[cos(j0), -sin(j0), 0, 0],[sin(j0), cos(j0), 0, 0],[0, 0, 1, 0]]
INFO: [[-98481*10000050586**(1/2)/10000050586, 0, -17365*10000050586**(1/2)/10000050586, -3249873*10000050586**(1/2)/10000050586000],[-17365*10000050586**(1/2)/10000050586, 0, 98481*10000050586**(1/2)/10000050586, -114609*10000050586**(1/2)/2000010117200],[0, 1, 0, 0]]
INFO: [[cos(j1), -sin(j1), 0, 0],[sin(j1), cos(j1), 0, 0],[0, 0, 1, 0]]
INFO: [[55169*17040874661**(1/2)/17040874661, 118310*17040874661**(1/2)/17040874661, 0, -366761*17040874661**(1/2)/340817493220],[-118310*17040874661**(1/2)/17040874661, 55169*17040874661**(1/2)/17040874661, 0, -1710239*17040874661**(1/2)/3408174932200],[0, 0, 1, 0]]
INFO: [[cos(j2), -sin(j2), 0, 0],[sin(j2), cos(j2), 0, 0],[0, 0, 1, 0]]
INFO: [[-5000000000*36373964057654194481**(1/2)/36373964057654194481, -3372530809*36373964057654194481**(1/2)/36373964057654194481, 0, 91058331843*36373964057654194481**(1/2)/7274792811530838896200],[3372530809*36373964057654194481**(1/2)/36373964057654194481, -5000000000*36373964057654194481**(1/2)/36373964057654194481, 0, 675000000*36373964057654194481**(1/2)/36373964057654194481],[0, 0, 1, 0]]
INFO: [[cos(j3), -sin(j3), 0, 0],[sin(j3), cos(j3), 0, 0],[0, 0, 1, 0]]
INFO: [[-10822*2500019909**(1/2)/2500019909, 0, 48815*2500019909**(1/2)/2500019909, 5411*2500019909**(1/2)/625004977250],[-48815*2500019909**(1/2)/2500019909, 0, -10822*2500019909**(1/2)/2500019909, 9763*2500019909**(1/2)/250001990900],[0, -1, 0, 0]]
INFO: [[cos(j4), -sin(j4), 0, 0],[sin(j4), cos(j4), 0, 0],[0, 0, 1, 0]]
INFO: [[-48815*2500019909**(1/2)/2500019909, -10822*2500019909**(1/2)/2500019909, 0, 0],[-10822*2500019909**(1/2)/2500019909, 48815*2500019909**(1/2)/2500019909, 0, 0],[0, 0, -1, -13/100]]
INFO: ikfast translation direction 5d: [j0, j1, j2, j3, j4]
INFO: attempting li/woernle/hiller general ik method
WARNING: 'Kohli/Osvatic method requires 3 unknown variables, has 2'
INFO: attempting li/woernle/hiller general ik method
WARNING: 'need exactly 8 equations of one variable'
INFO: attempting kohli/osvatic general ik method
WARNING: 'Kohli/Osvatic method requires 3 unknown variables'
INFO: attempting kohli/osvatic general ik method
WARNING: 'need 8 or less equations of one variable'
INFO: attempting manocha/canny general ik method
INFO: solving for all pairwise variables in (cj2, sj2, cj3, sj3), number of symbol coeffs are 0
INFO: finished with 9 equations
WARNING: 'number of equations 1 less than 6'
INFO: attempting manocha/canny general ik method
INFO: solving for all pairwise variables in (cj3, sj3), number of symbol coeffs are 0
INFO: finished with 6 equations
INFO: solving simultaneously for symbols: (cj0, sj0, cj1, sj1, cj2, sj2)
WARNING: failed with leftvar htj0: 'solveDialytically: more unknowns than equations 18>14'
WARNING: solveDialytically equations 14 > 12, should be equal...
WARNING: failed with leftvar htj1: 'equations are not linearly independent'
WARNING: failed with leftvar htj2: 'solveDialytically: more unknowns than equations 20>14'
WARNING: 'failed to solve dialytically'
Traceback (most recent call last):
  File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py", line 6121, in <module>
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py", line 1639, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py", line 2040, in solveFullIK_TranslationDirection5D
    raise self.CannotSolveError('raghavan roth equations too complex')
__main__.CannotSolveError: 'raghavan roth equations too complex'



Please advice on how to procede!

Kind regards,
Rasmus


[1]: http://moveit.ros.org/wiki/Kinematics/IKFast
[2]: https://groups.google.com/forum/#!msg/moveit-users/P2V9eW5BjW8/eDr9nCeRg3AJ
[3]: http://answers.ros.org/question/65940/difficulty-using-ikfast-generator-need-6-joints-error-with-kuka-youbot/
Loading...