Sum of Angles error

[zplizzi]

I've added the following DH params to the robots file:

    if(name == 'UR3'):
        dh = sp.Matrix([
            [0, sp.pi/2, 0, sp.pi/2],
            [0, sp.pi / 2, l_1, -sp.pi / 2 + th_1],
            [l_2, 0, 0, sp.pi / 2 - th_2],
            [l_3, 0, 0, -th_3],
            [0, sp.pi / 2, l_4, sp.pi / 2 - th_4],
            [0, -sp.pi / 2, l_5, sp.pi + th_5],
            ])

        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]

I'm not 100% sure these params are correct (and they're skipping the last joint of the real UR3 at the moment), but I figured I would give it a shot. When I run it, I get the following error:

% python ikSolver.py UR3                                                                                1 ↵ ✹ ✭

          Running IK solution 

             Working on UR3

kinematics pickle: trying to open  fk_eqns/UR3_pickle.p  in  /home/zplizzi/temp/IKBT
\Trying to read pre-computed forward kinematics from fk_eqns/UR3_pickle.p
Successfully read pre-computed forward kinematics
GOT HERE: robot name:  UR3
SoA: found  sin(th_3 + th_4 - th_5)  in  -sin(l_2)*sin(l_3)*sin(th_1)*cos(th_3 + th_4 - th_5) + sin(th_1)*sin(th_2)*cos(l_2)*cos(l_3)*cos(th_3 + th_4 - th_5) + sin(th_1)*sin(th_3 + th_4 - th_5)*cos(l_2)*cos(th_2) - sin(th_2)*sin(th_3 + th_4 - th_5)*cos(th_1) + cos(l_3)*cos(th_1)*cos(th_2)*cos(th_3 + 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/zplizzi/temp/IKBT/ikbtbasics/ik_classes.py", line 306, in sum_of_angles_transform
    th_xy = find_xy(d[thx], d[thy])
  File "/home/zplizzi/temp/IKBT/ikbtbasics/ik_classes.py", line 132, in find_xy
    thx_s = set(thxy_lookup[thx])
KeyError: th_3 + th_4

It looks like the sum of angles rule is trying to match something like sin(x + y) but is finding instead sin(x + y - z) or something along those lines, although that's just a wild guess.

BTW this project looks awesome!

[dianmuz]

Hi, I've looked at your DH table, it doesn't seem right, e.g. first DOF doesn't have a variable (if you're 'padding' it to be 6DOF, usually we move it to the last line), and the following joints have some strange offset with variable pi/2 + th_x. Please fix your DH parameters before running the solver (I'm pretty sure that's the problem). You are correct, sum-of-angle does recognize sin(x+y), not sin(x+y-z), again, it's most likely caused by incorrect DH parameters.

[dianmuz]

Close #4

[zplizzi]

@dianmuz Both of the things you mention about the DH table are intentional. The first line is meant to be a static transformation between the base frame and the joint_1 frame. The pi/2 + th_x terms are used to change the zero-position of a joint. That allows me to specify a certain pose that the robot should be in with all the joint angles set to 0. The demo robot "Wrist" uses this same feature.

I can definitely try getting rid of those features, but I don't think they're "wrong". But, something else definitely might be!

[dianmuz]

@zplizzi good point. All I'm saying is that th_x should incorporate the information (th_x +pi/2), as it is the variable you're trying to solve. But it worked with Wrist (unique in many ways), it should work with yours.

Let's assume that your DH parameter is correct (and what you wanted), then what it showed here, sin(x + y - z), is you have three joints that are on the same plane (x, y and z). Sum of angle doesn't recognize three joints on the same plane (it only recognize two). This was a design decision (we encountered this issue when we tested it with some randomly generated cases), because usually people don't design their robot that way, as you can imagine, the third angle would revert/reach back to the first joint.

Thanks for letting us know about this. If we receive more requests about this in the future, it'll prove that our assumption about robot design was wrong, we'll change that.

[zplizzi]

Ahh, that makes a lot of sense! In fact, the Universal Robots family of robots (UR3, UR5, UR10) do have this configuration - the second, third, and fourth joints are all parallel and on the same plane. Google for images, and see their page on the DH params for their robots here, although I computed mine somewhat differently I think.

For what it's worth, I think this is one of the most popular families of robots for various research and non-industrial automation tasks. I use it at work, and the ROS community around it is strong. There's also a robotics competition which just started that uses the UR10, which is what I was working on applying this towards.

[dianmuz]

@zplizzi Cool! Didn't know about that. Thanks for pointing me to those designs :)

[blake5634]

Thanks for the input Zack! It seems the offsets are probably not an issue, but in general I avoid them because you can always offset the joints later on after the IK is solved. It doesn't really matter what pose the robot has at "zero" -- after all in real robots the encoders have their various offsets. In the old days (hand cranking the equations) the offsets just made more work and points for making errors so I'm biased against them. Today with IKBT it's less of a deal.

I have seen a related IKBT issue however, if you enter a numerical value into a DH entry, the pickle feature breaks. This is some deep bug with sympy and pickle packages and the super easy workaround is to just use a symbolic parameter (e.g. l_5) and give it a value in pvals.

And yes - we should eventually add the case for three parallel axes.

[zplizzi]

@blake5634 I tried the same DH table without the offsets and got the same error (as expected, if the 3 parallel axis case isn't supported).

I would be happy to help adding that case, but unfortunately I think neither my math or my Sympy skills are up to the task..

[blake5634]

@zplizzi thanks. We'll set up a new features Todo list and three parallel axes will go there. It's just going to be a tweek to the two parallel axis case.