Forward and Inverse Kinematics for robot

Hello,

Hope you are doing well.

I am verifying the output of my forward kinematics through inverse kinematics and the results are not as desired. As the output of my inverse kinematics is not coming out to be the same as the input of forward kinematics.

The D-H parameters of manipulator is given as:

alpha a theta d

Link 1 -90 0 theta1* d1

Link 2 0 a2 theta2* 0

Link 3 0 a3 theta3* 0

Functions used are:

Forward kinematics:

function ph  = forward_kinematics(q)

%input: [q1 q2 q3]

l2 = 0.28;
l3 = 0.2;
d1 = 0.03;

q1 = q(1);
q2 = q(2);
q3 = q(3);

ph = zeros(3,1);
ph1 = l2*cos(q1)*cos(q2)+l3*cos(q1)*cos(q2+q3);
ph2 = l2*sin(q1)*cos(q2)+l3*sin(q1)*cos(q2+q3);
ph3 = d1-l2*sin(q2)-l3*sin(q2+q3);

ph=[ph1;ph2;ph3];

end

Inverse kinematics

function q = inv_kinematics(ph)

%input: [ph1 ph2 ph3]

l1 = 0.05;
l2 = 0.28;
l3 = 0.2;
d1 = 0.03;

ph1 = ph(1);
ph2 = ph(2);
ph3 = ph(3);

r=sqrt(ph1^2+(ph3-d1)^2);
alpha=acos((r^2+l2^2-l3^2)/(2*r*l2))

q = zeros(3,1);
q1=atan2(ph2,ph1);
q2=atan2(ph1,ph3-d1)-alpha;
q3=atan2(ph1-l2*sin(q2),ph3-d1-l2*cos(q2))-q2;

q=[q1;q2;q3];

end

[Mohsina Zafar]

Simulink model of the same is attached here.

[Mohd Musharaf Hussain Sarwari]

Thank you jazak Allah

[Mohd Musharaf Hussain Sarwari]

comming error

[Mohd Musharaf Hussain Sarwari]

Answer 1

[Mohd Musharaf Hussain Sarwari]

I WANT SCARA ROBOT forward-and-inverse-kinematics MATLAB CODE

[Abdullah]

Hi Bro

can i have scara robot Forward and inverse code ?

[Sam Chak]

Hi @Abdullah

Could you please copy and paste the mathematical algorithms of the Forward and Inverse Kinematics in code form? Otherwise, we cannot verify their correctness.

[Umar]

Hi @Mohd Musharaf Hussain Sarwari,

You mentioned, “ I WANT SCARA ROBOT forward-and-inverse-kinematics MATLAB CODE”

Please refer to this link which should help resolve your problem. https://www.mathworks.com/matlabcentral/fileexchange/128529-inverse-kinematics-of-scara-robot

Answer 2

[Krishna Akella]

Hi Mohsina,

I don't know the answer to your question but I looked at your model and I have a few observations. In your model I assume the inputs to your forward kinematics function are the joint angle values and the output is the end effector location. And I assume the inputs to your inverse kinematics function is the end effector location and the outputs are the joint values.

If this is correct, then why would you not pass the output from forward kinematics back to your inverse kinematics function to validate that you are getting back the same joint values?

I made this change and it seems like the first output value matches.

The reason the other joint angles might still not be matching is because you could have multiple solutions to worry about. For example, in the computation of your inverse kinematics function, you have

r = sqrt(ph1^2+(ph3-d1)^2);

There could be two solutions to the sqrt function. A positive and a negative value. And MATLAB returns the positive value. Similar thing is true for other functions like acos, where multiple angles can give the same result. All these will result in multiple solutions to result in the same end effector location.

Regards,

Krishna

[Mohsina Zafar]

Actually, I am giving output of inverse kinematics to the input of forward kinematics.

Thank you for your reply but unfortunately I am using MATLab 2016a version and am unable to open your code. Can you kindly share it in the previous version?

Answer 3

[Ram Bodhe]

function q = inv_kinematics(ph)

%input: [ph1 ph2 ph3]

l1 = 0.05; l2 = 0.28; l3 = 0.2; d1 = 0.03;

ph1 = ph(1); ph2 = ph(2); ph3 = ph(3);

r=sqrt(ph1^2+(ph3-d1)^2); alpha=acos((r^2+l2^2-l3^2)/(2*r*l2))

q = zeros(3,1); q1=atan2(ph2,ph1); q2=atan2(ph1,ph3-d1)-alpha; q3=atan2(ph1-l2*sin(q2),ph3-d1-l2*cos(q2))-q2;

q=[q1;q2;q3];

end

[Ram Bodhe]

iam getting error anyone can help with this

[Umar]

Hi @Ram Bodhe ,

I executed your code and got error message "Not enough input arguments" which indicates that the function is being called without the required input vector ph. To resolve this issue, you need to make sure that the function is called with the appropriate input. Additionally, enhance the function to include input validation, ensuring that the input is a three-element vector. Here is the corrected and updated version of the inv_kinematics function:

function q = inv_kinematics(ph)
  % Check if the input is a 3-element vector
  if nargin < 1 || length(ph) ~= 3
      error('Input must be a 3-element vector [ph1, ph2, ph3].');
  end

    % Define the lengths of the robotic arm segments
    l1 = 0.05; 
    l2 = 0.28; 
    l3 = 0.2; 
    d1 = 0.03;

    % Extract the position components from the input vector
    ph1 = ph(1); 
    ph2 = ph(2); 
    ph3 = ph(3);

    % Calculate the radius and angle for the inverse kinematics
    r = sqrt(ph1^2 + (ph3 - d1)^2); 
    alpha = acos((r^2 + l2^2 - l3^2) / (2 * r * l2));

    % Initialize the joint angles vector
    q = zeros(3, 1); 

    % Calculate the joint angles using inverse kinematics equations
    q1 = atan2(ph2, ph1); 
    q2 = atan2(ph1, ph3 - d1) - alpha; 
    q3 = atan2(ph1 - l2 * sin(q2), ph3 - d1 - l2 * cos(q2)) - q2;

    % Combine the joint angles into a single output vector
    q = [q1; q2; q3];
  end

Explanation of Changes Made

Input Validation: The function now checks if the input ph is provided and whether it contains exactly three elements. If not, it raises an error with a descriptive message. This prevents the function from executing with invalid inputs.

Code Structure: The overall structure of the code remains intact, ensuring that the calculations for the joint angles are preserved.

Comments: Additional comments have been added to clarify each step of the process, making the code more understandable for future users or developers.

Now, to utilize the inv_kinematics function, you can call it with a three-element vector representing the desired position of the end effector. For example:

% Define the desired position
desired_position = [0.1, 0.2, 0.15]; 

% Call the inverse kinematics function
joint_angles = inv_kinematics(desired_position);

% Display the results
disp('Calculated Joint Angles (radians):');
disp(joint_angles);

Please see attached.

When you will run the above example with a valid input vector, the function will compute and display the joint angles required to position the robotic arm at the specified coordinates. Hope, this answers your question.