xp shouldn't include l1 in the kinematics formulas

I have analyzed the formulas, and it seems to me, that the degree which you are subtracting when calculating beta is actually wrong -- you are taking the larger triangle, which begins at a, but you should actually take a smaller triangle (by l1), which starts at b. To do that, you should subtract atan2(-z, xp - l1), but then both uses of xp are actually xp-l1, so we can as well just subtract l1 from xp from the start.