inverse kinematics (self made function) errors

Question

Manuel 2024-6-21

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2130971-inverse-kinematics-self-made-function-errors

编辑： Divyajyoti Nayak 2024-7-16，2:44

% 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 2024-6-22

编辑：Umar 2024-7-15，6:06

Hi Manuel,

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

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?

请先登录，再进行评论。

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

Answer 1

Divyajyoti Nayak 2024-7-15，5:50

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2130971-inverse-kinematics-self-made-function-errors#answer_1485238

编辑：Divyajyoti Nayak 2024-7-16，2:44

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!