IK problem about left arm of NAO robot

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

IK problem about left arm of NAO robot

Gausslee
Hi, everybody,

Recently I have the naorobot.dae file and created my own xml file including the manipulator definition. I attached the two files here: https://www.dropbox.com/s/8nh734szmewo26y/IKfastNao.tar.gz

Firstly I use the command openrave-robot.py newrobot.dae --info joints, I get the results:
name           joint_index dof_index parent_link        child_link         mimic
--------------------------------------------------------------------------------
HeadYaw        0           0         TorsoLink          HeadYawLink            
HeadPitch      1           1         HeadYawLink        HeadPitchLink          
LHipYawPitch   2           2         TorsoLink          LHipYawPitchLink        
LHipRoll       3           3         LHipYawPitchLink   LHipRollLink            
LHipPitch      4           4         LHipRollLink       LHipPitchLink          
LKneePitch     5           5         LHipPitchLink      LKneePitchLink          
LAnklePitch    6           6         LKneePitchLink     LAnklePitchLink        
LAnkleRoll     7           7         LAnklePitchLink    LAnkleRollLink          
LShoulderPitch 8           8         TorsoLink          LShoulderPitchLink      
LShoulderRoll  9           9         LShoulderPitchLink LShoulderRollLink      
LElbowYaw      10          10        LShoulderRollLink  LElbowYawLink          
LElbowRoll     11          11        LElbowYawLink      LElbowRollLink          
LWristYaw      12          12        LElbowRollLink     LWristYawLink          
LFinger11      13          13        LWristYawLink      LFinger11Link          
LFinger12      14          14        LFinger11Link      LFinger12Link          
LFinger13      15          15        LFinger12Link      LFinger13Link          
LFinger21      16          16        LWristYawLink      LFinger21Link          
LFinger22      17          17        LFinger21Link      LFinger22Link          
LFinger23      18          18        LFinger22Link      LFinger23Link          
LThumb1        19          19        LWristYawLink      LThumb1Link            
LThumb2        20          20        LThumb1Link        LThumb2Link            
RHipYawPitch   21          21        TorsoLink          RHipYawPitchLink        
RHipRoll       22          22        RHipYawPitchLink   RHipRollLink            
RHipPitch      23          23        RHipRollLink       RHipPitchLink          
RKneePitch     24          24        RHipPitchLink      RKneePitchLink          
RAnklePitch    25          25        RKneePitchLink     RAnklePitchLink        
RAnkleRoll     26          26        RAnklePitchLink    RAnkleRollLink          
RShoulderPitch 27          27        TorsoLink          RShoulderPitchLink      
RShoulderRoll  28          28        RShoulderPitchLink RShoulderRollLink      
RElbowYaw      29          29        RShoulderRollLink  RElbowYawLink          
RElbowRoll     30          30        RElbowYawLink      RElbowRollLink          
RWristYaw      31          31        RElbowRollLink     RWristYawLink          
RFinger11      32          32        RWristYawLink      RFinger11Link          
RFinger12      33          33        RFinger11Link      RFinger12Link          
RFinger13      34          34        RFinger12Link      RFinger13Link          
RFinger21      35          35        RWristYawLink      RFinger21Link          
RFinger22      36          36        RFinger21Link      RFinger22Link          
RFinger23      37          37        RFinger22Link      RFinger23Link          
RThumb1        38          38        RWristYawLink      RThumb1Link            
RThumb2        39          39        RThumb1Link        RThumb2Link            
--------------------------------------------------------------------------------
name           joint_index dof_index parent_link        child_link         mimic


