0

Hope you are doing well.

I am verifying the output of my forward kinematics through inverse kinematics and the results are not as desired. As the output of my inverse kinematics is not coming out to be the same as the input of forward kinematics.

The D-H parameters of manipulator is given as:

Link: alpha, a, theta, d

Link 1 : -90 0 theta1* d1

Link 2 : 0 a2 theta2* 0

Link 3 : 0 a3 theta3* 0

Functions used are:

Inverse kinematics

function q = inv_kinematics(ph)
%input in radians: [ph1 ph2 ph3] = [0.1 0.2 0.4]
l1 = 0.05;
l2 = 0.28;
l3 = 0.2;
d1 = 0.03;
ph1 = ph(1);
ph2 = ph(2);
ph3 = ph(3);
r=sqrt(ph1^2+(ph3-d1)^2);
alpha=acos((r^2+l2^2-l3^2)/(2*r*l2))
q = zeros(3,1);
q1=atan2(ph2,ph1);
q2=atan2(ph1,ph3-d1)-alpha;
q3=atan2(ph1-l2*sin(q2),ph3-d1-l2*cos(q2))-q2;
q=[q1;q2;q3];
%output: q = [1.107 -0.265 1.314]
end

Forward kinematics:

function ph  = forward_kinematics(q)
%input: [q1 q2 q3] = [1.107 -0.265 1.314]
l2 = 0.28;
l3 = 0.2;
d1 = 0.03;
q1 = q(1);
q2 = q(2);
q3 = q(3);
ph = zeros(3,1);
ph1 = l2*cos(q1)*cos(q2)+l3*cos(q1)*cos(q2+q3);
ph2 = l2*sin(q1)*cos(q2)+l3*sin(q1)*cos(q2+q3);
ph3 = d1-l2*sin(q2)-l3*sin(q2+q3);
ph=[ph1;ph2;ph3];
%output: p = [0.1655 0.3308 -0.07005] 
end
Capri
  • 13
  • 2
  • 7
  • 1
    Although uploading your code is a good start, you are more likely to get a useful response if you supply a working example, showing what you get and why it isn't correct. – Phil Goddard Jun 29 '19 at 19:04
  • I am using output of inverse kinematics as input of forward dynamics. The problem is in the 3rd value i.e z-coordinate. The input values that I am using are present in this Simulink file: https://drive.google.com/open?id=1cH5JLDtkv0vauPBtYDpaCcv2Ba9-4Ek3 – Capri Jul 01 '19 at 06:08
  • Very few (if any) people here are going to download a random model from someone they don't know. As part of your question please provide a value for `q` (presumably a 3-element numeric vector) showing what your first function gives for `ph`, and then what your second function gives for `q`. – Phil Goddard Jul 01 '19 at 23:38

0 Answers0