Failed to create IKFast output for 4DOF manipulator

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

Failed to create IKFast output for 4DOF manipulator

JoshMarino
This post has NOT been accepted by the mailing list yet.
Hello,

I am following this tutorial here to generate an IK Solver for a hexapod robot I found here. I installed OpenRave using "sudo apt-get install ros-indigo-openrave".

I was trying to use MoveIt! with KDLKinematics for motion planning, but it failed to find an attempt when not specifying joint angles. Therefore I am trying to create my own kinematics solver for this robot.

I got to the step where I was able to test my newly generate collada file in OpenRave.

When trying to generate the IK solver, I ran into some problems. Here is what I typed into the command line:
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=hexapod.dae --iktype=translationxaxisangleznorm4d --baselink=5 --eelink=10 --savefile=ikfast61_leg_LF.cpp

Here is the output I get from the above command:
[plugindatabase.h:577 Create] Failed to create name fcl_, interface collisionchecker
[plugindatabase.h:577 Create] Failed to create name ode, interface collisionchecker
INFO: moved translation [0, 0, 0] to right end
INFO: moved translation [0, 0, 0] to left end
INFO: moved translation on intersecting axis [0, 0, 0] to left
INFO: [[1, 0, 0, 0],[0, 1, 0, 0],[0, 0, 1, 0]]
INFO: [[cos(j0), -sin(j0), 0, 0],[sin(j0), cos(j0), 0, 0],[0, 0, 1, 0]]
INFO: [[2**(1/2)/2, 0, 2**(1/2)/2, 208349/5000000],[2**(1/2)/2, 0, -2**(1/2)/2, 208349/5000000],[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, 5399/50000],[-1, 0, 0, 0],[0, 0, -1, 0]]
INFO: [[cos(j2), -sin(j2), 0, 0],[sin(j2), cos(j2), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 4387/50000],[0, 1, 0, 0],[0, 0, 1, 0]]
INFO: [[cos(j3), -sin(j3), 0, 0],[sin(j3), cos(j3), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 2413/20000],[0, -1, 0, 0],[0, 0, -1, 0]]
INFO: ikfast translation axis 4d, globaldir=[0]
[0]
[1], basedir=[1]
[0]
[0]: [j0, j1, j2, j3]
INFO: depth=0 c=1, [] [j0, j1, j2, j3]: cases=None
INFO: adding atan2(2**(1/2)*(px - py)/2, 2**(1/2)*(px + py)/2) = px**2 + py**2 all zeros check
INFO: adding atan2(2**(1/2)*(px - py)/2, 2**(1/2)*(px + py)/2) = px**2 + py**2 all zeros check
INFO: depth=0 c=3, [j0] [j1, j2, j3]: cases=set([])
INFO: adding atan2(2**(1/2)*(px - py)/2, 2**(1/2)*(px + py)/2) = px**2 + py**2 all zeros check
INFO: adding atan2(2**(1/2)*(px - py)/2, 2**(1/2)*(px + py)/2) = px**2 + py**2 all zeros check
WARNING: solveDialytically equations 18 > 12, should be equal...
INFO: ioffset 0
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 9521, in <module>
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 2283, in generateIkSolver
    chaintree.leftmultiply(Tleft=self.multiplyMatrix(LinksLeft), Tleftinv=self.multiplyMatrix(LinksLeftInv[::-1]))
  File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 1119, in leftmultiply
    self.Pfk = Tleft[0:2,0:2]*self.Pfk+Tleft[0:2,3]
  File "/home/josh/.local/lib/python2.7/site-packages/sympy/matrices/matrices.py", line 409, in __mul__
    return matrix_multiply(self,a)
  File "/home/josh/.local/lib/python2.7/site-packages/sympy/matrices/matrices.py", line 2580, in matrix_multiply
    raise ShapeError()
sympy.matrices.matrices.ShapeError


