Ikfast fails to generate solution file for 3dof arm using .dae(genereated from .wrl)

Question

Rosanswers logo

Hi all

I've tried to use ikfast module(iktype=translation3d) for 3DoF simple arm. I have been following the instructions here: http://moveit.ros.org/wiki/Kinematics/IKFast and http://answers.ros.org/question/205040/what-is-the-correct-ik-solver-type-for-a-3dof-leg/

My openrave-version is 0.9.0 on ubuntu 12.04.

First, I generated .dae from .wrl. Here is terminal output.

export-collada  -i GR001d_mod0810.wrl -o test0810.dae
omniORB: Warning: the local loop back interface (127.0.0.1)
is the only address available for this server.
Humanoid node
Joint nodeCHEST_P
  Segment node CHEST_P_LINK
  Joint nodeR_SHOULDER_P
    Segment node R_SHOULDER_P_LINK
    Joint nodeR_SHOULDER_R
      Segment node R_SHOULDER_R_LINK
      Joint nodeR_ELBOW_P
        Segment node R_ELBOW_P_LINK
Collada Info: init COLLADA writer version: 1.5.0, namespace: http://www.collada.org/2008/03/COLLADASchema
export-collada GR001d_mod0810.wrl was successfully exported to test0810.dae

Following is the joints and links information of the .dae.

~/openrave-0.9.0-src/python$ openrave-robot.py test0810.dae --info links
name              index parents          
-----------------------------------------
CHEST_P_LINK      0                      
R_SHOULDER_P_LINK 1     CHEST_P_LINK     
R_SHOULDER_R_LINK 2     R_SHOULDER_P_LINK
R_ELBOW_P_LINK    3     R_SHOULDER_R_LINK
-----------------------------------------
name              index parents
        
~/openrave-0.9.0-src/python$ openrave-robot.py test0810.dae --info joints
name         joint_index dof_index parent_link       child_link        mimic
----------------------------------------------------------------------------
R_SHOULDER_P 0           0         CHEST_P_LINK      R_SHOULDER_P_LINK      
R_SHOULDER_R 1           1         R_SHOULDER_P_LINK R_SHOULDER_R_LINK      
R_ELBOW_P    2           2         R_SHOULDER_R_LINK R_ELBOW_P_LINK         
----------------------------------------------------------------------------
name         joint_index dof_index parent_link       child_link        mimic

Next, I've tried to generate solution file using this .dae file.

However, terminal output 'failed to find a variable to solve'.

python ikfast.py --robot=test0810.dae --baselink=0 --eelink=3 --savefile=iktest.cpp --iktype=translation3d

