the followings are the equations I worked out for the inverse kinematics of a 5 dofs robotic arm. First of all, the kinematic diagram: enter image description here 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}))$

$\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

enter image description here enter image description here

  • $\begingroup$ 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$? $\endgroup$
    – CroCo
    Commented Jun 19, 2022 at 20:32
  • $\begingroup$ @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}$ $\endgroup$
    – newby_prog
    Commented Jun 22, 2022 at 9:06
  • $\begingroup$ 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. $\endgroup$
    – Tully
    Commented Jun 22, 2022 at 17:32


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.