Robot inverse kinematics solution based on geometric method--the first three joints of industrial robot

Using the geometric method to solve the inverse kinematics of an industrial robot is to convert the robot knots in space into a geometric solution method under several orthogonal planes, which has a simple and intuitive effect. However, for the situation that a space position and posture of the robot may have multiple joint angles, the solution using the geometric method may cause insufficient consideration, and the result of the solution is reduced. But based on its advantages and intuitiveness, I think I can still share my solution examples with you. I take the industrial robot staubli as an example. The first three joints of the robot determine the position of the robot, the last three joints determine the posture of the robot, and the sixth joint coordinate system is established at the wrist joint of the robot.
The solution of the first joint angle is shown in the figure below: the
Insert picture description here
corresponding matlab code is as follows:
first define the attitude relationship of the robot:

n1 = -1;   % 'l'
 n2 = -1;   % 'u'
 n4 = -1;   % 'n'
 if sol(1)==1%~isempty(strfind(configuration, 'l'))
     n1 = -1;
 end
 if sol(1)==2%~isempty(strfind(configuration, 'r'))
     n1 = 1;
 end
 if sol(2)==1%~isempty(strfind(configuration, 'u'))
     if n1 == 1
         n2 = 1;
     else
         n2 = -1;
     end
 end
 if sol(2)==2%~isempty(strfind(configuration, 'd'))
     if n1 == 1
         n2 = -1;
     else
         n2 = 1;
     end
 end
 if sol(3)==1%~isempty(strfind(configuration, 'n'))
     n4 = 1;
 end
 if sol(3)==2%~isempty(strfind(configuration, 'f'))
     n4 = -1;
 end

The posture is described as follows:
Insert picture description here
The inverse solution code of theta1 is:

 r = sqrt(Px^2 + Py^2);
 if sol(1) == 1
     theta(1) = atan2(Py,Px) + pi - asin(d3/r);
 else
     theta(1) = atan2(Py,Px) + asin(d3/r);
 end
 if theta(1)>pi
      theta(1) = theta(1)-2*pi;
 end

The solution of the second joint is shown in the figure below:
Insert picture description here
The inverse solution code of theta2 is:

V114 = Px*cos(theta(1)) + Py*sin(theta(1))-a1;
r = sqrt(V114^2 + Pz^2);
Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r));
if ~isreal(Psi)
    theta = [];
else
    
    theta(2) = atan2(Pz,V114) + n2*Psi;

The solution of the third joint is shown as follows:
Insert picture description here
The inverse solution code of theta3 is:

num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;
den = cos(theta(2))*Pz - sin(theta(2))*V114;
theta(3) = atan2(a3,d4) - atan2(num, den);

The above is my derivation and written by referring to the code in the robot toolbox. No problems have been found for the time being. If you find any problems during the reference process, please criticize and correct me.

Robot inverse kinematics solution based on geometric method--the first three joints of industrial robot

Guess you like