inverse kinematics (self made function) errors
3 views (last 30 days)
Show older comments
% 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
Answers (1)
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
See Also
Categories
Find more on Robotics in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!