Then I use the same command with different info parameters: .... --info links:
name               index parents          
-------------------------------------------
TorsoLink          0                      
HeadYawLink        1     TorsoLink        
HeadPitchLink      2     HeadYawLink      
LHipYawPitchLink   3     TorsoLink        
LHipRollLink       4     LHipYawPitchLink  
LHipPitchLink      5     LHipRollLink      
LKneePitchLink     6     LHipPitchLink    
LAnklePitchLink    7     LKneePitchLink    
LAnkleRollLink     8     LAnklePitchLink  
LShoulderPitchLink 9     TorsoLink        
LShoulderRollLink  10    LShoulderPitchLink
LElbowYawLink      11    LShoulderRollLink
LElbowRollLink     12    LElbowYawLink    
LWristYawLink      13    LElbowRollLink    
LFinger11Link      14    LWristYawLink    
LFinger12Link      15    LFinger11Link    
LFinger13Link      16    LFinger12Link    
LFinger21Link      17    LWristYawLink    
LFinger22Link      18    LFinger21Link    
LFinger23Link      19    LFinger22Link    
LThumb1Link        20    LWristYawLink    
LThumb2Link        21    LThumb1Link      
RHipYawPitchLink   22    TorsoLink        
RHipRollLink       23    RHipYawPitchLink  
RHipPitchLink      24    RHipRollLink      
RKneePitchLink     25    RHipPitchLink    
RAnklePitchLink    26    RKneePitchLink    
RAnkleRollLink     27    RAnklePitchLink  
RShoulderPitchLink 28    TorsoLink        
RShoulderRollLink  29    RShoulderPitchLink
RElbowYawLink      30    RShoulderRollLink
RElbowRollLink     31    RElbowYawLink    
RWristYawLink      32    RElbowRollLink    
RFinger11Link      33    RWristYawLink    
RFinger12Link      34    RFinger11Link    
RFinger13Link      35    RFinger12Link    
RFinger21Link      36    RWristYawLink    
RFinger22Link      37    RFinger21Link    
RFinger23Link      38    RFinger22Link    
RThumb1Link        39    RWristYawLink    
RThumb2Link        40    RThumb1Link      
-------------------------------------------
name               index parent

It seems this dae file works! Then I write another xml file including the previous dae file to define a manipulator (see naoleftarm.xml). After that, I would like to create a IK solver for this:

with the command:
sudo openrave.py --database inversekinematics --robot=/home/gausslee/IKfastNao/naoleftarm.xml --iktype=translationdirection5d --iktests=100

I got print out:

