-1
$\begingroup$

I am designing a robot that I need to model using ARTE library on MATLAB. The robot has 5 DOF with three prismatic and two revolute joints. Works like an inverse gantry robot, with a wrist for additional dexterity.

The problem is I am not able to understand how to model the inverse kinematics.

The MATLAB function I have attached gets the arm matrix for the any given point and is supposed to return the joint parameter values.

Here is the robot schematic followed by my code and the sample code given by the ARTE library developers for SCARA robot.

enter image description here

My Code is Below:

function q = inversekinematic_wz(robot, T)
fprintf('\nComputing inverse kinematics for the %s robot', robot.name);

%initialize q
q=zeros(5,2);

%Arrange all possible solutions
q=[ w(1)+0.030*Cos(asin(w(6)/sqrt(pow(w(4),2)+pow(w(6),2)))),    w(1)+0.030* -w(4)/sqrt(pow(w(4),2)+pow(w(6),2))));
w(3)- 0.030*(w(6)/(sqrt(pow(w(4),2)+pow(w(6),2)))),              w(3)-0.030*Sin(acos(-w(4)/sqrt(pow(w(4),2)+pow(w(6),2))));
w(2),                                                            w(2);
asin(w(6)/sqrt((pow(w(4),2)+pow(w(6),2)))),                      acos(-w(4)/sqrt((pow(w(4),2)+pow(w(6),2))));
roll5,                                                           roll5 ];

SAMPLE CODE GIVEN BY ARTE:

Q = INVERSEKINEMATIC_SCARA(robot, T)    
%   Solves the inverse kinematic problem for the SCARA example robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_SCARA returns 2 possible solutions, thus,
%   Q is a 4x4 matrix where each column stores 4 feasible joint values.
%   
%   Example code:
%
%   robot=load_robot('example', 'scara');
%   q = [0 0 0 0];  
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   %check that all of them are feasible solutions!
%   %and every Ti equals T
%   for i=1:2,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%
%   See also DIRECTKINEMATIC.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
% 
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_scara(robot, T)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fprintf('\nComputing inverse kinematics for the %s robot', robot.name);


%initialize q
q=zeros(4,2);

%Evaluate the DH table to obtain geometric parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%Store geometric parameters
L1=abs(d(1));
L2=abs(a(1));
L3=abs(a(2));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Distance of the point to the origin of S0
R = sqrt(Px^2+Py^2);

%Compute angles
gamma = real(acos((L2^2+R^2-L3^2)/(2*R*L2))); % dot product of l2 and l3
beta = atan2(Py,Px); 
delta = real(acos((L2^2+L3^2-R^2)/(2*L2*L3))); % dot product of l2 and l3 
%find the last rotation for the two possible configurations
q4_1= find_last_rotation(robot,[beta+gamma delta-pi L1-Pz 0], T);
q4_2= find_last_rotation(robot,[beta-gamma pi-delta L1-Pz 0], T);

%Arrange all possible solutions
q=[beta+gamma beta-gamma;
    delta-pi pi-delta;
    L1-Pz    L1-Pz;
    q4_1 q4_2];


% Compute the last rotation
function q4 = find_last_rotation(robot, q, T)

U = T(1:3,1);

%Recompute the DH table according to q1, q2 and q3
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%now compute the position/orientation of the system S3
H=eye(4);
for i=1:3,
    H=H*dh(theta(i), d(i), a(i), alpha(i));
end

X3=H(1:3,1);
Y3=H(1:3,2);

coseno=X3'*U;
seno=U'*Y3;
%compute the last rotation
q4=atan2(seno,coseno);`

$\endgroup$
  • $\begingroup$ From a quick look you are already missing something in your DH parametrization; the distance between the 2 revoute joints is not present in your table $\endgroup$ – N. Staub Oct 1 '17 at 9:11
  • $\begingroup$ yes I noticed that too, however according to definition, the a value (link length) is the distance between b_k and L_k. Which in the case of the last axis is at the same point. Is the definition wrong? Did I misunderstand something in the definition? $\endgroup$ – Ln_r1 Oct 1 '17 at 14:20
  • $\begingroup$ The quantity you mention, b_k and L_k are named so in the textbook you follow, but are not universal names so it's not 100% clear to me what you refer to. Between frame 4 and 5 there is a distance which must appear in your table of parameters, otherwise your kinematics is missing it. As already pointed out, there are several concurrent definitions of DH parametrization, and a few tricks/optimization always possible when selecting the origin of your frames. $\endgroup$ – N. Staub Oct 1 '17 at 15:35
  • $\begingroup$ I see, yes on digging through some videos, I found that moving that 4th Coordinate frame to the actual position is acceptable. $\endgroup$ – Ln_r1 Oct 1 '17 at 16:44
  • $\begingroup$ When I said L_k I'm referring to the origin of the coordinate frame-k for the axis k. In the lectures point b was defined as the intersection of Z_k-1 and X_k. The link length is defined as the distance between b_k and L_k. $\endgroup$ – Ln_r1 Oct 1 '17 at 16:51

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Browse other questions tagged or ask your own question.