Here is the output from "openrave-robot.py hexapod.dae --info links":
name                       index parents      
----------------------------------------------
base_link                  0                  
body_link                  1     base_link    
camera_depth_optical_frame 2     body_link    
camera_rgb_optical_frame   3     body_link    
imu_link                   4     body_link    
leg_center_LF              5     body_link    
coxa_LF                    6     leg_center_LF
femur_LF                   7     coxa_LF      
tibia_LF                   8     femur_LF    
tarsus_LF                  9     tibia_LF    
foot_LF                    10    tarsus_LF    
leg_center_LR              11    body_link    
coxa_LR                    12    leg_center_LR
femur_LR                   13    coxa_LR      
tibia_LR                   14    femur_LR    
tarsus_LR                  15    tibia_LR    
foot_LR                    16    tarsus_LR    
leg_center_RF              17    body_link    
coxa_RF                    18    leg_center_RF
femur_RF                   19    coxa_RF      
tibia_RF                   20    femur_RF    
tarsus_RF                  21    tibia_RF    
foot_RF                    22    tarsus_RF    
leg_center_RR              23    body_link    
coxa_RR                    24    leg_center_RR
femur_RR                   25    coxa_RR      
tibia_RR                   26    femur_RR    
tarsus_RR                  27    tibia_RR    
foot_RR                    28    tarsus_RR    
pan_link                   29    body_link    
----------------------------------------------
name                       index parents

I have attached the xacro file from the GitHub repository (with minor changes) and the URDF. I tried uploading the Collada file, but it was too large. It can be remade with the following command:
roscore & rosrun collada_urdf urdf_to_collada hexapod.urdf hexapod.dae

Since this is my first time using OpenRave, I have no idea where to go from here. Any help would be appreciated.

Thanks

hexapod_model.xacro
hexapod.urdf
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Failed to create IKFast output for 4DOF manipulator

JoshMarino
This post has NOT been accepted by the mailing list yet.
This post was updated on .
I have trimmed down the URDF file to only include one leg now. Here is the Collada file: https://github.com/JoshMarino/QuadrupedModel/blob/master/hexapod_test.dae

name          index parents      
---------------------------------
base_link     0                  
leg_center_RR 1     base_link    
coxa_RR       2     leg_center_RR
femur_RR      3     coxa_RR      
tibia_RR      4     femur_RR    
tarsus_RR     5     tibia_RR    
foot_RR       6     tarsus_RR    
---------------------------------
name          index parents

I am trying to run this command:
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=hexapod_test.dae --iktype=translationxaxisangle4d --baselink=1 --eelink=5 --savefile=output_ikfast61.cpp

