I'm trying to solve for my inverse kinematics for a 3RRR plannar pkm, my theta is close but apparently it should be accurate to about 2 decimal places and mine is off by 0.5

function [theta] = inverse_kinematics(a, x, y, z, psi, link_1, link_2, link_3, a_ix, a_iy) 

%Naming of changing variables 
a_ix= 298.5; % x-coordinate from link1 (connected to base) to where link 2 connects to end effector
a_iy= 348.14; % y-coordinate from link1 (connected to base) to where link 2 connects to end effector 
psi= 20.03; % end effector pose
phi= pi/6; %offset angle	
    
%Naming of non-changing variables
link1 = 300;
link2 = 375;
link3 = 46.8; 

x = a_ix + link3*cos(psi+phi); % x-coordinate from link1 (connected to base) to end effector center

y = a_iy + link3*sin(psi+phi); % y-coordinate from link1 (connected to base) to end effector center


%calculations of other variables

e1 = x^2 + y^2 + link1^2 - link2^2;
e2 = -2*x*link1;
e3 = -2*y*link1;
theta_up = 2*atan((-e3+sqrt(e3^2+e2^2-e1^2)) / (e1-e2));
theta_down = 2*atan((-e3-sqrt(e3^2+e2^2-e1^2)) / (e1-e2));
theta_up = rad2deg(theta_up);
theta_down = rad2deg(theta_down);

theta = [theta_up;theta_down];

Answer 1

[Umar]

Hi @Phembuvuyo,

I saw your post from earlier today about the inverse kinematics problem you're working on for your 3RRR planar PKM. You mentioned your theta values were off by about 0.5 degrees when they should be accurate to 2 decimal places. I took a look at your code and found the issue. The problem isn't with your math or your approach - those are actually solid. The issue is just in how you're calculating the arctangent.

The Fix:

Change this line:

theta_up = 2*atan((-e3+sqrt(e3^2+e2^2-e1^2)) / (e1-e2));
theta_down = 2*atan((-e3-sqrt(e3^2+e2^2-e1^2)) / (e1-e2));

To this:

theta_up = 2*atan2((-e3+sqrt(e3^2+e2^2-e1^2)), (e1-e2));
theta_down = 2*atan2((-e3-sqrt(e3^2+e2^2-e1^2)), (e1-e2));

Notice the change from atan() to atan2() - and instead of dividing the arguments, you pass them separately. So, why this works.The atan2 function is more numerically stable than atan because:

• It avoids the division operation which can introduce small rounding errors
• It correctly handles all four quadrants automatically
• It deals with edge cases (like when the denominator is near zero) much better

With this simple change, you should see your results match the expected values (103.19 degrees and 3.74 degrees) within the accuracy you need.

I'd also recommend adding a quick discriminant check before the calculation, just to catch cases where the position might be unreachable:

discriminant = e3^2 + e2^2 - e1^2;
if discriminant < 0
  error('Target position is unreachable - no solution exists');
end

Then use sqrt(discriminant) in your theta calculations instead of recalculating it. Hope this helps!

Let me know if you're still seeing issues after making these changes.

P.S. please see attached

[Phembuvuyo]

Thank you so much Umar, it works perfeclty fine now. Really silly how such a small yet pivotal fix could result in such critical change. I also added the descriminant check, should help when trying to figure out the workspace using discretization.