How does Forward kinematik work in Robotics System Toolbox?
How does the Robot System Toolbox work internally?
I noticed some small offset in the results of the Forward Kinematics if I compare the result of getTransform with the plain geometric Forward Kinematics calculated with the parameters from the Datasheet (p. 61).
The offset also seems to be dynamic. So I wonder what the reason could be.
Is there some kind of dynamic simulation of the joints stiffnes?
Attached is some code to reproduce this:
% Compare DH forward Kinematics with Matlab Model of KinGen3
%% get robot model
close all
gen3 = loadrobot("kinovaGen3");
gen3.DataFormat = ‘column’;
eeName = ‘EndEffector_Link’;
% define q:
q=[ 1.18 -68.68 18.47 -69.09 94.36 112.93 46.00]’;
%q=[ 1.18+180 -68.68 18.47 -69.09 94.36 112.93 46.00]’;
%q=[0 0 0 0 0 0 0]’;
%% calculate FK with DH and with Matlab modell:
% DH
H_DH = getSingleH(getHcomplete(q),0,8)
pos_DH = H_DH(1:3,4);
% Model
H_mod = getTransform(gen3, q/180*pi’, eeName)
pos_mod=H_mod(1:3,4);
%calculate difference:
pos_dif=(pos_DH-pos_mod)*1000
%% Funktiondecalrations for forward kinematics
function H = getHcomplete(q)
% calculates H cell according to input angles
% wheras H{1}=H01, H{2}=H12, H{3}=H23, etc…
i=1;
R{i}=rotx(180)*rotz(q(i)); % Rotation
D{i}=[0 0 +156.4]’/1000; % Displacement
H{i}=RnD2H(R{i},D{i});
i=2;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 5.4 -128.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=3;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -210.4 -6.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=4;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 6.4 -210.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=5;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -208.4 -6.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=6;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 0 -105.9]’/1000;
H{i}=RnD2H(R{i},D{i});
i=7;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -105.9 0]’/1000;
H{i}=RnD2H(R{i},D{i});
i=8;
R{i}=rotx(180)*rotz(0);
D{i}=[0 0 -61.5]’/1000;
H{i}=RnD2H(R{i},D{i});
end
function H = RnD2H(R,D)
% combines Rotation and Displacement into homogenous transform
H = horzcat(R,D);
H = vertcat(H,[0 0 0 1]);
end
function H = getSingleH(h,von,bis)
% returns homogenous transfrormation from to defined frames
H=eye(4);
for i=von+(bis-von>0):sign(bis-von):bis+(von-bis>0)
H=H*h{i}^sign(bis-von);
end
endHow does the Robot System Toolbox work internally?
I noticed some small offset in the results of the Forward Kinematics if I compare the result of getTransform with the plain geometric Forward Kinematics calculated with the parameters from the Datasheet (p. 61).
The offset also seems to be dynamic. So I wonder what the reason could be.
Is there some kind of dynamic simulation of the joints stiffnes?
Attached is some code to reproduce this:
% Compare DH forward Kinematics with Matlab Model of KinGen3
%% get robot model
close all
gen3 = loadrobot("kinovaGen3");
gen3.DataFormat = ‘column’;
eeName = ‘EndEffector_Link’;
% define q:
q=[ 1.18 -68.68 18.47 -69.09 94.36 112.93 46.00]’;
%q=[ 1.18+180 -68.68 18.47 -69.09 94.36 112.93 46.00]’;
%q=[0 0 0 0 0 0 0]’;
%% calculate FK with DH and with Matlab modell:
% DH
H_DH = getSingleH(getHcomplete(q),0,8)
pos_DH = H_DH(1:3,4);
% Model
H_mod = getTransform(gen3, q/180*pi’, eeName)
pos_mod=H_mod(1:3,4);
%calculate difference:
pos_dif=(pos_DH-pos_mod)*1000
%% Funktiondecalrations for forward kinematics
function H = getHcomplete(q)
% calculates H cell according to input angles
% wheras H{1}=H01, H{2}=H12, H{3}=H23, etc…
i=1;
R{i}=rotx(180)*rotz(q(i)); % Rotation
D{i}=[0 0 +156.4]’/1000; % Displacement
H{i}=RnD2H(R{i},D{i});
i=2;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 5.4 -128.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=3;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -210.4 -6.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=4;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 6.4 -210.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=5;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -208.4 -6.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=6;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 0 -105.9]’/1000;
H{i}=RnD2H(R{i},D{i});
i=7;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -105.9 0]’/1000;
H{i}=RnD2H(R{i},D{i});
i=8;
R{i}=rotx(180)*rotz(0);
D{i}=[0 0 -61.5]’/1000;
H{i}=RnD2H(R{i},D{i});
end
function H = RnD2H(R,D)
% combines Rotation and Displacement into homogenous transform
H = horzcat(R,D);
H = vertcat(H,[0 0 0 1]);
end
function H = getSingleH(h,von,bis)
% returns homogenous transfrormation from to defined frames
H=eye(4);
for i=von+(bis-von>0):sign(bis-von):bis+(von-bis>0)
H=H*h{i}^sign(bis-von);
end
end How does the Robot System Toolbox work internally?
I noticed some small offset in the results of the Forward Kinematics if I compare the result of getTransform with the plain geometric Forward Kinematics calculated with the parameters from the Datasheet (p. 61).
The offset also seems to be dynamic. So I wonder what the reason could be.
Is there some kind of dynamic simulation of the joints stiffnes?
Attached is some code to reproduce this:
% Compare DH forward Kinematics with Matlab Model of KinGen3
%% get robot model
close all
gen3 = loadrobot("kinovaGen3");
gen3.DataFormat = ‘column’;
eeName = ‘EndEffector_Link’;
% define q:
q=[ 1.18 -68.68 18.47 -69.09 94.36 112.93 46.00]’;
%q=[ 1.18+180 -68.68 18.47 -69.09 94.36 112.93 46.00]’;
%q=[0 0 0 0 0 0 0]’;
%% calculate FK with DH and with Matlab modell:
% DH
H_DH = getSingleH(getHcomplete(q),0,8)
pos_DH = H_DH(1:3,4);
% Model
H_mod = getTransform(gen3, q/180*pi’, eeName)
pos_mod=H_mod(1:3,4);
%calculate difference:
pos_dif=(pos_DH-pos_mod)*1000
%% Funktiondecalrations for forward kinematics
function H = getHcomplete(q)
% calculates H cell according to input angles
% wheras H{1}=H01, H{2}=H12, H{3}=H23, etc…
i=1;
R{i}=rotx(180)*rotz(q(i)); % Rotation
D{i}=[0 0 +156.4]’/1000; % Displacement
H{i}=RnD2H(R{i},D{i});
i=2;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 5.4 -128.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=3;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -210.4 -6.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=4;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 6.4 -210.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=5;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -208.4 -6.4]’/1000;
H{i}=RnD2H(R{i},D{i});
i=6;
R{i}=rotx(+90)*rotz(q(i));
D{i}=[0 0 -105.9]’/1000;
H{i}=RnD2H(R{i},D{i});
i=7;
R{i}=rotx(-90)*rotz(q(i));
D{i}=[0 -105.9 0]’/1000;
H{i}=RnD2H(R{i},D{i});
i=8;
R{i}=rotx(180)*rotz(0);
D{i}=[0 0 -61.5]’/1000;
H{i}=RnD2H(R{i},D{i});
end
function H = RnD2H(R,D)
% combines Rotation and Displacement into homogenous transform
H = horzcat(R,D);
H = vertcat(H,[0 0 0 1]);
end
function H = getSingleH(h,von,bis)
% returns homogenous transfrormation from to defined frames
H=eye(4);
for i=von+(bis-von>0):sign(bis-von):bis+(von-bis>0)
H=H*h{i}^sign(bis-von);
end
end robotics system toolbox, gettransform, kinovagen3, loadrobot MATLAB Answers — New Questions
