Loesung 

Wir benoetigen einige Befehle aus dem Paket linalg. Daher wird es hier geladen, falls Sie die Loesung unabhaengig vom Rest des Worksheets betrachten wollen. 

>	with(linalg):

 

Die Drehmatrizen lassen sich wie oben direkt aus den Achsenrichtungen und den Winkeln angeben. Aber die letzte Drehung ist nicht um den Ursprung, sodass noch eine Translation erforderlich ist. 

Wir bezeichnen mit Rd1,Rd2,Rd3 die Drehmatrizen zu den entsprechenden Achsen an den Gelenken, mit p0 = (1,0,0)^T den Knickpunkt des dritten Gelenks in der Ausgangsposition  = 0. Nach den ersten beiden Teilbewegungen liegt dieses Gelenk an der Stelle, die wir durch  Rd2 (Rd1 p0) = q angeben koennen. Die dritte Bewegung ist also eine Drehung um den Punkt q. Um nun diese Drehung, eine affin lineare  Abbildung , mit Hilfe der  Drehmatrix Rd3 anzugeben, muessen wir eine Translation von q in den Ursprung vorschalten und nach dem Drehen wieder verschieben, d.h. die affin lineare Abbildung ist gegeben durch  

       q + Rd3 (y-q) 

fuer ein y in . Insgesamt erhalten wir fuer einen Bildpunkt 

       b = Rd2 Rd1 p0  + Rd3( (Rd2 (Rd1 (x-p0))) ) = (E3-Rd3) Rd2 Rd1 p0 + Rd3 Rd2 Rd1 x . 

Das Programm muss somit den Punkt  

       p = (E3-Rd3) Rd2 Rd1 p0 

und die Matrix 

      A = Rd3 Rd2 Rd1 

in Abhaengigkeit der drei Wiinkel berechnen.  

Nach diesen Ueberlegungen ist es leicht, ein entsprechendes Programm zu schreiben.     

>

bagger:=proc(phi1,phi2,phi3)  local Rd1,Rd2,Rd3,E3,p0,d1,d2, sd2, tmp, Sd;  global A, p;  p0 := matrix(3,1,[1,0,0]);  d1 := matrix(3,1,[0,0,1]);  d2 := matrix(3,1,[0,-1,0]);  E3 := diag(1,1,1);  tmp := d1 &* transpose(d1);  Sd  := matrix(3,3,[[0,-d1[3,1],d1[2,1]],[d1[3,1],0,-d1[1,1]],[-d1[2,1], d1[1,1],0]]);  Rd1 := evalm(tmp + cos(phi1)*(E3-tmp) + sin(phi1)*Sd);  sd2 := evalm(Rd1 &* d2);  tmp := evalm(sd2 &* transpose(sd2));  Sd  := matrix(3,3,[[0,-sd2[3,1],sd2[2,1]],[sd2[3,1],0,-sd2[1,1]],[-sd2[2,1], sd2[1,1],0]]);  Rd2 := evalm(tmp + cos(phi2)*(E3-tmp) + sin(phi2)*Sd);  Rd3 := evalm(tmp + cos(phi3)*(E3-tmp) + sin(phi3)*Sd);  A := evalm(Rd3 &* Rd2 &* Rd1);  p := evalm((E3-Rd3) &* Rd2 &* Rd1 &* p0);  " Bildpunkt ist durch Ax + p bestimmbar " end:

 

Testen wir nun den Fall in der Abbildung 

>	bagger(0,Pi/4,-Pi/2);

 

Dann bekommen wir fuer das Ende des Arms, wie erwartet 

>	x:=matrix(3,1,[2,0,0]); simplify(evalm(A&*x+p));

 

Drehen wir auch noch am ersten Gelenk, so ist etwa 

>	bagger(-Pi/2,Pi/2,-Pi/4); simplify(evalm(A&*x+p));

 

>

 

>