SumOfAngles (KeyError) for custom DH table

[C-Aniruddh]

I've defined my robot as follows :

if(name == 'WRO2018'):
        sp.var('th_1 th_2 th_3 th_4 th_5')
        
        ###   These somewhat wierd DH params give you the ZYX Euler Angles
        #       matrix of example 4.7  (don't ask how I got this!)
        dh = sp.Matrix([
        [  sp.pi/2,    0,   l_1,     th_1  ], 
        [        0,    l_2,   0,    -(th_2 + sp.pi/2)  ],   
        [        0,   l_3,   0,    (th_3 + sp.pi/2)  ],     # must fill remaining rows with zeros
        [ -sp.pi/2,     l_4,   0,   th_4  ],
        [      0 ,     0,   l_5,   th_5  ],
        [      0 ,     0,   0,   0  ]
        ])

        vv = [1,1,1,1,1,1]
        variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5)]
        params = [l_1, l_2, l_3, l_4, l_5]
        pvals = {l_1:1, l_2:1, l_3:1, l_4:1, l_5:1}

But I got the error as follows

GOT HERE: robot name:  WRO2018
found new 'joint' (sumofangle) variable:  th_45
sumofanglesT: appending  th_45 = th_4 + th_5
th_4 + th_5
Traceback (most recent call last):
  File "ikSolver.py", line 324, in <module>
    R.sum_of_angles_transform(unknowns) #get the sum of angle
  File "/home/aniruddh/wro_2018/inverse_kinematics/IKBT/ikbtbasics/ik_classes.py", line 306, in sum_of_angles_transform
    th_xy = find_xy(d[thx], d[thy])
  File "/home/aniruddh/wro_2018/inverse_kinematics/IKBT/ikbtbasics/ik_classes.py", line 132, in find_xy
    thx_s = set(thxy_lookup[thx])
KeyError: th_1 + th_3

I checked the #4 , but couldn't help myself.

[blake5634]

For some reason it's trying to find a sum-of-angle relation between th_1 and th_3 but it's not obvious why. I don't see anything wrong with your setup file. I'll keep issue open and hopefully we can have time to dig into it.

[blake5634]

Note: find_xy() is multiply defined:

ikbtbasics/ik_classes AND
ikbtleaves/sum_transform

Next step - delete one! We should re-write for a new method which doesn't require a limited lookup table.

[C-Aniruddh]

Thank you @blake5634, looking forward to the solution.

In the mean time, I tried the solver with another set of DH parameters (5DOF again), and it failed.

if(name == 'IOT'):

        dh = sp.Matrix([
        [  sp.pi/2,      0,   l_1,     th_1], 
        [        0,    l_2,     0,     th_2],   
        [        0,    l_3,     0,     th_3],
        [  sp.pi/2,    l_4,     0,     th_4],
        [        0,      0,   l_5,     th_5],
        [        0,      0,   0,   0  ]
        ])

        vv = [1,1,1,1,1,1]
        variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5)]
        params = [l_1, l_2, l_3, l_4, l_5]
        pvals = {l_1:65, l_2:241, l_3:165, l_4:110, l_5:110}

Error message :

GOT HERE: robot name:  IOT
found new 'joint' (sumofangle) variable:  th_45
sumofanglesT: appending  th_45 = th_4 + th_5
th_4 + th_5
Traceback (most recent call last):
  File "ikSolver.py", line 324, in <module>
    R.sum_of_angles_transform(unknowns) #get the sum of angle
  File "/home/aniruddh/wro_2018/inverse_kinematics/IKBT/ikbtbasics/ik_classes.py", line 306, in sum_of_angles_transform
    th_xy = find_xy(d[thx], d[thy])
  File "/home/aniruddh/wro_2018/inverse_kinematics/IKBT/ikbtbasics/ik_classes.py", line 132, in find_xy
    thx_s = set(thxy_lookup[thx])
KeyError: th_1 + th_2

[blake5634]

Thank you again for helping us improve IKBT. Can you confirm that the examples in ikbtfunctions/ik_robots.py are working for you?

Thanks, BH

[C-Aniruddh]

Yes, I could obtain correct solutions for Wrist and Dhzang robots. However, the solver failed when I tried the Khat6DOF.

[blake5634]

I've now got 3-way and 2-way sum-of-angles working on BH-dev branch (with a pull request). Unfortunately, this does not cause the UR5 to solve. Next step is to look at transformed UR5 equations and see what solution methods to use and figure out why the BT isn't applying them.

[blake5634]

UR5 is now working (mostly) and is committed to the master branch! The fix is very general and may apply to other "IKBT/unsolvable" robots. Thanks for bringing this up!

[C-Aniruddh]

Thank you for sorting out the issue! I'll check it out!