robot_armrobot_arm_init

Explaining analytical approach to inverse kinematics for robot arm. This method is applyed only for robots that have a spherical wrist (axis of joints P4,P5,P6 meets at one point). And robot should not have joint offsets.
reference : textbook "Robotics" by Hirose

Vector $(a,b)$ represents the direction and rotation of end effector.
\begin{align*}
(a,b) = R^Y R^P R^R\begin{bmatrix} 1 & 0 \\ 0 & 0 \\ 0 & 1 \end{bmatrix}
\end{align*}
\begin{align*}
\begin{bmatrix} a_{x} & b_{x} \\ a_{y} & b_{y} \\ a_{z} & b_{z} \end{bmatrix} = \begin{bmatrix} C_{Y} C_{P} & C_{Y} S_{P} C_{R} + S_{Y} S_{R} \\ S_{Y} C_{P} & S_{Y} S_{P} C_{R} - C_{Y} S_{R} \\ - S_{P} & C_{P} C_{R} \end{bmatrix}
\end{align*}
Yaw,Pitch,Roll angles are given by unity sliders.

px = S_Slider.value; //position
py = L_Slider.value;
pz = U_Slider.value;
rx = R_Slider.value; //rotation
ry = B_Slider.value;
rz = T_Slider.value;

ax = Mathf.Cos(rz * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);
ay = Mathf.Sin(rz * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);
az = -Mathf.Sin(ry * 3.14f / 180.0f);

bx = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Sin(ry * 3.14f / 180.0f) * Mathf.Cos(rz * 3.14f / 180.0f)
            - Mathf.Sin(rx * 3.14f / 180.0f) * Mathf.Sin(rz * 3.14f / 180.0f);
by = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Sin(ry * 3.14f / 180.0f) * Mathf.Sin(rz * 3.14f / 180.0f)
            - Mathf.Sin(rx * 3.14f / 180.0f) * Mathf.Cos(rz * 3.14f / 180.0f);
bz = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);

Point $P_{5}$ is on the vector $a$ line from $P_{r}$.
\begin{align*}
P_{5} = P_{r} - (l_{5} + l_{6})a
\end{align*}
or
\begin{align*}
P_{5x} = P_{x} - (l_{5} + l_{6})a_{x} \\
P_{5y} = P_{y} - (l_{5} + l_{6})a_{y} \\
P_{5z} = P_{z} - (l_{5} + l_{6})a_{z}
\end{align*}

p5x = px - (L5 + L6) * ax;
p5y = py - (L5 + L6) * ay;
p5z = pz - (L5 + L6) * az;

From the structure of arm robot,
\begin{align*}
\theta_{1} = atan2(P_{5y}, P_{5x}) \quad or \quad atan2(P_{5y}, P_{5x}) + \pi
\end{align*}

theta[0] = Mathf.Atan2(p5y, p5x);

