% Listing 4.1  Funktion zur Berechnung der DH-Parameter

function [trafo_liste,para_liste]=dh_parameter(geraden_liste, ...
                                              anfangs_koor_ku)
% Initialisiere Grafik
axis([-1 8 -1 5 -1 10]);
%grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');
 
% Berechne Anfangskoordinatensystem
[n_geraden s]=size(geraden_liste);
ii=1;
hv1=[geraden_liste(ii,1:3)]';      
rv1=[geraden_liste(ii,4:6)]';
ku=anfangs_koor_ku;
usp=hv1+ku*rv1;
zz=rv1/norm(rv1);
xx=cross(zz,[0 0 1]); % x0 orthogonal zu z0 und zB
betrag_xx=norm(xx);
if betrag_xx<eps      % Überprüfung, ob zz parallel zu [0 0 1]
    xx=[1 0 0]';
else
    xx=xx'/betrag_xx;
end
yy=cross(zz,xx);
akt_koor=[xx yy zz usp];    
 
zeichne_ks(akt_koor,ks_linie); % zeichne Anfangskoor K0
hold on
bereich=geraden_liste(ii,7:8);
zeichne_gerade(hv1,rv1,bereich); % zeichne g1 mit Beschriftung
text_pos=hv1+bereich(1)*rv1;
str= strcat('A',num2str(ii),' - K0'); 
text(text_pos(1),text_pos(2),text_pos(3),str);
  
%Berechne alle Folge-Koordinatensysteme und DH-Parameter
    
for ii=2:n_geraden
    hv2=[geraden_liste(ii,1:3)]';
    rv2=[geraden_liste(ii,4:6)]';          % DH-Parameter a
    [a s1 s2]=abstand_geraden_3d(hv1, rv1, hv2, rv2);
    d=norm(s1-usp);                        % DH-Parameter d
        
    avek=s2-s1; 
    if norm(avek)>eps
        xx_n=avek;
            % Zeichne Abstandsvektor der Geraden
        plot3([s1(1) s2(1)],[s1(2) s2(2)],[s1(3) s2(3)],'R');
    else
        xx_n=cross(rv1,rv2);
    end
                                % DH-Parameter theta und alpha
    theta=winkel_2_vektoren(xx,xx_n, rv1);
    alpha=winkel_2_vektoren(rv1,rv2,xx_n);
    
    dh=dh_trafo(theta, d, a, alpha); % DH-Transformation
    trafo_liste{ii-1}=dh;            % Ausgabeliste der Trafos
    para_liste{ii-1}=[theta,d,a,alpha]; % und der Parameter
 
    akt_koor=akt_koor*dh;   % Koordinatensystem Ki in 
                            % Basiskoordinaten
    bereich=geraden_liste(ii,7:8);
    zeichne_gerade(hv2,rv2,bereich);    % Zeichne gi
    zeichne_ks(akt_koor,ks_linie);
    text_pos=hv2+bereich(1)*rv2;
    if ii<n_geraden
        str= strcat('A',num2str(ii));
    else
        str= strcat('A',num2str(ii),' - KF');
    end
    text(text_pos(1),text_pos(2),text_pos(3),str);
        
    hv1=hv2;   % Abspeichern der Vektoren der Vorgängergeraden
    rv1=rv2;
    xx=xx_n;
    usp=s2;
    
end % Ende for-Schleife
 

function dh = dh_trafo(theta,d,a,alpha)
 	dh = rotz(theta) * transl(a,0,d) * rotx(alpha);
    
    
    
% Listing 4.2  Grafikfunktionen für Gerade und Koordinatensystem

function zeichne_gerade(hvek, rvek, bereich)
anfpos=hvek+bereich(1)*rvek;
endpos=hvek+bereich(2)*rvek;
plot3([anfpos(1),endpos(1)],[anfpos(2),endpos(2)],
        [anfpos(3),endpos(3)],'K');