Following output is now:
[plugindatabase.h:577 Create] Failed to create name fcl_, interface collisionchecker
[plugindatabase.h:577 Create] Failed to create name ode, interface collisionchecker
INFO: moved translation [0, 0, 0] to right end
INFO: moved translation [0, 0, 0] to left end
INFO: moved translation on intersecting axis [0, 0, 0] to left
INFO: [[1, 0, 0, 0],[0, 1, 0, 0],[0, 0, 1, 0]]
INFO: [[cos(j0), -sin(j0), 0, 0],[sin(j0), cos(j0), 0, 0],[0, 0, 1, 0]]
INFO: [[-2**(1/2)/2, 0, 2**(1/2)/2, -208349/5000000],[-2**(1/2)/2, 0, -2**(1/2)/2, -208349/5000000],[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, 5399/50000],[1, 0, 0, 0],[0, 0, -1, 0]]
INFO: [[cos(j2), -sin(j2), 0, 0],[sin(j2), cos(j2), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 4387/50000],[0, 1, 0, 0],[0, 0, 1, 0]]
INFO: [[cos(j3), -sin(j3), 0, 0],[sin(j3), cos(j3), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 0],[0, 1, 0, 0],[0, 0, 1, 0]]
INFO: ikfast translation axis 4d, globaldir=[0]
[0]
[1], basedir=[1]
[0]
[0]: [j0, j1, j2, j3]
INFO: depth=0 c=1, [] [j0, j1, j2, j3]: cases=None
INFO: adding atan2(2**(1/2)*(px - py)/2, 2**(1/2)*(px + py)/2) = px**2 + py**2 all zeros check
INFO: adding atan2(2**(1/2)*(px - py)/2, 2**(1/2)*(px + py)/2) = px**2 + py**2 all zeros check
INFO: adding atan2(2**(1/2)*(px - py)/2, 2**(1/2)*(px + py)/2) = px**2 + py**2 all zeros check
INFO: adding atan2(2**(1/2)*(px - py)/2, 2**(1/2)*(px + py)/2) = px**2 + py**2 all zeros check
INFO: depth=0 c=3, [j0] [j1, j2, j3]: cases=set([])
INFO: adding atan2(5399*2**(1/2)*(-2500000*cj0*px - 2500000*cj0*py + 2500000*px*sj0 - 2500000*py*sj0 - 208349)/125000000000, -5399*pz/25000) = 5399*Abs(pz)/25000 + 5399*Abs(2**(1/2)*(-2500000*cj0*px - 2500000*cj0*py + 2500000*px*sj0 - 2500000*py*sj0 - 208349))/125000000000 all zeros check
INFO: adding atan2(5399*2**(1/2)*(-2500000*cj0*px - 2500000*cj0*py + 2500000*px*sj0 - 2500000*py*sj0 - 208349)/125000000000, -5399*pz/25000) = 5399*Abs(pz)/25000 + 5399*Abs(2**(1/2)*(-2500000*cj0*px - 2500000*cj0*py + 2500000*px*sj0 - 2500000*py*sj0 - 208349))/125000000000 all zeros check
INFO: adding atan2(5399*2**(1/2)*(-2500000*cj0*px - 2500000*cj0*py + 2500000*px*sj0 - 2500000*py*sj0 - 208349)/125000000000, -5399*pz/25000) = 5399*Abs(pz)/25000 + 5399*Abs(2**(1/2)*(-2500000*cj0*px - 2500000*cj0*py + 2500000*px*sj0 - 2500000*py*sj0 - 208349))/125000000000 all zeros check
INFO: adding atan2(5399*2**(1/2)*(-2500000*cj0*px - 2500000*cj0*py + 2500000*px*sj0 - 2500000*py*sj0 - 208349)/125000000000, -5399*pz/25000) = 5399*Abs(pz)/25000 + 5399*Abs(2**(1/2)*(-2500000*cj0*px - 2500000*cj0*py + 2500000*px*sj0 - 2500000*py*sj0 - 208349))/125000000000 all zeros check
INFO: depth=0 c=5, [j0, j1] [j2, j3]: cases=set([])
INFO: j2 solution: equations used for atan2: [-2**(1/2)*cj0*px*sj1/2 - 2**(1/2)*cj0*py*sj1/2 + cj1*pz + 2**(1/2)*px*sj0*sj1/2 - 2**(1/2)*py*sj0*sj1/2 - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 2**(1/2)*cj0*cj1*px/2 + 2**(1/2)*cj0*cj1*py/2 - 2**(1/2)*cj1*px*sj0/2 + 2**(1/2)*cj1*py*sj0/2 + 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, 2**(1/2)*cj0*cj1*px*sin(j2)/2 + 2**(1/2)*cj0*cj1*py*sin(j2)/2 - 2**(1/2)*cj0*px*sj1*cos(j2)/2 - 2**(1/2)*cj0*py*sj1*cos(j2)/2 - 2**(1/2)*cj1*px*sj0*sin(j2)/2 + 2**(1/2)*cj1*py*sj0*sin(j2)/2 + cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + 2**(1/2)*px*sj0*sj1*cos(j2)/2 - 2**(1/2)*py*sj0*sj1*cos(j2)/2 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, 2**(1/2)*cj0*cj1*px*cos(j2)/2 + 2**(1/2)*cj0*cj1*py*cos(j2)/2 + 2**(1/2)*cj0*px*sj1*sin(j2)/2 + 2**(1/2)*cj0*py*sj1*sin(j2)/2 - 2**(1/2)*cj1*px*sj0*cos(j2)/2 + 2**(1/2)*cj1*py*sj0*cos(j2)/2 - cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 - 2**(1/2)*px*sj0*sj1*sin(j2)/2 + 2**(1/2)*py*sj0*sj1*sin(j2)/2 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [-2**(1/2)*cj0*px*sj1/2 - 2**(1/2)*cj0*py*sj1/2 + cj1*pz + 2**(1/2)*px*sj0*sj1/2 - 2**(1/2)*py*sj0*sj1/2 - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 2**(1/2)*cj0*cj1*px/2 + 2**(1/2)*cj0*cj1*py/2 - 2**(1/2)*cj1*px*sj0/2 + 2**(1/2)*cj1*py*sj0/2 + 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, 2**(1/2)*cj0*cj1*px*sin(j2)/2 + 2**(1/2)*cj0*cj1*py*sin(j2)/2 - 2**(1/2)*cj0*px*sj1*cos(j2)/2 - 2**(1/2)*cj0*py*sj1*cos(j2)/2 - 2**(1/2)*cj1*px*sj0*sin(j2)/2 + 2**(1/2)*cj1*py*sj0*sin(j2)/2 + cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + 2**(1/2)*px*sj0*sj1*cos(j2)/2 - 2**(1/2)*py*sj0*sj1*cos(j2)/2 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, 2**(1/2)*cj0*cj1*px*cos(j2)/2 + 2**(1/2)*cj0*cj1*py*cos(j2)/2 + 2**(1/2)*cj0*px*sj1*sin(j2)/2 + 2**(1/2)*cj0*py*sj1*sin(j2)/2 - 2**(1/2)*cj1*px*sj0*cos(j2)/2 + 2**(1/2)*cj1*py*sj0*cos(j2)/2 - cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 - 2**(1/2)*px*sj0*sj1*sin(j2)/2 + 2**(1/2)*py*sj0*sj1*sin(j2)/2 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [-2**(1/2)*cj0*px*sj1/2 - 2**(1/2)*cj0*py*sj1/2 + cj1*pz + 2**(1/2)*px*sj0*sj1/2 - 2**(1/2)*py*sj0*sj1/2 - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 2**(1/2)*cj0*cj1*px/2 + 2**(1/2)*cj0*cj1*py/2 - 2**(1/2)*cj1*px*sj0/2 + 2**(1/2)*cj1*py*sj0/2 + 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, 2**(1/2)*cj0*cj1*px*sin(j2)/2 + 2**(1/2)*cj0*cj1*py*sin(j2)/2 - 2**(1/2)*cj0*px*sj1*cos(j2)/2 - 2**(1/2)*cj0*py*sj1*cos(j2)/2 - 2**(1/2)*cj1*px*sj0*sin(j2)/2 + 2**(1/2)*cj1*py*sj0*sin(j2)/2 + cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + 2**(1/2)*px*sj0*sj1*cos(j2)/2 - 2**(1/2)*py*sj0*sj1*cos(j2)/2 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, 2**(1/2)*cj0*cj1*px*cos(j2)/2 + 2**(1/2)*cj0*cj1*py*cos(j2)/2 + 2**(1/2)*cj0*px*sj1*sin(j2)/2 + 2**(1/2)*cj0*py*sj1*sin(j2)/2 - 2**(1/2)*cj1*px*sj0*cos(j2)/2 + 2**(1/2)*cj1*py*sj0*cos(j2)/2 - cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 - 2**(1/2)*px*sj0*sj1*sin(j2)/2 + 2**(1/2)*py*sj0*sj1*sin(j2)/2 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [-2**(1/2)*cj0*px*sj1/2 - 2**(1/2)*cj0*py*sj1/2 + cj1*pz + 2**(1/2)*px*sj0*sj1/2 - 2**(1/2)*py*sj0*sj1/2 - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 2**(1/2)*cj0*cj1*px/2 + 2**(1/2)*cj0*cj1*py/2 - 2**(1/2)*cj1*px*sj0/2 + 2**(1/2)*cj1*py*sj0/2 + 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, 2**(1/2)*cj0*cj1*px*sin(j2)/2 + 2**(1/2)*cj0*cj1*py*sin(j2)/2 - 2**(1/2)*cj0*px*sj1*cos(j2)/2 - 2**(1/2)*cj0*py*sj1*cos(j2)/2 - 2**(1/2)*cj1*px*sj0*sin(j2)/2 + 2**(1/2)*cj1*py*sj0*sin(j2)/2 + cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + 2**(1/2)*px*sj0*sj1*cos(j2)/2 - 2**(1/2)*py*sj0*sj1*cos(j2)/2 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, 2**(1/2)*cj0*cj1*px*cos(j2)/2 + 2**(1/2)*cj0*cj1*py*cos(j2)/2 + 2**(1/2)*cj0*px*sj1*sin(j2)/2 + 2**(1/2)*cj0*py*sj1*sin(j2)/2 - 2**(1/2)*cj1*px*sj0*cos(j2)/2 + 2**(1/2)*cj1*py*sj0*cos(j2)/2 - cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 - 2**(1/2)*px*sj0*sj1*sin(j2)/2 + 2**(1/2)*py*sj0*sj1*sin(j2)/2 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: depth=0 c=7, [j0, j1, j2] [j3]: cases=set([])
INFO: adding atan2(-cj1*cj2 - sj1*sj2, cj1*sj2 - cj2*sj1) = Abs(-cj1*cj2 - sj1*sj2) + Abs(cj1*sj2 - cj2*sj1) all zeros check
INFO: adding atan2(-cj1*cj2 - sj1*sj2, cj1*sj2 - cj2*sj1) = Abs(-cj1*cj2 - sj1*sj2) + Abs(cj1*sj2 - cj2*sj1) all zeros check
INFO: depth=0, c=2, iter=0/2, starting newcases: set([py, px])
INFO: depth=2 c=9, [] [j0, j1, j2, j3]: cases=set([py, px])
INFO: depth=2 c=11, [j1] [j0, j2, j3]: cases=set([py, px])
INFO: j2 solution: equations used for atan2: [cj1*pz - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, -cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [cj1*pz - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, -cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [cj1*pz - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, -cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [cj1*pz - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, -cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: depth=2 c=13, [j1, j2] [j0, j3]: cases=set([py, px])
INFO: adding atan2(-cj1*cj2 - sj1*sj2, cj1*sj2 - cj2*sj1) = Abs(-cj1*cj2 - sj1*sj2) + Abs(cj1*sj2 - cj2*sj1) all zeros check
INFO: adding atan2(-cj1*cj2 - sj1*sj2, cj1*sj2 - cj2*sj1) = Abs(-cj1*cj2 - sj1*sj2) + Abs(cj1*sj2 - cj2*sj1) all zeros check
INFO: depth=2 c=15, [j1, j2, j3] [j0]: cases=set([py, px])
INFO: trying to guess variable from [j0]
INFO: have only one variable left j0 and most likely it is not in equations [-sin(j1)*sin(j2)*cos(j3) - sin(j1)*sin(j3)*cos(j2) + sin(j2)*sin(j3)*cos(j1) - cos(j1)*cos(j2)*cos(j3) - cos(r00), pz*cos(j1) - 208349*2**(1/2)*sin(j1)/5000000 + 4387*cos(j2)/50000, pz*sin(j1) + 4387*sin(j2)/50000 + 208349*2**(1/2)*cos(j1)/5000000 + 5399/50000, -pz**2 - 5399*pz*sin(j1)/25000 - 1124876251*2**(1/2)*cos(j1)/125000000000 - 92926465801/12500000000000, pz*sin(j1)*sin(j2) + pz*cos(j1)*cos(j2) - 208349*2**(1/2)*sin(j1)*cos(j2)/5000000 + 208349*2**(1/2)*sin(j2)*cos(j1)/5000000 + 5399*sin(j2)/50000 + 4387/50000, pz*sin(j1)*cos(j2) - pz*sin(j2)*cos(j1) + 208349*2**(1/2)*sin(j1)*sin(j2)/5000000 + 208349*2**(1/2)*cos(j1)*cos(j2)/5000000 + 5399*cos(j2)/50000]
INFO: depth=0, c=2, iter=0/2, adding newcases: set([py, px])
INFO: depth=0, c=2, iter=1/2, starting newcases: set([Abs(px) + Abs(py)])
INFO: depth=1 c=16, [] [j0, j1, j2, j3]: cases=set([Abs(px) + Abs(py)])
INFO: depth=1 c=18, [j1] [j0, j2, j3]: cases=set([Abs(px) + Abs(py)])
INFO: j2 solution: equations used for atan2: [cj1*pz - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, -cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [cj1*pz - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, -cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [cj1*pz - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, -cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: j2 solution: equations used for atan2: [cj1*pz - 208349*2**(1/2)*sj1/5000000 + 4387*cos(j2)/50000, 208349*2**(1/2)*cj1/5000000 + pz*sj1 + 4387*sin(j2)/50000 + 5399/50000, cj1*pz*cos(j2) + 208349*2**(1/2)*cj1*sin(j2)/5000000 + pz*sj1*sin(j2) - 208349*2**(1/2)*sj1*cos(j2)/5000000 + 5399*sin(j2)/50000 + 4387/50000, -cj1*pz*sin(j2) + 208349*2**(1/2)*cj1*cos(j2)/5000000 + pz*sj1*cos(j2) + 208349*2**(1/2)*sj1*sin(j2)/5000000 + 5399*cos(j2)/50000]
INFO: depth=1 c=20, [j1, j2] [j0, j3]: cases=set([Abs(px) + Abs(py)])
INFO: adding atan2(-cj1*cj2 - sj1*sj2, cj1*sj2 - cj2*sj1) = Abs(-cj1*cj2 - sj1*sj2) + Abs(cj1*sj2 - cj2*sj1) all zeros check
INFO: adding atan2(-cj1*cj2 - sj1*sj2, cj1*sj2 - cj2*sj1) = Abs(-cj1*cj2 - sj1*sj2) + Abs(cj1*sj2 - cj2*sj1) all zeros check
INFO: depth=1 c=22, [j1, j2, j3] [j0]: cases=set([Abs(px) + Abs(py)])
INFO: trying to guess variable from [j0]
INFO: have only one variable left j0 and most likely it is not in equations [-sin(j1)*sin(j2)*cos(j3) - sin(j1)*sin(j3)*cos(j2) + sin(j2)*sin(j3)*cos(j1) - cos(j1)*cos(j2)*cos(j3) - cos(r00), pz*cos(j1) - 208349*2**(1/2)*sin(j1)/5000000 + 4387*cos(j2)/50000, pz*sin(j1) + 4387*sin(j2)/50000 + 208349*2**(1/2)*cos(j1)/5000000 + 5399/50000, -pz**2 - 5399*pz*sin(j1)/25000 - 1124876251*2**(1/2)*cos(j1)/125000000000 - 92926465801/12500000000000, pz*sin(j1)*sin(j2) + pz*cos(j1)*cos(j2) - 208349*2**(1/2)*sin(j1)*cos(j2)/5000000 + 208349*2**(1/2)*sin(j2)*cos(j1)/5000000 + 5399*sin(j2)/50000 + 4387/50000, pz*sin(j1)*cos(j2) - pz*sin(j2)*cos(j1) + 208349*2**(1/2)*sin(j1)*sin(j2)/5000000 + 208349*2**(1/2)*cos(j1)*cos(j2)/5000000 + 5399*cos(j2)/50000]
INFO: depth=0, c=2, iter=1/2, adding newcases: set([Abs(px) + Abs(py)])
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 9521, in <module>
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 2283, in generateIkSolver
    chaintree.leftmultiply(Tleft=self.multiplyMatrix(LinksLeft), Tleftinv=self.multiplyMatrix(LinksLeftInv[::-1]))
  File "/opt/ros/indigo/lib/python2.7/site-packages/openravepy/_openravepy_/ikfast.py", line 1119, in leftmultiply
    self.Pfk = Tleft[0:2,0:2]*self.Pfk+Tleft[0:2,3]
  File "/home/josh/.local/lib/python2.7/site-packages/sympy/matrices/matrices.py", line 409, in __mul__
    return matrix_multiply(self,a)
  File "/home/josh/.local/lib/python2.7/site-packages/sympy/matrices/matrices.py", line 2580, in matrix_multiply
    raise ShapeError()
sympy.matrices.matrices.ShapeError

Still have no idea how to proceed, any help or suggestions would be appreciated.
Loading...