[ Prev ] [ Index ] [ Next ]

Forward kinematics with DH-parameters

Created Donnerstag 28 April 2016

When DH paramerters of a robot is known, and we would like to do forward kinematics, we could use the following function:

function T = fkine_dh(n, theta, d, a, alpha, offset)
% This functions calculate end-effetor position and orientation from
% given joint values.
%
% d, a, alpha and offset are the DH parameters.
% theta is the joint values.
% n is number of the links.
% d, a, alpha and offset are 1xn matrices.
%
% This gives the same result as the RVC toolbox
%
% Contact: manurung.auralius@gmail.com
%
% References: 
% https://www.cs.duke.edu/brd/Teaching/Bio/asmb/current/Papers/chap3-forward-kinematics.pdf
% See page 75    

T = eye(4);

for i = 1:1:n
    ct = cos(theta(i)+offset(i));
    st = sin(theta(i)+offset(i));
    ca = cos(alpha(i));
    sa = sin(alpha(i));

    T = T* [ ct    -st*ca  st*sa    a(i)*ct ; ...
             st    ct*ca  -ct*sa    a(i)*st ; ...
             0     sa     ca        d(i)    ; ...
             0     0      0         1       ];
end

end

./fkine_dh.m

Let's take an example of a 4DOF planar robot, the DH-parameters are:

We then can calculate its end-effector position and orientation:

d = zeros(1, 4);
a = [0.1 0.1 0.1 0.1]; % this is l1, l2, l3, and l4
alpha = zeros(1, 4);
offset = zeros(1, 4);

theta = [0 0 0 0]; % joint values

T = fkine_dh(4, theta, d, a, alpha, offset)

./demo4r.m

References


https://www.cs.duke.edu/brd/Teaching/Bio/asmb/current/Papers/chap3-forward-kinematics.pdf