function zeichne_ks(P,L)
B = P(1:3,1:3) .* L;
O = [P(1:3,4)'; P(1:3,4)'; P(1:3,4)']';
B = B + O;
hold on
plot3([P(1,4),B(1,1)],[P(2,4),B(2,1)],[P(3,4),B(3,1)],
       'Color','r','LineWidth',3);
plot3([P(1,4),B(1,2)],[P(2,4),B(2,2)],[P(3,4).B(3,2)],
       'Color','g','LineWidth',3);
plot3([P(1,4),B(1,3)],[P(2,4),B(2,3)],[P(3,4),B(3,3)],
       'Color','b','LineWidth',3);
       
       
% Listing 4.3  Anwendungsbeispiel für die Berechnung der  DH-Parameter

g_liste=[      0 0 0 0 0 1 -1 7;
               1 0 6 0 1 0 -2 2;
               4 0 6 0 1 0 -2 2;
               4 0 6 1 0 0 -1 3;
               6 0 6 0 1 0 -2 2;
               6 0 6 0 0 1 -1 4;
               6 0 9 0 1 0 -1 1];
anfkoor_ku=1;
[trafo_liste para_liste]=dh_parameter(g_liste, anfkoor_ku);



% Listing 4.4  Definition der Kinematikparameter für den Robotertyp ROB1

function [robot] = ROB1_DAT()
 
%Kinematik Parameter 
% Angabe aller Werte in m oder rad
%            atyp vorz  theta  d      a       alpha 
%           (1/2)(1/-1)
robot.dhp = [ 1    1     0      0.4    0      -pi/2;
              1    1     0      0      0.2     0;
              1    1     -pi/2  0      0      -pi/2; 
              1    1     0      0.3    0      -pi/2;
              1    1     0      0      0       pi/2;
              1    1     0      0.1    0       0  ];   

%Effektor-Transformation
robot.ef  = [ 1  0   0   0;
              0  1   0   0;
              0  0   1   0.2;
              0  0   0   1   ];
%Basiskoordinatensystem für den Roboter
robot.bas = [ 1  0   0   0;
              0  1   0   0;
              0  0   1   0;
              0  0   0   1   ];
          
              
% Listing 4.5  Implementierung der Vorwärtstransformation

function [ef_w] = vtraf(q,robot)
dhp = robot.dhp;
bas = robot.bas;
ef  = robot.ef;        % Effektor-Transformation
ef_w= bas;             % Roboter-Basiskoordinatensystem
 
[na s]=size(dhp);      % Anzahl der Achsen
[z nq]=size(q); 
if na<nq
    error('Anzahl der als Zeilenvektor übergebenen Achswerte ist groesser als die Anzahl der definierten Achsen.')
    return
end
 
for ii=1:nq
    vorz=dhp(ii,2);
    if dhp(ii,1)==1     % Drehachse
        traf=dh_trafo(dhp(ii,3)+vorz*q(ii),dhp(ii,4),
                      dhp(ii,5),dhp(ii,6));
    elseif dhp(ii,1)==2 % Schubachse
        traf=dh_trafo(dhp(ii,3),dhp(ii,4)+vorz*q(ii),
                      dhp(ii,5),dhp(ii,6));
    else
        error('nur Drehachse (1) und Schubachse (2) möglich')
    end
    ef_w=ef_w*traf;
end
ef_w=ef_w*ef;


% Listing 4.6  Implementierung der Rücktransformation für zwei parallele Drehachsen

function [q] = rtraf_2_dreh_p(H,kf,robot)
dhp = robot.dhp;      
bas = robot.bas;      % Roboter-Basiskoordinatensystem
ef = robot.ef;        % Effektor-Transformation
hx0=H(1,4); hy0=H(2,4); hz0=H(3,4);
 
% Transformation von H nach K1
 
hy1=-(hz0-dhp(1,4));                % d1
hx1=sqrt(hx0^2+hy0^2)-dhp(1,5);     % a1
 
% Berechnung der Strecken
 
oa=dhp(2,5); % a2
ua=sqrt(dhp(3,5)^2+dhp(4,4)^2); % a3, d4
 
l=sqrt(hx1^2+hy1^2);
if (oa+ua<l | norm(oa-ua)>l | l<eps )
    error('H ausserhalb der Reichweite');
    q=[0 0];
    return
end
 
% Hilfswinkel
alpha=atan2(hy1,hx1);
epsilon=atan(dhp(3,5)/dhp(4,4)); % atan(a3/d4)
beta=acos( (oa^2-ua^2+l^2)/(2*l*oa) );
gamma=acos( (oa^2+ua^2-l^2)/(2*oa*ua) );
 
% Achswinkel für A2, A3
if  kf==1           % 1. Lösung
    q2=alpha-beta;
    q3=-gamma+pi/2+epsilon;
elseif kf==2        % 2. Lösung
    q2=alpha+beta;
    q3=gamma-3*pi/2+epsilon;
end
q(1)=q2; q(2)=q3; 



% Listing 4.7  Testprozedur der Rücktransformation für zwei parallele Achsen


%           atyp   vorz  theta     d      a       alpha 
%           (1/2) (1/-1)
robot.dhp = [ 1     1     0       0.2     0       -pi/2;
              1     1     0       0       0.4     0;
              1     1     -pi/2   0       0       -pi/2; 
              1     1     0       0.4     0       pi/2;   ];

ks_linie=0.05;
dhp=robot.dhp;
hold on
 
% Pose 1, Vorwärtstransformation, Arm gestreckt
ww=[0 0 -pi/2 0 ];     
koor=koortraf(ww,robot);
[zz nk]=size(koor);
zeichne_kin_kette(koor, ks_linie);
num=1; % Nummer der Pose
text(koor{6}(1,4),koor{6}(2,4),koor{6}(3,4)+0.05,num2str(num))
 
% Pose 2, Rücktransformation
% oa, ua und l bilden ein gleichseitiges Dreieck
H=eye(4); H(1:3,4)=[0.4 0 0.2]; 
q2d= rtraf_2_dreh_p(H,2,robot);  % 2. Lösung
ww=[0 q2d(1) q2d(2) 0 ];
koor=koortraf(ww,robot);
zeichne_kin_kette(koor, ks_linie);
num=2;
text(koor{6}(1,4),koor{6}(2,4),koor{6}(3,4)+0.05,num2str(num));
 
% Pose 3-5, Rücktransformation, H liegt auf einer Geraden,
for dd=0.3:-0.1:0.1             
    H(1:3,4)=[0.55 0 dd];
    q2d= rtraf_2_dreh_p(H,1,robot); % 1. Lösung
    ww=[0 q2d(1) q2d(2) 0 ];
    koor=koortraf(ww,robot);
    num=num+1;
    text(koor{6}(1,4),koor{6}(2,4),koor{6}(3,4)+
         0.05,num2str(num));
    zeichne_kin_kette(koor, ks_linie);
end



% Listing 4.8  Implementierung der Rücktransformation für die drei Grundachsen eines Gelenkarmroboters

function [q] = rtraf_3_dreh_orth(H,kf,robot) 

  %                           --  wie Listing 4.6 --

% Transformation von H nach K1
hy1=-(hz0-dhp(1,4));                % d1

% Berechnung von hx1, hx2, q1 in Abhängigkeit 
% der gewählten Konfiguration
if  kf==1 | kf==2           % Lösung 1 und Lösung 2
    q1=atan2(hy0,hx0);    
    hx1=sqrt(hx0^2+hy0^2)-dhp(1,5);     % a1
elseif kf==3 | kf==4        % Lösung 3 und Lösung 4
    q1=atan2(hy0,hx0)+pi;   
    hx1=-sqrt(hx0^2+hy0^2)-dhp(1,5);    % a1
end
q(1)=q1;

                           % --  wie Listing 4.6 --
                           
                           
                           
% Listing 4.9  Vollständige Implementierung der Rücktransformation

function [q] = rtraf_6_gelenk(ZF,kf,robot)

% Eingangsparameter:
% ZF: Ziel-Frame in Weltkoordinaten, [4 4] Array; 
% kf: gewählte Roboterkonfiguration 1-8
%           1: q11, q21, q31, q41, q51, q61,     
%           2: q11, q21, q31, q42, q52, q62,   
%           3: q11, q22, q32, q41, q51, q61,  
%           4: q11, q22, q32, q42, q52, q62,    
%           5: q12, q23, q33, q41, q51, q61,        
%           6: q12, q23, q33, q42, q52, q62,  
%           7: q12, q24, q34, q41, q51, q61,
%           8: q12, q24, q34, q42, q52, q62,
% robot:    Roboterparameter
%           robot.dhp   DH-Parameter, [n 6] Array
%           Spalten:    atyp   vorz  theta  d    a    alpha 
%                       (0/1) (1/-1)
%           robot.ef:   Effektortransformation
%           [4 4] Array
%           robot.bas   Roboterbasis in Weltkoordinaten
%           [4 4] Array
%                              
% Rückgabeparameter:
% q:        Liste der Achswerte,[6] Array, Zeilenvektor
%           Angabe im Bogenmass oder in m.     

if kf<1 | kf>8
    error('Wert für kf nicht 1<=kf<=8')
end
 
dhp = robot.dhp;      
BAS = robot.bas;      % Roboter-Basiskoordinatensystem
EF = robot.ef;        % Effektor-Transformation
 
% Flanschtransformation
FL=inv(BAS)*ZF*inv(EF);
 
% Handwurzelpunkt
hwp= FL(1:3,4)-FL(1:3,3)*dhp(6,4); % hwp=uf-zf*d6
hx0=hwp(1); hy0=hwp(2); hz0=hwp(3);
 
% Achswinkel q1  
% Transformation von H nach K1
hy1=-(hz0-dhp(1,4));        % d1-hz
 
if  kf==1 | kf==2 | kf==3 | kf==4 
    q1=atan2(hy0,hx0);    
    hx1=sqrt(hx0^2+hy0^2)-dhp(1,5);     % a1
else
    q1=atan2(hy0,hx0)+pi;   
    hx1=-sqrt(hx0^2+hy0^2)-dhp(1,5); 
end
 
% Berechnung der Strecken
oa=dhp(2,5); % a2
ua=sqrt(dhp(3,5)^2+dhp(4,4)^2); % a3, d4
 
l=sqrt(hx1^2+hy1^2);
if (oa+ua<l | norm(oa-ua)>l | l<eps )
    error('H ausserhalb der Reichweite');
    return
end
 
% Hilfswinkel
alpha=atan2(hy1,hx1);
epsilon=atan(dhp(3,5)/dhp(4,4)); % atan(a3/d4)
beta=acos( (oa^2-ua^2+l^2)/(2*l*oa) );
gamma=acos( (oa^2+ua^2-l^2)/(2*oa*ua) );
 
% Achswinkel q2, q3
if  kf==1 | kf==2 | kf==5 | kf==6
    %q2=-alpha-beta; % z1-Achse zeigt in die Ebene
    q2=alpha-beta; % z1-Achse zeigt in die Ebene
    q3=-gamma+pi/2+epsilon;
else
    q2=alpha+beta; % z1-Achse zeigt in die Ebene
    q3=gamma-3*pi/2-epsilon; % ?? -gamma, ?? -epsilon
end
q(1)=q1; q(2)=q2; q(3)=q3;
 
% Orientierung im HWP
K3=eye(4);
for ii=1:3
    vorz=dhp(ii,2);
    if dhp(ii,1)==1     % Drehachse
        traf=dh_trafo(dhp(ii,3)+vorz*q(ii),dhp(ii,4),
                      dhp(ii,5),dhp(ii,6));
    else
        error('es dürfen nur Drehachsen definiert sein')
    end
    K3=K3*traf;
end
OH=K3(1:3,1:3);
 
% Differenzorientierung
OF=FL(1:3,1:3);
DF=inv(OH)*OF;
 
% Handwinkel q4, q5, q6
if kf==1|kf==3|kf==5|kf==7 % direkt
    [aw bw gw ]=inv_zyz_euler(DF,1);
    q(4)=aw; q(5)=bw; q(6)= gw; 
else                      % gedreht
    [aw bw gw ]=inv_zyz_euler(DF,2);
    q(4)=aw; q(5)=bw; q(6)= gw; 
end
 






   