~/openrave-0.9.0-src/python$ python ikfast.py --robot=test0810.dae --baselink=0 --eelink=3 --savefile=iktest.cpp --iktype=translation3d
[colladareader.cpp:3240] Target Node 'visual1/node_joint0_axis0' not found
[colladareader.cpp:3560] failed to find joint target 'kmodel1/jointsid1000' in _mapJointSids
[colladareader.cpp:3565] could not find openrave joint 'CHEST_P'!
[colladareader.cpp:2618] failed to find joint kmodel1/jointsid1000 in actuator CHEST_P
INFO: moved translation [0, 0, 1/100000] to right end
INFO: moved translation [0, 0, 1/40] to left end
INFO: [[1, 0, 0, -1/100],[0, 0, -1, -33/500],[0, 1, 0, 21/500]]
INFO: [[cos(j0), -sin(j0), 0, 0],[sin(j0), cos(j0), 0, 0],[0, 0, 1, 0]]
INFO: [[0, 0, 1, 1/100000],[-1, 0, 0, -1/125],[0, -1, 0, 0]]
INFO: [[cos(j1), -sin(j1), 0, 0],[sin(j1), cos(j1), 0, 0],[0, 0, 1, 0]]
INFO: [[0, 1, 0, 47/1000],[0, 0, 1, 0],[1, 0, 0, 9/1000]]
INFO: [[cos(j2), -sin(j2), 0, 0],[sin(j2), cos(j2), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 0],[0, 0, -1, 0],[0, 1, 0, 1/100000]]
INFO: ikfast translation3d: [j0, j1, j2]
INFO: [] [j0, j1, j2]
INFO: j0 solution: equations used for atan2: [-px*cos(j0) - py*sin(j0) + 901/100000, -pp + 2*px*sin(j0)/125 - 2*py*cos(j0)/125 + 11130901/5000000000]
INFO: [j1] [j0, j2]
INFO: j0 solution: equations used for atan2: [-px*cos(j0) - py*sin(j0) + 901/100000, -pp + 2*px*sin(j0)/125 - 2*py*cos(j0)/125 + 11130901/5000000000, 47*cj1/1000 - px*sin(j0) + py*cos(j0) - sj1/100000 + 1/125, -cj1*px*sin(j0) + cj1*py*cos(j0) + cj1/125 + pz*sj1 + 47/1000, cj1*pz + px*sj1*sin(j0) - py*sj1*cos(j0) - sj1/125 + 1/100000]
INFO: j0 solution: equations used for atan2: [-px*cos(j0) - py*sin(j0) + 901/100000, -pp + 2*px*sin(j0)/125 - 2*py*cos(j0)/125 + 11130901/5000000000, 47*cj1/1000 - px*sin(j0) + py*cos(j0) - sj1/100000 + 1/125, -cj1*px*sin(j0) + cj1*py*cos(j0) + cj1/125 + pz*sj1 + 47/1000, cj1*pz + px*sj1*sin(j0) - py*sj1*cos(j0) - sj1/125 + 1/100000]
INFO: [j1, j0] [j2]
Traceback (most recent call last):
  File "ikfast.py", line 6265, in <module>
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "ikfast.py", line 1638, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "ikfast.py", line 1815, in solveFullIK_Translation3D
    return self._solveFullIK_Translation3D(LinksRaw,jointvars,isolvejointvars,basepos)
  File "ikfast.py", line 1835, in _solveFullIK_Translation3D
    transtree = self.solveAllEquations(AllEquations,curvars=solvejointvars[:],othersolvedvars=self.freejointvars,solsubs = self.freevarsubs[:],endbranchtree=endbranchtree)
  File "ikfast.py", line 4271, in solveAllEquations
    return self.addSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases)
  File "ikfast.py", line 4571, in addSolution
    nextsolutions[var] = self.solveAllEquations(AllEquations,curvars=newvars,othersolvedvars=othersolvedvars+[var],solsubs=solsubs+self.Variable(var).subs,endbranchtree=endbranchtree,currentcases=currentcases)
  File "ikfast.py", line 4271, in solveAllEquations
    return self.addSolution(solutions,AllEquations,curvars,othersolvedvars,solsubs,endbranchtree,currentcases=currentcases)
  File "ikfast.py", line 4571, in addSolution
    nextsolutions[var] = self.solveAllEquations(AllEquations,curvars=newvars,othersolvedvars=othersolvedvars+[var],solsubs=solsubs+self.Variable(var).subs,endbranchtree=endbranchtree,currentcases=currentcases)
  File "ikfast.py", line 4380, in solveAllEquations
    raise self.CannotSolveError('failed to find a variable to solve')
__main__.CannotSolveError: 'failed to find a variable to solve'

But, when I use following command as the end and the base link is reversed,

python ikfast.py --robot=test0810.dae --baselink=3 --eelink=0 --savefile=iktest.cpp --iktype=translation3d

IKfast succeeded to gererate the solution file.