Form the Kinematics of arm robot,
\begin{align*}
P_{5} &= \begin{bmatrix} 0 \\ 0 \\ l_{0}+l_{1}\end{bmatrix}\\\\
&+ \begin{bmatrix} C_{1} & -S_{1} & 0 \\ S_{1} & C_{1} & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} C_{2} & 0 & S_{2} \\ 0 & 1 & 0 \\ -S_{2} & 0 & C_{2}\end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ l_{2}\end{bmatrix}\\\\
&+ \begin{bmatrix} C_{1} & -S_{1} & 0 \\ S_{1} & C_{1} & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} C_{23} & 0 & S_{23} \\ 0 & 1 & 0 \\ -S_{23} & 0 & C_{23}\end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ l_{3}\end{bmatrix}\\\\
&+ \begin{bmatrix} C_{1} & -S_{1} & 0 \\ S_{1} & C_{1} & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} C_{23} & 0 & S_{23} \\ 0 & 1 & 0 \\ -S_{23} & 0 & C_{23}\end{bmatrix}\begin{bmatrix} C_{4} & -S_{4} & 0 \\ S_{4} & C_{4} & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ l_{4}\end{bmatrix}\\\\
&= \begin{bmatrix} 0 \\ 0 \\ l_{0}+l_{1}\end{bmatrix} + \begin{bmatrix} C_{1}S_{2} \\ S_{1}S_{2} \\ C_{2}\end{bmatrix}l_{2} + \begin{bmatrix} C_{1}S_{23} \\ S_{1}S_{23} \\ C_{23}\end{bmatrix}(l_{3} + l_{4})
\end{align*}
\begin{align*}
P_{5x} &= C_{1}(S_{2}l_{2} + S_{23}(l_{3} + l_{4})) \tag{1}\\
P_{5y} &= S_{1}(S_{2}l_{2} + S_{23}(l_{3} + l_{4})) \tag{2}\\
P_{5z} - (l_{0} + l_{1}) &= C_{2}l_{2} + C_{23}(l_{3} + l_{4})\tag{3}
\end{align*}
Sum of squares,
\begin{align*}
&(P_{5x})^2 + (P_{5y})^2 + (P_{5z} - (l_{0} + l_{1}))^2 \\
&= (S_{2}l_{2} + S_{23}(l_{3} + l_{4}))^2 + (C_{2}l_{2} + C_{23}(l_{3} + l_{4}))^2\\
&= l_{2}^2 + (l_{3} + l_{4})^2 + 2l_{2}(l_{3} + l_{4})(S_{2}S_{23} + C_{2}C_{23})\\
&= l_{2}^2 + (l_{3} + l_{4})^2 + 2l_{2}(l_{3} + l_{4})C_{3}
\end{align*}
(Above used Cosine theorem)
\begin{align*}
C_{3} = \frac{1}{2l_{2}(l_{3} + l_{4})}(P_{5x}^2 + P_{5y}^2 + (P_{5z} - (l_{0} + l_{1}))^2 - l_{2}^2 - (l_{3} + l_{4})^2)
\end{align*}
for $S^2 + C^2 = 1$
\begin{align*}
\theta_{3} = atan2(\pm\sqrt{1 - C_{3}^2}, C_{3})
\end{align*}

C3 = (Mathf.Pow(p5x, 2) + Mathf.Pow(p5y, 2) + Mathf.Pow(p5z - L1, 2) - Mathf.Pow(L2, 2) - Mathf.Pow(L3+L4, 2))
            / (2 * L2 * (L3+ L4));
theta[2] = Mathf.Atan2(Mathf.Pow(1 - Mathf.Pow(C3, 2), 0.5f), C3);

Squareroot of (squre of (1) + square of (2)) and (3),
using $S_{23} = S_{2}C_{3} + C_{2}S_{3}, C_{23} = C_{2}C_{3} - S_{2}S_{3}$,
\begin{align*}
\sqrt{P_{5x}^2 + P_{5y}^2} &= (l_{2} + (l_{3} + l_{4})C_{3})S_{2} + ((l_{3} + l_{4})S_{3})C_{2}\\\\
P_{z5} - (l_{0} + l_{1}) &= - ((l_{3} + l_{4})S_{3})S_{2} + (l_{2} + (l_{3} + l_{4})C_{3})C_{2}
\end{align*}
When we define,
\begin{align*}
\sqrt{P_{5x}^2 + P_{5y}^2} \equiv A, \quad (l_{2} + (l_{3} + l_{4})C_{3}) \equiv M \\\\
P_{z5} - (l_{0} + l_{1}) \equiv B, \quad ((l_{3} + l_{4})S_{3}) \equiv N
\end{align*}
\begin{align*}
\begin{bmatrix} A \\ B \end{bmatrix} = \begin{bmatrix} M & N \\ -N & M\end{bmatrix}\begin{bmatrix} S_{2} \\ C_{2}\end{bmatrix}
\end{align*}
to get $S_{2}, C_{2}$
\begin{align*}
\begin{bmatrix} S_{2} \\ C_{2} \end{bmatrix} = \frac{1}{M^2 + N^2}\begin{bmatrix} M & -N \\ N & M\end{bmatrix}\begin{bmatrix} A \\ B\end{bmatrix}
\end{align*}
so $\theta_{2}$ is
\begin{align*}
\theta_{2} = atan2(MA - NB, NA + MB)
\end{align*}

float M = L2 + (L3+ L4) * C3;
float N = (L3+ L4) * Mathf.Sin((float)theta[2]);
float A = Mathf.Pow(p5x*p5x + p5y*p5y, 0.5f);
float B = p5z - L1;
theta[1] = Mathf.Atan2(M*A - N*B, N*A + M*B);

Now we have $\theta_{1},\theta_{2},\theta_{3}$(theta[0],theta[1],theta[2]).

