Trouble Understanding Diagram for Inverse Kinematics of 6-DoF Manipulator

Question

I am having difficulties writing the Inverse Kinematics for a 6-DoF manipulator, particularly the ABB 4600. I am using this paper as a resource. Here is the image that I am having trouble with, which is also on page 6 of the paper. All that is given is the link lengths and the coordinates of point c. I know how to calculate the α and δ angles, but not the β and γ angles. It seems that the sum of α and γ is equal to q2, which is not given. α is simple to calculate, but I cannot figure out how to calculate β without knowing the value of q2, which is what I am trying to find in the first place. The same goes for γ: δ is simple to calculate, while it seems impossible to find γ without being given q3. If someone could take a look at the paper, it would be much appreciated. Also, I am trying to write the Inverse Kinematics in Python, so if anyone has done anything like this please message me.

Answer 1

For the scope of this question it appears that we don't have to deal with 6 degrees of freedom, but only two real degrees of freedom corresponding to the two joints $A$ and $B$, plus 1 boolean choice representing a 180° rotation around the base, which determines the position of $A$.

Let's go through the text in the paper step by step. $q_1$ is the part happening in the floor plane. There are two ways to line up the plane of the arm with the target wrist position, with a difference of $\pi$ between them. This is the difference between $A$ being right of the vertical axis oder left, so one of the $q_1$ values corresponds to the upper row of images (“front”), the other to the lower (“rear”).

Now switch from the floor plane to the plane of the arm. You know where $A$ is (from $q_1$ and the lengths) and where $C$ is (from input). So the core question is where is $B$. To that effect, ignore the fixed right angle between $B$ and $C$, and replace it by the hypothenuse of that triangle. So the length between $B$ and $C$ is $d_{34}$ no matter what shape you have between these.

So at this point your task is one of constructing a triangle given three lengths. You have positions $A$ and $C$ and the lengths from these to $B$. Geometrically knowing one length means you know the point is on a certain circle. Knowing two lengths means you have to intersect two circles. There are in general two points of intersection (or none if the target is out of reach). One solution will be above the line $AC$ (left pictures), the other below (right pictures).

So how do you compute the angles of a triangle given its edge lengths? Use the cosine law! Reading Wikipedia you find

$$c^2=a^2+b^2-2ab\cos\gamma$$

which you can reformulate to

$$\gamma=\pm\arccos\frac{a^2+b^2-c^2}{2ab}$$

In your specific situation that would translate to

$$\beta=\arccos\frac{a_2^2+g^2-d_{34}^2}{2\,a_2\,g} \qquad \delta=\arccos\frac{a_2^2+d_{34}^2-g^2}{2\,a_2\,d_{34}}$$

since those are interior angles in $\triangle ABC$ at $A$ res. $B$ and we seem to be using positive angles everywhere. You also have

$$\tan\alpha=\frac{h_z}{h_{xy}}$$

from which you can read $\alpha$ up to a possible term of $\ldots+\pi$, just as in the computation for $q_1$. Pick that so that it matches the actual signs of $h_z$ and $h_{xy}$, use atan2 when programming.

The angle $\gamma$ is fixed, defined by the shape of the arm between $B$ and $C$. It should be irrelevant, if the further computation were thinking about $BC$ as a straight line segment, but that isn't the case. So you have to compute an angle in a right-angled triangle again.

$$\gamma=\arctan\frac{d_4}{a_3}$$

From all of these you can compute $q_2$ and $q_3$ as described in the paper.