inverse kinematics (self made function) errors

% This is my current code. I am attempting to use inverse kinematics to
% plot the same semicircle i made using forward kinematics. My issue is
% that the function i am currently using doesn't properly calculate the
% "joint2" angle (it prints the same value always)


% NOTE: the error i am having occurs mostly in STEP 4 of my code.




clear;
clc;


% Step one: Defining linked robot and confirming it is correct
L_1(1) = Link([0 199.07 0 pi/2]);
L_1(2) = Link([0 -27 -23.33 -pi/2]);
L_1(3) = Link([0 -12.18 0 0]);
L_1(4) = Link([0 0 0 -pi/2]);
L_1(5) = Link([0 0 107.92 0]);
L_1(6) = Link([0 0 0 0]);
L_1(7) = Link([0 0 133.99 0]);

robot1 = SerialLink(L_1);

figure(2);

robot1.plot([pi/2 0 pi/2 0 0.6542 1.36 0], 'nojoints');
hold on;
% Success

% --------------------------------------------------------------------- %

% Step two: retrieving end effector coordinates and confirming thier
% position
coor = robot1.fkine([pi/2 0 pi/2 0 0.6542 1.36 0]);
    xF = coor.t(1);
    yF = coor.t(2);
    zF = coor.t(3);
    % disp(['(', num2str(xF), ' , ', num2str(yF) , ' , ' num2str(zF), ')']);
    % plot3(xF,yF,zF, 'o', 'Color', 'g');
% Success

% --------------------------------------------------------------------- %

% Step three: plotting a semicircle

radius_S3 = 200;
pointsNum = ( pi/(pi/16) + 1);
semiCircleArray = zeros(pointsNum, 3);

index = 1;
for theta = pi : -pi/16 : 0
    xF = radius_S3 * cos(theta);
    yF = -23.33;
    zF = radius_S3 * sin(theta);
     plot3(xF,yF,zF, '*', 'Color', 'r');
    % disp(['(', num2str(xForward), ' , ', num2str(yForward) , ' , ' num2str(zForward), ')']);
    
    semiCircleArray(index, :) = [xF, yF, zF];
    index = index + 1;
    

end

  plot3(semiCircleArray(:, 1), semiCircleArray(:, 2), semiCircleArray(:, 3), 'o', 'Color', 'b');

% Success

% --------------------------------------------------------------------- %

% Step four: inserting array values into function to confirm angles.

joint1 = zeros(pointsNum, 1);
joint2 = zeros(pointsNum, 1);
 for i = 1:pointsNum
     [joint1(i), joint2(i)] = theta2Output(semiCircleArray(i, 3), semiCircleArray(i, 1));
      disp(['j1: ', num2str(joint1(i)), ' , j2: ', num2str(joint2(i))]);
 
 end


% This is where something goes wrong, which confirms there is an issue
% with my function. The value of one joint angle slowly increments
% whilst the other stays unchanged. 

title('Right Leg - J1 J2 J3');
xlabel('x-axis');
ylabel('y-axis');
zlabel('z-axis');

function [joint1 , joint2] = theta2Output(x, z)
    l1 = 107.92;
    l2 = 133.99;
    x = x(:);
    z = z(:);
    term1 = x.^2;
    term2 = z.^2;
    term3 = l1^2;
    term4 = l2^2;

    cos_joint2 = (term1 + term2 - term3 - term4) / (2 * l2 * l1);
    joint2 = acos(cos_joint2);
        
    joint1 = atan2(x , z) - atan2((l2 * sin(joint2)) , (l1 + (l2*cos(joint2))));

end

[Umar]

Hi Manuel,

Please do verification check of the correctness of trigonometric calculations, especially the usage of acos and atan2.

[Manuel]

hello Umar, I appreciate the input but I'm unsure as to what changes you refer to. From what i understand you are telling me to implement the same code i have already written. Would you mind clarifying what you mean?

Answer 1

[Divyajyoti Nayak]

Hi @Manuel, the reason you are getting constant values for ‘joint2’ is because in the definition of ‘cos_joint2’ the sum of ‘term1’ and ‘term2’ is constant and all other terms are constant so ‘joint2’ ends up being constant.

cos_joint2 = (term1 + term2 - term3 - term4) / (2 * l2 * l1);

joint2 = acos(cos_joint2);

‘term1’ and ‘term2’ are assigned ‘x^2’ and ‘z^2’ respectively, and earlier you have defined ‘x’ and ‘z’ as ‘radius_S3 * cos(theta)’ and ‘radius_S3 * sin(theta)’ respectively. As you probably know, the sum of squares of sine and cosine of an angle is 1, therefore the sum of ‘term1’ and ‘term2’ becomes just the square of ‘radius_S3’ which is a constant.

xF = radius_S3 * cos(theta);

yF = -23.33;

zF = radius_S3 * sin(theta);

Hope this helps!

[Umar]

Thanks for your contribution, Divyajyoti. Really appreciate your input.