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.
Because the first one is at the hip, at the bigger triangle. You want the smaller triangle, starting at the second motor, and going to the end of the leg, then along the floor, and then back up.