Clear Filters
Clear Filters

inverse kinematics (self made function) errors

3 views (last 30 days)
Manuel
Manuel on 21 Jun 2024
Edited: Divyajyoti Nayak on 16 Jul 2024
% 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
  2 Comments
Umar
Umar on 22 Jun 2024
Edited: Umar on 15 Jul 2024
Hi Manuel,
Please do verification check of the correctness of trigonometric calculations, especially the usage of acos and atan2.
Manuel
Manuel on 22 Jun 2024
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?

Sign in to comment.

Answers (1)

Divyajyoti Nayak
Divyajyoti Nayak on 15 Jul 2024
Edited: Divyajyoti Nayak on 16 Jul 2024
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!
  1 Comment
Umar
Umar on 15 Jul 2024
Thanks for your contribution, Divyajyoti. Really appreciate your input.

Sign in to comment.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!