~/openrave-0.9.0-src/python$ python ikfast.py --robot=test0810.dae --baselink=3 --eelink=0 --savefile=iktest.cpp --iktype=translation3d
[colladareader.cpp:3240] Target Node 'visual1/node_joint0_axis0' not found
[colladareader.cpp:3560] failed to find joint target 'kmodel1/jointsid1000' in _mapJointSids
[colladareader.cpp:3565] could not find openrave joint 'CHEST_P'!
[colladareader.cpp:2618] failed to find joint kmodel1/jointsid1000 in actuator CHEST_P
INFO: moved translation [0, 0, -1/40] to right end
INFO: moved translation [0, 0, -1/100000] to left end
INFO: [[1, 0, 0, 0],[0, 0, 1, -1/100000],[0, -1, 0, 0]]
INFO: [[cos(j2), sin(j2), 0, 0],[-sin(j2), cos(j2), 0, 0],[0, 0, 1, 0]]
INFO: [[0, 0, 1, -9/1000],[1, 0, 0, -47/1000],[0, 1, 0, 0]]
INFO: [[cos(j1), sin(j1), 0, 0],[-sin(j1), cos(j1), 0, 0],[0, 0, 1, 0]]
INFO: [[0, -1, 0, -1/125],[0, 0, -1, 0],[1, 0, 0, -1/100000]]
INFO: [[cos(j0), sin(j0), 0, 0],[-sin(j0), cos(j0), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 1/100],[0, 0, 1, -21/500],[0, -1, 0, -33/500]]
INFO: ikfast translation3d: [j2, j1, j0]
INFO: [] [j2, j1, j0]
INFO: multiplying all determinant coefficients for solving htj2
INFO: [j2] [j1, j0]
INFO: j1 solution: equations used for atan2: [cj2*py*sin(j1) + px*sj2*sin(j1) + pz*cos(j1) + 47*sin(j1)/1000 - 33/500, -901*cj2*px/50000 - 2*cj2*py*cos(j1)/125 - 47*cj2*py/500 - pp - 2*px*sj2*cos(j1)/125 - 47*px*sj2/500 + 901*py*sj2/50000 + 2*pz*sin(j1)/125 - 47*cos(j1)/62500 + 38658199/10000000000]
INFO: j0 solution: equations used for atan2: [-cj2*px + py*sj2 - 21*sin(j0)/500 + cos(j0)/100 - 901/100000, -901*cj2*px/50000 - 47*cj2*py/500 - pp - 47*px*sj2/500 + 901*py*sj2/50000 - sin(j0)/6250 - 21*cos(j0)/31250 + 39938199/10000000000]
INFO: [j2, j0] [j1]
INFO: j1 solution: equations used for atan2: [cj2*py*sin(j1) + px*sj2*sin(j1) + pz*cos(j1) + 47*sin(j1)/1000 - 33/500, -21*cj0*sin(j1)/500 - pz - sj0*sin(j1)/100 + sin(j1)/125 + 33*cos(j1)/500, -21*cj0/500 + cj2*py*cos(j1) + px*sj2*cos(j1) - pz*sin(j1) - sj0/100 + 47*cos(j1)/1000 + 1/125, 21*cj0*cos(j1)/500 - cj2*py - px*sj2 + sj0*cos(j1)/100 + 33*sin(j1)/500 - cos(j1)/125 - 47/1000, -901*cj2*px/50000 - 2*cj2*py*cos(j1)/125 - 47*cj2*py/500 - pp - 2*px*sj2*cos(j1)/125 - 47*px*sj2/500 + 901*py*sj2/50000 + 2*pz*sin(j1)/125 - 47*cos(j1)/62500 + 38658199/10000000000]
INFO: j1 solution: equations used for atan2: [cj2*py*sin(j1) + px*sj2*sin(j1) + pz*cos(j1) + 47*sin(j1)/1000 - 33/500, -21*cj0*sin(j1)/500 - pz - sj0*sin(j1)/100 + sin(j1)/125 + 33*cos(j1)/500, -21*cj0/500 + cj2*py*cos(j1) + px*sj2*cos(j1) - pz*sin(j1) - sj0/100 + 47*cos(j1)/1000 + 1/125, 21*cj0*cos(j1)/500 - cj2*py - px*sj2 + sj0*cos(j1)/100 + 33*sin(j1)/500 - cos(j1)/125 - 47/1000, -901*cj2*px/50000 - 2*cj2*py*cos(j1)/125 - 47*cj2*py/500 - pp - 2*px*sj2*cos(j1)/125 - 47*px*sj2/500 + 901*py*sj2/50000 + 2*pz*sin(j1)/125 - 47*cos(j1)/62500 + 38658199/10000000000]
INFO: generating cpp code...

