The robot "arm" can be considered as composed of four segments connecting five points. Point A is at the base of the central pivot on the horizontal plane, point B is directly above A at the first joint, point C is the second joint, point D is the third joint, and point E hangs directly below D and above the plane ready to pick up the object. Let the length of segment AB be called a, the length of BC be b, the length of CD be c, the length of DE be d and let point E be a distance e above the horizontal plane. As you recall I called the horizontal plane coordinates of the point A (0,0) and the those of the object on the plane to be picked up (x,y). We want to use all these these data to determine the three angles ABC, BCD, CDE which must be achieved by the motors at the three joints, B, C, and D.
The computation goes as follows.
r = sqrt(x^2+y^2); % Horizontal distance from object to base
s = sqrt(r2+(a-d-e)^2); % Distance between B and D
angleBDE = pi/2+atan2(a-d-e,r);
angleABD = pi - angleBDE;
angleCBD = acos((b^2+s^2-c^2)/(2*b*s)); % Cosine law of triangles
angleCDB = acos((c^2+s^2-b^2(/(2*c*s)); % " " " "
angleABC = angleABD+angleCBD;
angleCDE = angleCDB+angleBDE;
angleBCD = pi-angleCBD-angleCDB;
These last are the three required angles, all in radians. I already computed the azimuth angle of the motor at the base for you.
