inverse kinematics (self made function) errors

1 次查看（过去 30 天）
Manuel
Manuel 2024-6-21
编辑： Divyajyoti Nayak 2024-7-16
% 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 个评论
Umar
Umar 2024-6-22
编辑：Umar 2024-7-15
Hi Manuel,
Please do verification check of the correctness of trigonometric calculations, especially the usage of acos and atan2.
Manuel
Manuel 2024-6-22
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?

请先登录，再进行评论。

请先登录，再回答此问题。

回答（1 个）

Divyajyoti Nayak
Divyajyoti Nayak 2024-7-15
编辑：Divyajyoti Nayak 2024-7-16
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!

类别

在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息

标签

Community Treasure Hunt

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

Start Hunting!

Translated by