[xmlreaders-core.cpp:2386] failed to create iksolver naoIKsolver
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 8 is also part of arm dof! excluding from gripper...
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 9 is also part of arm dof! excluding from gripper...
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 10 is also part of arm dof! excluding from gripper...
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 11 is also part of arm dof! excluding from gripper...
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 12 is also part of arm dof! excluding from gripper...
openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip leftarm: TranslationDirection5D [8, 9, 10, 11, 12] (this might take up to 10 min)
openravepy.databases.inversekinematics: generate, creating ik file /home/gausslee/.openrave/kinematics.8f7479ec831af8cf958c7b052ffce9e7/ikfast61.TranslationDirection5D.8_9_10_11_12.cpp
/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py:1122: RuntimeWarning: invalid value encountered in divide
  axisangle /= angle
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 1119/20000] to right end
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 0] to left end
openravepy.ikfast: forwardKinematicsChain, moved translation on intersecting axis [0, 0, 0] to left
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, 1, 49/500],[0, -1, 0, 1/10]]
openravepy.ikfast: generateIkSolver, [[cos(j8), -sin(j8), 0, 0],[sin(j8), cos(j8), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, -1, 0],[0, 1, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j9), -sin(j9), 0, 0],[sin(j9), cos(j9), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 21/200],[0, 1, 0, 3/200],[-1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j10), -sin(j10), 0, 0],[sin(j10), cos(j10), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j11), -sin(j11), 0, 0],[sin(j11), cos(j11), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 0],[0, 1, 0, 0],[-1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j12), -sin(j12), 0, 0],[sin(j12), cos(j12), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 1119/20000]]
openravepy.ikfast: solveFullIK_TranslationDirection5D, ikfast translation direction 5d: [j8, j9, j10, j11, j12]
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller general ik method
openravepy.ikfast: solveLiWoernleHiller, special structure in equations detected, try to solve through elimination
openravepy.ikfast: solveAllEquations, [] [j8]
openravepy.ikfast: solveLiWoernleHiller, 'failed to find a variable to solve'
openravepy.ikfast: solveAllEquations, [] [j9]
openravepy.ikfast: solveLiWoernleHiller, 'failed to find a variable to solve'
openravepy.ikfast: solveLiWoernleHiller, try to solve first two variables pairwise
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant coefficients for solving htj8
openravepy.ikfast: solveAllEquations, [j8] [j9]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [3*cj8*px*sin(j9)/100 - 21*cj8*px*cos(j9)/100 + pp + 3*py*sj8*sin(j9)/100 - 21*py*sj8*cos(j9)/100 - 21*pz*sin(j9)/100 - 3*pz*cos(j9)/100 + 3247839/400000000, 3*cj8*r00*sin(j9)/200 - 21*cj8*r00*cos(j9)/200 + px*r00 + py*r01 + pz*r02 + 3*r01*sj8*sin(j9)/200 - 21*r01*sj8*cos(j9)/200 - 21*r02*sin(j9)/200 - 3*r02*cos(j9)/200]
openravepy.ikfast: solveAllEquations, [j8, j9] [j10]
openravepy.ikfast: solveFullIK_TranslationDirection5D, solved coupled variables: [j8, j9]
openravepy.ikfast: solveAllEquations, [j8, j9] [j10, j11, j12]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sj12*sin(j10) + cj12*cos(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, cj11*sj12*cos(j10) + cj12*sin(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sj12*sin(j10) + cj12*cos(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, cj11*sj12*cos(j10) + cj12*sin(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj12*cos(j10) + cj8*r01 - r00*sj8 - sj12*sin(j10), cj12*sin(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj12*cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj12*cos(j10) + cj8*r01 - r00*sj8 + sj12*sin(j10), cj12*sin(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - sj12*cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*r01 - r00*sj8 - sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*r01 - r00*sj8 + sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 - 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 + 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*r01 - r00*sj8 + sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*r01 - r00*sj8 - sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 - 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 + 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 - sj10*sin(j12), cj10*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 + sj10*sin(j12), -cj10*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [-cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [-cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant coefficients for solving htj10
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 - sj10*sin(j12), cj10*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant coefficients for solving htj10
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 + sj10*sin(j12), -cj10*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: writeIkSolver, generating cpp code...
openravepy.databases.inversekinematics: save, inversekinematics generation is done, compiled shared object: /home/gausslee/.openrave/kinematics.8f7479ec831af8cf958c7b052ffce9e7/ikfast61.TranslationDirection5D.i686.8_9_10_11_12.so
openravepy.databases.inversekinematics: RunFromParser, testing the success rate of robot /home/gausslee/IKfastNao/naoleftarm.xml
[xmlreaders-core.cpp:2386] failed to create iksolver naoIKsolver
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 8 is also part of arm dof! excluding from gripper...
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 9 is also part of arm dof! excluding from gripper...
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 10 is also part of arm dof! excluding from gripper...
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 11 is also part of arm dof! excluding from gripper...
[robotmanipulator.cpp:950] manipulator leftarm gripper dof 12 is also part of arm dof! excluding from gripper...
[ikfastmodule.cpp:928] FindIKSolution: No ik solution found, i = 0
Joint Val: 0 0 0 0 0
Transform: 1442840583 0 0 1 0.16095 0.113 0.12
raw ik command: 0 0 1 0.16095 0 0 0 0.113 0 0 0 0.12

[ikfastmodule.cpp:983] FindIKSolutions: No ik solution found for, i = 0
Joint Val: 0 0 0 0 0
Transform: 1442840583 0 0 1 0.16095 0.113 0.12

openravepy.databases.inversekinematics: testik, success rate: 0.990000, wrong solutions: 0.000000, no solutions: 0.010000, missing solution: 0.040000

It seems it could not find a IK solver. It is an arm with 5 dof. Why it cannot find an IK solver? Had I done something wrong?
I need your help!

Thank you so much!

Cheers,

Gauss

Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: IK problem about left arm of NAO robot

Fabien R
Gauss,

This thread might help you:
http://openrave-users-list.185357.n3.nabble.com/Compiler-errors-when-using-ikfast-td4026357.html

-
Fabien

------------------------------------------------------------------------------
Flow-based real-time traffic analytics software. Cisco certified tool.
Monitor traffic, SLAs, QoS, Medianet, WAAS etc. with NetFlow Analyzer
Customize your own dashboards, set traffic alerts and generate reports.
Network behavioral analysis & security monitoring. All-in-one tool.
http://pubads.g.doubleclick.net/gampad/clk?id=126839071&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: IK problem about left arm of NAO robot

Gausslee
Hi, Fabien,

Thanks for you reply. I checked the link and I did similar stuff as another guy did.

1. I use command

sudo openrave.py --database inversekinematics --robot=/home/gausslee/IKfastNao/naoleftarm1.xml --iktype=translationdirection5d --iktests=100

outputs are:

openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip leftarm: TranslationDirection5D [8, 9, 10, 11, 12] (this might take up to 10 min)
openravepy.databases.inversekinematics: generate, creating ik file /home/gausslee/.openrave/kinematics.8f7479ec831af8cf958c7b052ffce9e7/ikfast61.TranslationDirection5D.8_9_10_11_12.cpp
/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py:1122: RuntimeWarning: invalid value encountered in divide
  axisangle /= angle
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 1119/20000] to right end
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 0] to left end
openravepy.ikfast: forwardKinematicsChain, moved translation on intersecting axis [0, 0, 0] to left
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, 1, 49/500],[0, -1, 0, 1/10]]
openravepy.ikfast: generateIkSolver, [[cos(j8), -sin(j8), 0, 0],[sin(j8), cos(j8), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, -1, 0],[0, 1, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j9), -sin(j9), 0, 0],[sin(j9), cos(j9), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 21/200],[0, 1, 0, 3/200],[-1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j10), -sin(j10), 0, 0],[sin(j10), cos(j10), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j11), -sin(j11), 0, 0],[sin(j11), cos(j11), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 0],[0, 1, 0, 0],[-1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j12), -sin(j12), 0, 0],[sin(j12), cos(j12), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0, 1119/20000]]
openravepy.ikfast: solveFullIK_TranslationDirection5D, ikfast translation direction 5d: [j8, j9, j10, j11, j12]
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller general ik method
openravepy.ikfast: solveLiWoernleHiller, special structure in equations detected, try to solve through elimination
openravepy.ikfast: solveAllEquations, [] [j8]
openravepy.ikfast: solveLiWoernleHiller, 'failed to find a variable to solve'
openravepy.ikfast: solveAllEquations, [] [j9]
openravepy.ikfast: solveLiWoernleHiller, 'failed to find a variable to solve'
openravepy.ikfast: solveLiWoernleHiller, try to solve first two variables pairwise
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant coefficients for solving htj8
openravepy.ikfast: solveAllEquations, [j8] [j9]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for atan2: [3*cj8*px*sin(j9)/100 - 21*cj8*px*cos(j9)/100 + pp + 3*py*sj8*sin(j9)/100 - 21*py*sj8*cos(j9)/100 - 21*pz*sin(j9)/100 - 3*pz*cos(j9)/100 + 3247839/400000000, 3*cj8*r00*sin(j9)/200 - 21*cj8*r00*cos(j9)/200 + px*r00 + py*r01 + pz*r02 + 3*r01*sj8*sin(j9)/200 - 21*r01*sj8*cos(j9)/200 - 21*r02*sin(j9)/200 - 3*r02*cos(j9)/200]
openravepy.ikfast: solveAllEquations, [j8, j9] [j10]
openravepy.ikfast: solveFullIK_TranslationDirection5D, solved coupled variables: [j8, j9]
openravepy.ikfast: solveAllEquations, [j8, j9] [j10, j11, j12]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sj12*sin(j10) + cj12*cos(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, cj11*sj12*cos(j10) + cj12*sin(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sj12*sin(j10) + cj12*cos(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, cj11*sj12*cos(j10) + cj12*sin(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj12*cos(j10) + cj8*r01 - r00*sj8 - sj12*sin(j10), cj12*sin(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj12*cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj12*cos(j10) + cj8*r01 - r00*sj8 + sj12*sin(j10), cj12*sin(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - sj12*cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*r01 - r00*sj8 - sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*r01 - r00*sj8 + sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 - 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 + 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*r01 - r00*sj8 + sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*r01 - r00*sj8 - sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 + 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for atan2: [cj8*py - px*sj8 - 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 + 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 - sj10*sin(j12), cj10*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 + sj10*sin(j12), -cj10*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [-cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [-cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*r01 - r00*sj8 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant coefficients for solving htj10
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 - sj10*sin(j12), cj10*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant coefficients for solving htj10
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 + sj10*sin(j12), -cj10*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: writeIkSolver, generating cpp code...
openravepy.databases.inversekinematics: save, inversekinematics generation is done, compiled shared object: /home/gausslee/.openrave/kinematics.8f7479ec831af8cf958c7b052ffce9e7/ikfast61.TranslationDirection5D.i686.8_9_10_11_12.so
openravepy.databases.inversekinematics: RunFromParser, testing the success rate of robot /home/gausslee/IKfastNao/naoleftarm1.xml
[ikfastmodule.cpp:928] FindIKSolution: No ik solution found, i = 0
Joint Val: 0 0 0 0 0
Transform: 1442840583 0 0 1 0.16095 0.113 0.12
raw ik command: 0 0 1 0.16095 0 0 0 0.113 0 0 0 0.12

[ikfastmodule.cpp:983] FindIKSolutions: No ik solution found for, i = 0
Joint Val: 0 0 0 0 0
Transform: 1442840583 0 0 1 0.16095 0.113 0.12

[ikfastmodule.cpp:928] FindIKSolution: No ik solution found, i = 25
Joint Val: -1.463828318599424 -2.110707192935337 0 0.8102313499364886 0
Transform: 1442840583 -0.9942843756522415 5.551115123125783e-17 0.1067641341173717 -0.02267960128137254 -0.05369331219639015 0.07611582558906954
raw ik command: -0.9942843756522415 5.551115123125783e-17 0.1067641341173717 -0.02267960128137254 0 0 0 -0.05369331219639015 0 0 0 0.07611582558906954

[ikfastmodule.cpp:983] FindIKSolutions: No ik solution found for, i = 25
Joint Val: -1.463828318599424 -2.110707192935337 0 0.8102313499364886 0
Transform: 1442840583 -0.9942843756522415 5.551115123125783e-17 0.1067641341173717 -0.02267960128137254 -0.05369331219639015 0.07611582558906954

openravepy.databases.inversekinematics: testik, success rate: 0.980000, wrong solutions: 0.000000, no solutions: 0.020000, missing solution: 0.080000


It seems it generated IK solver.

2. Four files are automatically generated:
    ikfast61.TranslationDirection5D.8_9_10_11_12.cpp
ikfast61.TranslationDirection5D.8_9_10_11_12.pp
ikfast61.TranslationDirection5D.i686.8_9_10_11_12.so
ikfast.h

3. I compiled the cpp file for testing by using the command:
sudo g++ -lstdc++ -o IK ikfast61.TranslationDirection5D.8_9_10_11_12.cpp

4. I tested the cpp file: ./IK 1 0 0 0.1 0 1 0 0.0 0 0 1 0.0
Failed to get ik solution

It seems it cannot find solution for this. I am using tranlationdirection5d for my nao robot's arm with 5 dofs. Why it cannot get solutions? How do I set the rotation matrix? in the test, I just set it to 1 0 0 0 1 0 0 0 1, the identity rotation matrix.

Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: IK problem about left arm of NAO robot

Rosen Diankov
Administrator
dear gauss,

all you've proven with "./IK 1 0 0 0.1 0 1 0 0.0 0 0 1 0.0" is that there's no IK solution when the robot hand is at

x=0.1 y=0 z=0

and direction is

x=1 y=0 z=0

you should check out the following tutorial on how to really test your IK:

http://www.openrave.org/docs/latest_stable/tutorials/openravepy_examples/#inverse-kinematics-transform6d

iktype=IkParameterizationType.TranslationDirection5D



2014-02-28 7:02 GMT+09:00 Gausslee <[hidden email]>:
Hi, Fabien,

Thanks for you reply. I checked the link and I did similar stuff as another
guy did.

1. I use command

sudo openrave.py --database inversekinematics
--robot=/home/gausslee/IKfastNao/naoleftarm1.xml
--iktype=translationdirection5d --iktests=100

outputs are:

openravepy.databases.inversekinematics: generate, Generating inverse
kinematics for manip leftarm: TranslationDirection5D [8, 9, 10, 11, 12]
(this might take up to 10 min)
openravepy.databases.inversekinematics: generate, creating ik file
/home/gausslee/.openrave/kinematics.8f7479ec831af8cf958c7b052ffce9e7/ikfast61.TranslationDirection5D.8_9_10_11_12.cpp
/usr/lib/python2.7/dist-packages/openravepy/_openravepy_0_8/ikfast.py:1122:
RuntimeWarning: invalid value encountered in divide
  axisangle /= angle
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0,
1119/20000] to right end
openravepy.ikfast: forwardKinematicsChain, moved translation [0, 0, 0] to
left end
openravepy.ikfast: forwardKinematicsChain, moved translation on intersecting
axis [0, 0, 0] to left
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, 1, 49/500],[0, -1,
0, 1/10]]
openravepy.ikfast: generateIkSolver, [[cos(j8), -sin(j8), 0, 0],[sin(j8),
cos(j8), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[1, 0, 0, 0],[0, 0, -1, 0],[0, 1, 0,
0]]
openravepy.ikfast: generateIkSolver, [[cos(j9), -sin(j9), 0, 0],[sin(j9),
cos(j9), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 21/200],[0, 1, 0,
3/200],[-1, 0, 0, 0]]
openravepy.ikfast: generateIkSolver, [[cos(j10), -sin(j10), 0, 0],[sin(j10),
cos(j10), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0,
0]]
openravepy.ikfast: generateIkSolver, [[cos(j11), -sin(j11), 0, 0],[sin(j11),
cos(j11), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, 1, 0],[0, 1, 0, 0],[-1, 0, 0,
0]]
openravepy.ikfast: generateIkSolver, [[cos(j12), -sin(j12), 0, 0],[sin(j12),
cos(j12), 0, 0],[0, 0, 1, 0]]
openravepy.ikfast: generateIkSolver, [[0, 0, -1, 0],[0, 1, 0, 0],[1, 0, 0,
1119/20000]]
openravepy.ikfast: solveFullIK_TranslationDirection5D, ikfast translation
direction 5d: [j8, j9, j10, j11, j12]
openravepy.ikfast: solveLiWoernleHiller, attempting li/woernle/hiller
general ik method
openravepy.ikfast: solveLiWoernleHiller, special structure in equations
detected, try to solve through elimination
openravepy.ikfast: solveAllEquations, [] [j8]
openravepy.ikfast: solveLiWoernleHiller, 'failed to find a variable to
solve'
openravepy.ikfast: solveAllEquations, [] [j9]
openravepy.ikfast: solveLiWoernleHiller, 'failed to find a variable to
solve'
openravepy.ikfast: solveLiWoernleHiller, try to solve first two variables
pairwise
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant
coefficients for solving htj8
openravepy.ikfast: solveAllEquations, [j8] [j9]
openravepy.ikfast: solveSingleVariable, j9 solution: equations used for
atan2: [3*cj8*px*sin(j9)/100 - 21*cj8*px*cos(j9)/100 + pp +
3*py*sj8*sin(j9)/100 - 21*py*sj8*cos(j9)/100 - 21*pz*sin(j9)/100 -
3*pz*cos(j9)/100 + 3247839/400000000, 3*cj8*r00*sin(j9)/200 -
21*cj8*r00*cos(j9)/200 + px*r00 + py*r01 + pz*r02 + 3*r01*sj8*sin(j9)/200 -
21*r01*sj8*cos(j9)/200 - 21*r02*sin(j9)/200 - 3*r02*cos(j9)/200]
openravepy.ikfast: solveAllEquations, [j8, j9] [j10]
openravepy.ikfast: solveFullIK_TranslationDirection5D, solved coupled
variables: [j8, j9]
openravepy.ikfast: solveAllEquations, [j8, j9] [j10, j11, j12]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj8*px*sj9 + cj9*pz -
py*sj8*sj9 - 1119*sj11*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sj12*sin(j10) +
cj12*cos(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 -
1119*sj11*cos(j10)/20000 - 3/200, cj11*sj12*cos(j10) + cj12*sin(j10) -
cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sj12*sin(j10) +
cj12*cos(j10) + cj8*r01 - r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 -
1119*sj11*cos(j10)/20000 - 3/200, cj11*sj12*cos(j10) + cj12*sin(j10) -
cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj12*cos(j10) + cj8*r01 - r00*sj8 - sj12*sin(j10), cj12*sin(j10) -
cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj12*cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj12*cos(j10) + cj8*r01 - r00*sj8 + sj12*sin(j10), cj12*sin(j10) -
cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - sj12*cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sin(j10) + cj8*r01
- r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 -
3/200, cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, -cj11*sin(j10) + cj8*r01
- r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 -
3/200, cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 +
cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 -
cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz -
py*sj8*sj9 - 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 - 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz -
py*sj8*sj9 + 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01
- r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 -
3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01
- r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 -
3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01
- r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 -
3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sj11*sin(j10)/20000, cj11*sin(j10) + cj8*r01
- r00*sj8, -cj8*px*sj9 + cj9*pz - py*sj8*sj9 - 1119*sj11*cos(j10)/20000 -
3/200, -cj11*cos(j10) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 -
cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - sin(j10), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 +
cos(j10)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 + 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz -
py*sj8*sj9 - 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j12] [j10]
openravepy.ikfast: solveSingleVariable, j10 solution: equations used for
atan2: [cj8*py - px*sj8 - 1119*sin(j10)/20000, -cj8*px*sj9 + cj9*pz -
py*sj8*sj9 + 1119*cos(j10)/20000 - 3/200]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00
+ cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 +
cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00
+ cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 +
cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) - cj11*sj10*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00
+ cj9*r01*sj8 + r02*sj9 - sj11*sin(j12), cj10*cj11*sin(j12) - cj8*r00*sj9 +
cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 - sj10*sin(j12), cj10*sin(j12) -
cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 + sj10*sin(j12), -cj10*sin(j12) -
cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 -
sj11*sin(j12), cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 -
sj11*sin(j12), cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 -
sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 +
sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 +
sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 -
sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 -
sj11*sin(j12), -cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 -
sj11*sin(j12), -cj11*sin(j12) - cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 -
sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - cos(j12), cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 +
sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 -
sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - cos(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 +
sin(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [-cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 +
r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [-cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 +
r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 +
cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 +
cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 +
cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 +
cj9*r02 - r01*sj8*sj9 + cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 +
r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj11*sin(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 +
r02*sj9 - sj11*sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 -
cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*r01 - r00*sj8 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 -
cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 - sin(j12), -cj8*r00*sj9 +
cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj8*cj9*r00 + cj9*r01*sj8 + r02*sj9 + sin(j12), -cj8*r00*sj9 +
cj9*r02 - r01*sj8*sj9 - cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 +
r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 +
r02*sj9 - sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 +
r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8, cj8*cj9*r00 + cj9*r01*sj8 +
r02*sj9 + sin(j12), -cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant
coefficients for solving htj10
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 - sj10*sin(j12), cj10*sin(j12) -
cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: solveAllEquations, [j8, j9, j11] [j10, j12]
openravepy.ikfast: solvePairVariablesHalfAngle, multiplying all determinant
coefficients for solving htj10
openravepy.ikfast: solveAllEquations, [j8, j9, j11, j10] [j12]
openravepy.ikfast: solveSingleVariable, j12 solution: equations used for
atan2: [cj10*cos(j12) + cj8*r01 - r00*sj8 + sj10*sin(j12), -cj10*sin(j12) -
cj8*r00*sj9 + cj9*r02 - r01*sj8*sj9 + sj10*cos(j12)]
openravepy.ikfast: writeIkSolver, generating cpp code...
openravepy.databases.inversekinematics: save, inversekinematics generation
is done, compiled shared object:
/home/gausslee/.openrave/kinematics.8f7479ec831af8cf958c7b052ffce9e7/ikfast61.TranslationDirection5D.i686.8_9_10_11_12.so
openravepy.databases.inversekinematics: RunFromParser, testing the success
rate of robot /home/gausslee/IKfastNao/naoleftarm1.xml
[ikfastmodule.cpp:928] FindIKSolution: No ik solution found, i = 0
Joint Val: 0 0 0 0 0
Transform: 1442840583 0 0 1 0.16095 0.113 0.12
raw ik command: 0 0 1 0.16095 0 0 0 0.113 0 0 0 0.12

[ikfastmodule.cpp:983] FindIKSolutions: No ik solution found for, i = 0
Joint Val: 0 0 0 0 0
Transform: 1442840583 0 0 1 0.16095 0.113 0.12

[ikfastmodule.cpp:928] FindIKSolution: No ik solution found, i = 25
Joint Val: -1.463828318599424 -2.110707192935337 0 0.8102313499364886 0
Transform: 1442840583 -0.9942843756522415 5.551115123125783e-17
0.1067641341173717 -0.02267960128137254 -0.05369331219639015
0.07611582558906954
raw ik command: -0.9942843756522415 5.551115123125783e-17 0.1067641341173717
-0.02267960128137254 0 0 0 -0.05369331219639015 0 0 0 0.07611582558906954

[ikfastmodule.cpp:983] FindIKSolutions: No ik solution found for, i = 25
Joint Val: -1.463828318599424 -2.110707192935337 0 0.8102313499364886 0
Transform: 1442840583 -0.9942843756522415 5.551115123125783e-17
0.1067641341173717 -0.02267960128137254 -0.05369331219639015
0.07611582558906954

openravepy.databases.inversekinematics: testik, success rate: 0.980000,
wrong solutions: 0.000000, no solutions: 0.020000, missing solution:
0.080000


It seems it generated IK solver.

2. Four files are automatically generated:
    ikfast61.TranslationDirection5D.8_9_10_11_12.cpp
ikfast61.TranslationDirection5D.8_9_10_11_12.pp
ikfast61.TranslationDirection5D.i686.8_9_10_11_12.so
ikfast.h

3. I compiled the cpp file for testing by using the command:
sudo g++ -lstdc++ -o IK ikfast61.TranslationDirection5D.8_9_10_11_12.cpp

4. I tested the cpp file: ./IK 1 0 0 0.1 0 1 0 0.0 0 0 1 0.0
Failed to get ik solution

It seems it cannot find solution for this. I am using tranlationdirection5d
for my nao robot's arm with 5 dofs. Why it cannot get solutions? How do I
set the rotation matrix? in the test, I just set it to 1 0 0 0 1 0 0 0 1,
the identity rotation matrix.





--
View this message in context: http://openrave-users-list.185357.n3.nabble.com/IK-problem-about-left-arm-of-NAO-robot-tp4026922p4026924.html
Sent from the OpenRAVE Users List mailing list archive at Nabble.com.

------------------------------------------------------------------------------
Flow-based real-time traffic analytics software. Cisco certified tool.
Monitor traffic, SLAs, QoS, Medianet, WAAS etc. with NetFlow Analyzer
Customize your own dashboards, set traffic alerts and generate reports.
Network behavioral analysis & security monitoring. All-in-one tool.
http://pubads.g.doubleclick.net/gampad/clk?id=126839071&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users


------------------------------------------------------------------------------
Flow-based real-time traffic analytics software. Cisco certified tool.
Monitor traffic, SLAs, QoS, Medianet, WAAS etc. with NetFlow Analyzer
Customize your own dashboards, set traffic alerts and generate reports.
Network behavioral analysis & security monitoring. All-in-one tool.
http://pubads.g.doubleclick.net/gampad/clk?id=126839071&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: IK problem about left arm of NAO robot

Fabien R
In reply to this post by Gausslee
On 27/02/14 23:02, Gausslee wrote:

> Hi, Fabien,
>
> Thanks for you reply. I checked the link and I did similar stuff as another
> guy did.
>
> 1. I use command
>
> sudo openrave.py --database inversekinematics
> --robot=/home/gausslee/IKfastNao/naoleftarm1.xml
> --iktype=translationdirection5d --iktests=100
Gauss,

The easiest way to test your solver is:

openrave.py --database inversekinematics
--robot=/home/gausslee/IKfastNao/naoleftarm1.xml
--iktype=translationdirection5d --iktests=100
--manip=<your_manipulator> --show

-
Fabien

------------------------------------------------------------------------------
Flow-based real-time traffic analytics software. Cisco certified tool.
Monitor traffic, SLAs, QoS, Medianet, WAAS etc. with NetFlow Analyzer
Customize your own dashboards, set traffic alerts and generate reports.
Network behavioral analysis & security monitoring. All-in-one tool.
http://pubads.g.doubleclick.net/gampad/clk?id=126839071&iu=/4140/ostg.clktrk
_______________________________________________
Openrave-users mailing list
[hidden email]
https://lists.sourceforge.net/lists/listinfo/openrave-users
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: IK problem about left arm of NAO robot

Gausslee
In reply to this post by Rosen Diankov
Thanks a lot! Rosen. I finally got it to work following the approach in python like below:

with robot: # lock environment and save robot state
    target=[0.1044, 0.04, 0.02]#-(random.rand(3)-0.1)
    print target
    direction =[0.37866608  ,0.34968892  ,0.3211107]#np.random.rand(3)-0.1#[0.1, 0.2, 0.1]
    direction /= np.linalg.norm(direction)
    print direction
    sols = ikmodel.manip.FindIKSolutions(IkParameterization(Ray(target,direction),IkParameterization.Type.TranslationDirection5D),IkFilterOptions.CheckEnvCollisions)

I still have one more question which is "how to set up the direction in 5D IK model". In the above code, direction is composed of three members. What do they mean? In my understanding, it means the coordinator values (x, y, z) in the end-effector coordinator. You can set this to express the orientation? But 5D only can control two dimensions in the orientation, how does openrave do it?

Cheers,

Gauss
   
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: IK problem about left arm of NAO robot

Gausslee
In reply to this post by Rosen Diankov
Hi, Rosen,

Do you know how to solve the forward kinematics with known joint values? Cannot find any in the documentation.

Cheers,

Gauss
Loading...