0
$\begingroup$

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}))$

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

enter image description here enter image description here

Improve this question
$\endgroup$
3
  • $\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
    Jun 19 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
    Jun 22 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
    Jun 22 at 17:32

0

Reset to default

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.