# Analytical solution to inverse kinematics of 5 dof robot arm

the followings are the equations I worked out for the inverse kinematics of a 5 dofs robotic arm. First of all, the kinematic diagram: For the joint angles $$q_{2}, q_{3}$$ and $$q_{4}$$ I resorted to the IK solution of the 3-link planar arm. px,py,pz are desired position coordinates, $$\phi$$ should be the angle between the $$z_{5}$$ axis and the horizontal plane $$x_{0}-y_{0}$$ which determine the orientation in the plane where the joints lie, see second image.

First compute
$$q_{1} = atan2(p_{y}, p_{x})$$

Then, see third image
$$r = \sqrt{p_{x}^{2}+p_{y}^{2}}$$
$$r_{w} = r - d_{5}*cos(\phi)$$
$$r$$ and $$r_{w}$$ are the projections on the $$x_{0}-y_{0}$$ plane
$$p_{wz} = p_{z} - d_{5}*sin(\phi)$$
$$s_{w} = \sqrt{r_{w}^2+(p_{wz}-d_{1})^2}$$

Apply cosine theorem to triangle a2-a3-sw:
$$s_{w}^2 = a_{2}^2+a_{3}^2 - 2*a_{2}*a_{3}*cos(\pi-q_{3})$$ ->
$$cos(q_{3}) = \frac{s_{w}^2 - a_{2}^2 - a_{3}^2}{2*a_{2}*a_{3}}$$

Two solutions for $$q_{3}$$:

$$q_{3}= acos(cos(q_{3}))$$
$$q_{33}= -acos(cos(q_{3}))$$

Then
$$\alpha = atan2(p_{wz} - d_{1}, r_{w})$$

Apply again cosine theorem to the triangle a2-a3-sw
$$a_{3}^2 = s_{w}^2 + a_{2}^2 - 2*a_{2}*s_{w}*cos(\beta)$$ ->
$$cos(\beta) = \frac{s_{w}^2 + a_{2}^2 - a_{3}^2}{2*a_{2}*s_{w}}$$
$$\beta = acos(cos(\beta))$$

Two solutions for $$q_{2}$$
$$q_{2} = \alpha - \beta$$
$$q_{22} = \alpha + \beta$$

Two solutions for $$q_{4}$$
$$q_{4} = \phi - q_{2} - q_{3}$$
$$q_{44} = \phi - q_{22} - q_{33}$$

Now, how to get $$q_{5}$$??

To apply these equations I need to specify the target pose with px,py,pz, $$\phi$$. I tried to specify the target pose that I want the robot to reach using the following matrix multiplication to transform from the {0} frame to the {5} frame: $$D(x)*D(y)*D(z)*R_{z}(atan(py/px))*R_{y}(\pi/2-\phi)*R_{z}(\pi+\theta_{5})$$
From this matrix I get the position vector px,py,pz and the $$\phi$$ angle. $$\theta_{5}$$ should match the $$q_{5}$$ joint angle, which I'm not sure how to get, but then I thought I could specify directly the desired last joint rotation without any computation.

Are this solution and the way I choose the target pose correct? Give me some feedback please

• Your question is vague. Do you need to compute the forward kinematics? If so, which method (i.e. DH, product of exponentials, dual quaternion,...,etc). Also, what is $\phi = q_2+q_3+q_4$? Jun 19 at 20:32
• @CroCo hi, look at the second picture I attached. This is a 3-link planar arm which corresponds to joints 2,3,4 of my robot, first picture. $\phi$ should be clear now. I need to impose the value of $\phi$ because it's used in the analytical solution of IK to solve for $q_{4}$ once I calculated $q_{2}$ and $q_{3}$ Jun 22 at 9:06
• Hi @newby_prog This looks like a homework question, and on stack exchange, questions asking for homework help must include a summary of the work you've done so far to solve/understand the problem, and a description of the difficulty you are having solving/understanding it. Please edit your question to add this information and take a look at How to Ask and tour for more information on how stack exchange works. For advice on how to write a good question, see the Robotics question checklist. Jun 22 at 17:32