OpenRAVE Users List

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

classic Classic list List threaded Threaded
♦
♦
 3 messages Options
Naohiro Hayashi
Reply | Threaded
Open this post in threaded view
♦
♦
|

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

Naohiro Hayashi
Hi all

I've tried to use ikfast module(iktype=translation3d) for 3DoF simple arm.
I have been following the instructions here:

My openrave-version is 0.9.0.

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.





------------------------------------------------------------------------------

_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users

test0810.dae (23K) Download Attachment
GR001d_mod0810.wrl (8K) Download Attachment
Rosen Diankov
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Ikfast fails to generate solution file for 3dofarm using .dae(genereated from .wrl)

Rosen Diankov
Administrator
just tried ikfast from openrave master branch, and it works(!)

2015-08-10 19:50 GMT+09:00 林 直宏 <[hidden email]>:
Hi all

I've tried to use ikfast module(iktype=translation3d) for 3DoF simple arm.
I have been following the instructions here:

My openrave-version is 0.9.0.

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.





------------------------------------------------------------------------------

_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users



------------------------------------------------------------------------------

_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users
Naohiro Hayashi
Reply | Threaded
Open this post in threaded view
♦
♦
|

Re: Ikfast fails to generate solution file for 3dofarm using .dae(genereated from .wrl)

Naohiro Hayashi
Hi Rosen,

I was able to generate ik solution file using the openrave master branch.

Thank you for your information!

Naohiro

On 2015/08/10 23:19, Rosen Diankov wrote:
just tried ikfast from openrave master branch, and it works(!)

2015-08-10 19:50 GMT+09:00 林 直宏 <[hidden email]>:
Hi all

I've tried to use ikfast module(iktype=translation3d) for 3DoF simple arm.
I have been following the instructions here:

My openrave-version is 0.9.0.

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.





------------------------------------------------------------------------------

_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users




------------------------------------------------------------------------------



_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users


------------------------------------------------------------------------------

_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users
Free forum by Nabble Edit this page