Additionaly, this .dae succeeded to generate solution file by following command,

python ikfast.py --robot=test0810.dae --baselink=1 --eelink=3  --savefile=iktest.cpp --iktype=lookat3d
python ikfast.py --robot=test0810.dae --baselink=0 --eelink=3 --freeindex=0  --savefile=iktest.cpp --iktype=lookat3d
python ikfast.py --robot=test0810.dae --baselink=3 --eelink=0 --freeindex=2  --savefile=iktest.cpp --iktype=lookat3d

and, this command failed to generate solution file.

python ikfast.py --robot=test0810.dae --baselink=0 --eelink=3 --freeindex=2 --savefile=iktest.cpp --iktype=lookat3d
python ikfast.py --robot=test0810.dae --baselink=3 --eelink=0 --freeindex=0  --savefile=iktest.cpp --iktype=lookat3d

Does anyone know what the problem might be? Many thanks.

Naohiro.

Originally posted by Naohiro on ROS Answers with karma: 16 on 2015-08-10

Post score: 0

NaohiroNaohiro · Accepted Answer · 2015-08-10 06:14:37Z

Rosanswers logo

Aditionaly, a part of definitation codes of arm link in .wrl is following.

DEF GR001 Humanoid{
  humanoidBody [
    DEF CHEST_P Joint {
     jointType "fixed"
     translation 0.044 0.0 0.02
     rotation 0 0 1 0
     children[
       DEF CHEST_P_LINK Segment{
         mass 0.02905 
       }
       DEF R_SHOULDER_P Joint {
         jointType "rotate"
         jointAxis 0 1 0
         translation -0.01 -0.041 0.042
        rotation 0 0 1 0
         children[
           DEF R_SHOULDER_P_LINK Segment{
            mass 0.02905 
           }
           DEF R_SHOULDER_R Joint {
               jointType "rotate"
               jointAxis 1 0 0
               translation 0.00001 -0.025 -0.008
             rotation 0 0 1 0
               children[
                 DEF R_SHOULDER_R_LINK Segment{
                mass 0.02905 
                 }
                 DEF R_ELBOW_P Joint {
                   jointType "rotate"
                   jointAxis 0 1 0
                   translation 0.009 0.00001 -0.047  # by Hara
                 rotation 0 0 1 0
                   children[
                     DEF R_ELBOW_P_LINK Segment{
                     mass 0.02905 
                     }
                   ]
                 }
               ]
           }
         ]
       }
     ]
    }
  ]
  joints [
   USE CHEST_P,
   USE R_SHOULDER_P,
   USE R_SHOULDER_R,
   USE R_ELBOW_P,
  ]
  segments [
   USE CHEST_P_LINK,
   USE R_SHOULDER_P_LINK,
   USE R_SHOULDER_R_LINK,
   USE R_ELBOW_P_LINK,
  ]
}

Many thanks.

Originally posted by Naohiro with karma: 16 on 2015-08-10

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by Naohiro on 2015-09-10:
I was able to generate ik solution file using the openrave master branch on github. https://github.com/rdiankov/openrave

Thank you

Stack Exchange Network

Ikfast fails to generate solution file for 3dof arm using .dae(genereated from .wrl)

1 Answer 1

Original comments

Your Answer

Hot Network Questions

Ikfast fails to generate solution file for 3dof arm using .dae(genereated from .wrl)

1 Answer 1

Original comments

Your Answer

Sign up or log in

Post as a guest

Related

Hot Network Questions