Relation between pose of end effector $(a,b)$ and pose of base $(\hat{a},\hat{b})$ is
\begin{align*}
(a,b) = R_{1}R_{2}R_{3}R_{4}R_{5}R_{6}(\hat{a},\hat{b})
\end{align*}
apply reverse rotations
\begin{align*}
R_{3}^{-1}R_{2}^{-1}R_{1}^{-1}(a,b) = R_{4}R_{5}R_{6}(\hat{a},\hat{b})
\end{align*}
Left side can be calculated with know values.
\begin{align*}
&.\begin{bmatrix} C_{23} & 0 & -S_{23} \\ 0 & 1 & 0 \\ S_{23} & 0 & C_{23} \end{bmatrix}\begin{bmatrix} C_{1} & S_{1} & 0 \\ -S_{1} & C_{1} & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} a_{x} & b_{x} \\ a_{y} & b_{y} \\ a_{z} & b_{z}\end{bmatrix} \\\\
&= \begin{bmatrix} C_{23}(C_{1}a_{x} + S_{1}a_{y}) - S_{23}a_{z} & C_{23}(C_{1}b_{x} + S_{1}b_{y}) - S_{23}b_{z} \\ -S_{1}a_{x} + C_{1}a_{y} & -S_{1}b_{x} + C_{1}b_{y} \\ S_{23}(C_{1}a_{x} + S_{1}a_{y}) + C_{23}a_{z} & S_{23}(C_{1}b_{x} + S_{1}b_{y}) + C_{23}b_{z}\end{bmatrix} \\\\
&\equiv \begin{bmatrix}a_{x}^{*} & b_{x}^{*} \\ a_{y}^{*} & b_{y}^{*} \\ a_{z}^{*} & b_{z}^{*}\end{bmatrix}
\end{align*}
Right side is
\begin{align*}
&.\begin{bmatrix} C_{4} & -S_{4} & 0 \\ S_{4} & c_{4} & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} C_{5} & 0 & S_{5} \\ 0 & 1 & 0 \\ -S_{5} & 0 & C_{5} \end{bmatrix}\begin{bmatrix} C_{6} & -S_{6} & 0 \\ S_{6} & C_{6} & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} 0 & 1 \\ 0 & 0 \\ 1 & 0\end{bmatrix} \\\\
&= \begin{bmatrix} C_{4}S_{5} & C_{4}C_{5}C_{6} - S_{4}S_{6} \\ S_{4}S_{5} & S_{4}C_{5}C_{6} + C_{4}S_{6} \\ C_{5} & -S_{5}C_{6}\end{bmatrix}
\end{align*}
So $\theta_{4},\theta_{5},\theta_{6}$ are
\begin{eqnarray} \left\{ \begin{array}{l}
\theta_{4} = atan2(a_{y}^{*}, \ a_{x}^{*}) \quad or \quad atan2(a_{y}^{*}, \ a_{x}^{*}) + \pi\\
\theta_{5} = atan2(C_{4}a_{x}^{*} + S_{4}a_{y}^{*}, \ a_{z}^{*})\\
\theta_{6} = atan2(C_{4}b_{y}^{*} + S_{4}b_{x}^{*}, \ -b_{z}^{*}/S_{5})\\ \end{array} \right.\end{eqnarray}

C1 = Mathf.Cos((float)theta[0]);
C23 = Mathf.Cos((float)theta[1] + (float)theta[2]);
S1 = Mathf.Sin((float)theta[0]);
S23 = Mathf.Sin((float)theta[1] + (float)theta[2]);

asx = C23 * (C1 * ax + S1 * ay) - S23 * az;
asy = -S1 * ax + C1 * ay;
asz = S23 * (C1 * ax + S1 * ay) + C23 * az;
bsx = C23 * (C1 * bx + S1 * by) - S23 * bz;
bsy = -S1 * bx + C1 * by;
bsz = S23 * (C1 * bx + S1 * by) + C23 * bz;

theta[3] = Mathf.Atan2(asy, asx);
theta[4] = Mathf.Atan2(Mathf.Cos((float)theta[3]) * asx + Mathf.Sin((float)theta[3]) * asy, asz);
theta[5] = Mathf.Atan2(Mathf.Cos((float)theta[3]) * bsy - Mathf.Sin((float)theta[3]) * bsx,
            -bsz / Mathf.Sin((float)theta[4]));