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% 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;

robot1 = SerialLink(L_1);

figure(2);

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

% ——————————————————————— %

% Step three: plotting a semicircle

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

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.

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 % 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;

robot1 = SerialLink(L_1);

figure(2);

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

% ——————————————————————— %

% Step three: plotting a semicircle

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

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.

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 inverse kinematics, serial robot MATLAB Answers — New Questions

Cart

Cart