Aufgabe 4.3 (Jacobi-Matrix einer durch Eulerwinkel parametrierten Drehung)
Erstellt: 16.3.2020
Ersteller: W. Kemmetm\303\274ller
\303\204nderungen: WK, 27.5.2020: Korrektur Berechnung von omega
restart;
with(LinearAlgebra):
with(VectorCalculus,Jacobian):
with(VectorCalculus,SpaceCurve):
with(plots):
Kinematik
Definition der elementaren Drehmatrizen
RR_z:=phi-><<cos(phi),sin(phi),0>|<-sin(phi),cos(phi),0>|<0,0,1>>;
RR_y:=phi-><<cos(phi),0,-sin(phi)>|<0,1,0>|<sin(phi),0,cos(phi)>>;
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSVSUl96R0YoZio2I0kkcGhpR0YoRig2JEkpb3BlcmF0b3JHRihJJmFycm93R0YoRigtSSQ8fGdyPkdGJzYlLUkkPCw+R0YnNiUtSSRjb3NHRiU2IzkkLUkkc2luR0YlRjwiIiEtRjg2JSwkRj4hIiJGOkZALUY4NiVGQEZAIiIiRihGKEYoNyNGLg==
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSVSUl95R0YoZio2I0kkcGhpR0YoRig2JEkpb3BlcmF0b3JHRihJJmFycm93R0YoRigtSSQ8fGdyPkdGJzYlLUkkPCw+R0YnNiUtSSRjb3NHRiU2IzkkIiIhLCQtSSRzaW5HRiVGPCEiIi1GODYlRj4iIiJGPi1GODYlRkBGPkY6RihGKEYoNyNGLg==
Drehmatrizen (Hintereinanderausf\303\274hrung einer Drehung um z-, y-, z-Achse)
RR_0_1:=RR_z(phi);
RR_1_2:=RR_y(theta);
RR_2_3:=RR_z(psi);
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSdSUl8wXzFHRihYLCUhR0YoRihbZ2whIiUhISEjKiIkIiQtSSVtcm93RzYkRiYvSSttb2R1bGVuYW1lR0YoRiQ2JS1JI21pR0YyNiRRJGNvc0YoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1JI21vR0YyNiNRMCZBcHBseUZ1bmN0aW9uO0YoLUkobWZlbmNlZEdGMjYjLUY3NiNRJiZwaGk7RigtRjE2JS1GNzYkUSRzaW5GKEY6Rj1GQS1JI21uR0YyNiNRIjBGKC1GMTYkLUY+NiNRKiZ1bWludXMwO0YoRkdGMEZMRkxGTC1GTTYjUSIxRig3Iy1JJ01hdHJpeEdGJTYjL0kkJWlkR0YoIjVtQE8icGZwdVklPQ==
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSdSUl8xXzJHRihYLCUhR0YoRihbZ2whIiUhISEjKiIkIiQtSSVtcm93RzYkRiYvSSttb2R1bGVuYW1lR0YoRiQ2JS1JI21pR0YyNiRRJGNvc0YoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1JI21vR0YyNiNRMCZBcHBseUZ1bmN0aW9uO0YoLUkobWZlbmNlZEdGMjYjLUY3NiNRKCZ0aGV0YTtGKC1JI21uR0YyNiNRIjBGKC1GMTYkLUY+NiNRKiZ1bWludXMwO0YoLUYxNiUtRjc2JFEkc2luRihGOkY9RkFGRy1GSDYjUSIxRihGR0ZQRkdGMDcjLUknTWF0cml4R0YlNiMvSSQlaWRHRigiNSEqcE4icGZwdVklPQ==
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSdSUl8yXzNHRihYLCUhR0YoRihbZ2whIiUhISEjKiIkIiQtSSVtcm93RzYkRiYvSSttb2R1bGVuYW1lR0YoRiQ2JS1JI21pR0YyNiRRJGNvc0YoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1JI21vR0YyNiNRMCZBcHBseUZ1bmN0aW9uO0YoLUkobWZlbmNlZEdGMjYjLUY3NiNRJiZwc2k7RigtRjE2JS1GNzYkUSRzaW5GKEY6Rj1GQS1JI21uR0YyNiNRIjBGKC1GMTYkLUY+NiNRKiZ1bWludXMwO0YoRkdGMEZMRkxGTC1GTTYjUSIxRig3Iy1JJ01hdHJpeEdGJTYjL0kkJWlkR0YoIjU5PU4icGZwdVklPQ==
Gesamte Drehmatrix
RR_0_3:=RR_0_1.RR_1_2.RR_2_3;
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSdSUl8wXzNHRihYLCUhR0YoRihbZ2whIiUhISEjKiIkIiQtSSVtcm93RzYkRiYvSSttb2R1bGVuYW1lR0YoRiQ2JS1GMTYnLUYxNiUtSSNtaUdGMjYkUSRjb3NGKC9JKmZvbnRzdHlsZUdGKFEnbm9ybWFsRigtSSNtb0dGMjYjUTAmQXBwbHlGdW5jdGlvbjtGKC1JKG1mZW5jZWRHRjI2Iy1GOzYjUSYmcGhpO0YoLUZCNiNRMSZJbnZpc2libGVUaW1lcztGKC1GMTYlRjpGQS1GRjYjLUY7NiNRKCZ0aGV0YTtGKEZLLUYxNiVGOkZBLUZGNiMtRjs2I1EmJnBzaTtGKC1GQjYjUSgmbWludXM7RigtRjE2JS1GMTYlLUY7NiRRJHNpbkYoRj5GQUZFRkstRjE2JUZdb0ZBRlctRjE2JS1GMTYnRltvRktGTkZLRlUtRkI2I1EnJnBsdXM7RigtRjE2JUY4RktGYG8tRjE2JC1GQjYjUSomdW1pbnVzMDtGKC1GMTYlLUYxNiVGXW9GQUZQRktGVS1GMTYmRl1wLUYxNidGOEZLRk5GS0Zgb0Zmbi1GMTYlRltvRktGVS1GMTYmRl1wLUYxNidGW29GS0ZORktGYG9GZm8tRjE2JUY4RktGVS1GMTYlRmJwRktGYG8tRjE2JUY4RktGYnAtRjE2JUZbb0ZLRmJwRk43Iy1JJ01hdHJpeEdGJTYjL0kkJWlkR0YoIjVlbk0icGZwdVklPQ==
Vektor der Freiheitsgrade und zeitliche Ableitung
qq:=<phi,theta,psi>;
qqp:=<phip,thetap,psip>;
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSNxcUdGKFgqJSFHRihGKFtnbCEjJSEhISIkIiQtSSNtaUc2JEYmL0krbW9kdWxlbmFtZUdGKEYkNiNRJiZwaGk7RigtRjE2I1EoJnRoZXRhO0YoLUYxNiNRJiZwc2k7Rig3Iy0mSSdWZWN0b3JHRiU2I0knY29sdW1uR0YoNiMvSSQlaWRHRigiNSEqUkwicGZwdVklPQ==
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSRxcXBHRihYKiUhR0YoRihbZ2whIyUhISEiJCIkLUkjbWlHNiRGJi9JK21vZHVsZW5hbWVHRihGJDYjUSVwaGlwRigtRjE2I1EndGhldGFwRigtRjE2I1ElcHNpcEYoNyMtJkknVmVjdG9yR0YlNiNJJ2NvbHVtbkdGKDYjL0kkJWlkR0YoIjU1ZkwicGZwdVklPQ==
Prozedur zur Berechnung von SS aus RR, qq und qqp:
Calc_SS := proc(RR::Matrix,qq::Vector,qqp::Vector)::Matrix;
local k,qql,N,SSh;
description "Calculate SS(omega) from given rotation matrix RR, vector qq of generalized coordinates and velocities qqp";
qql:=convert(qq,list);
N:=numelems(qql);
SSh:=ZeroMatrix(3,3);
for k from 1 to N do
SSh:=SSh+map(diff,RR,qql[k]).RR^+ *qqp[k];
od;
SSh:=simplify(SSh);
end proc:
Berechnung von SS_0_3:
SS_0_3:=Calc_SS(RR_0_3,qq,qqp);
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSdTU18wXzNHRihYLEkpYW55dGhpbmdHRiZGKEYoW2dsISIlISEhIyoiJCIkLUkjbW5HNiRGJi9JK21vZHVsZW5hbWVHRihGJDYjUSIwRigtSSVtcm93R0YyNiUtRjg2JS1JI21pR0YyNiNRJXBzaXBGKC1JI21vR0YyNiNRMSZJbnZpc2libGVUaW1lcztGKC1GODYlLUY9NiRRJGNvc0YoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1GQTYjUTAmQXBwbHlGdW5jdGlvbjtGKC1JKG1mZW5jZWRHRjI2Iy1GPTYjUSgmdGhldGE7RigtRkE2I1EnJnBsdXM7RigtRj02I1ElcGhpcEYoLUY4NiYtRkE2I1EqJnVtaW51czA7RigtRjg2Jy1GODYlLUY9NiRRJHNpbkYoRklGTEZPRkAtRjg2JUZeb0ZMLUZQNiMtRj02I1EmJnBoaTtGKEZARjwtRkE2I1EoJm1pbnVzO0YoLUY4NiUtRj02I1EndGhldGFwRihGQC1GODYlRkZGTEZjby1GODYmRmduRjpGaG9GWEYwLUY4NiUtRjg2J0Zcb0ZARmBwRkBGPEZoby1GODYlRl1wRkBGYW8tRjg2JUZqbkZVRltwLUY4NiZGZ25GZnBGVUZocEYwNyMtSSdNYXRyaXhHRiU2Iy9JJCVpZEdGKCI1RSQ9OHBmcHVZJT0=
Bestimmung von omega aus SS:
ww:=SS-><SS[3,2],SS[1,3],SS[2,1]>;
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSN3d0dGKGYqNiNJI1NTR0YoRig2JEkpb3BlcmF0b3JHRihJJmFycm93R0YoRigtSSQ8LD5HRic2JSY5JDYkIiIkIiIjJkY4NiQiIiJGOiZGODYkRjtGPkYoRihGKDcjRi4=
Anwendung auf SS_0_3
ww_0_3:=ww(SS_0_3);
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSd3d18wXzNHRihYKkkpYW55dGhpbmdHRiZGKEYoW2dsISMlISEhIiQiJC1JJW1yb3dHNiRGJi9JK21vZHVsZW5hbWVHRihGJDYlLUYxNictRjE2JS1JI21pR0YyNiRRJHNpbkYoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1JI21vR0YyNiNRMCZBcHBseUZ1bmN0aW9uO0YoLUkobWZlbmNlZEdGMjYjLUY7NiNRKCZ0aGV0YTtGKC1GQjYjUTEmSW52aXNpYmxlVGltZXM7RigtRjE2JS1GOzYkUSRjb3NGKEY+RkEtRkY2Iy1GOzYjUSYmcGhpO0YoRkstRjs2I1ElcHNpcEYoLUZCNiNRKCZtaW51cztGKC1GMTYlLUY7NiNRJ3RoZXRhcEYoRkstRjE2JUY6RkFGUy1GMTYlLUYxNidGOEZLRl1vRktGWC1GQjYjUScmcGx1cztGKC1GMTYlRmpuRktGTi1GMTYlLUYxNiVGWEZLLUYxNiVGUEZBRkVGY28tRjs2I1ElcGhpcEYoNyMtJkknVmVjdG9yR0YlNiNJJ2NvbHVtbkdGKDYjL0kkJWlkR0YoIjUpZkBAdGZwdVklPQ==
Geometrische Manipulator Jacobi-Matrix
JJw_0_3:=Jacobian(ww_0_3,convert(qqp,list));
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SShKSndfMF8zR0YoWCxJKWFueXRoaW5nR0YmRihGKFtnbCEiJSEhISMqIiQiJC1JI21uRzYkRiYvSSttb2R1bGVuYW1lR0YoRiQ2I1EiMEYoRjAtRjE2I1EiMUYoLUklbXJvd0dGMjYkLUkjbW9HRjI2I1EqJnVtaW51czA7RigtRjs2JS1JI21pR0YyNiRRJHNpbkYoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1GPjYjUTAmQXBwbHlGdW5jdGlvbjtGKC1JKG1mZW5jZWRHRjI2Iy1GRDYjUSYmcGhpO0YoLUY7NiUtRkQ2JFEkY29zRihGR0ZKRk1GMC1GOzYlRlMtRj42I1ExJkludmlzaWJsZVRpbWVzO0YoLUY7NiVGQ0ZKLUZONiMtRkQ2I1EoJnRoZXRhO0YoLUY7NiVGQUZaRmduLUY7NiVGVUZKRmluNyMtSSdNYXRyaXhHRiU2Iy9JJCVpZEdGKCI1VSQ9QHRmcHVZJT0=
\303\234berpr\303\274fung des Ergebnisses
JJw_0_3.qqp;
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyNYKkkpYW55dGhpbmdHRiZGKEYoW2dsISMlISEhIiQiJC1JJW1yb3dHNiRGJi9JK21vZHVsZW5hbWVHRihGJDYlLUYvNictRi82JS1JI21pR0YwNiRRJHNpbkYoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1JI21vR0YwNiNRMCZBcHBseUZ1bmN0aW9uO0YoLUkobWZlbmNlZEdGMDYjLUY5NiNRKCZ0aGV0YTtGKC1GQDYjUTEmSW52aXNpYmxlVGltZXM7RigtRi82JS1GOTYkUSRjb3NGKEY8Rj8tRkQ2Iy1GOTYjUSYmcGhpO0YoRkktRjk2I1ElcHNpcEYoLUZANiNRKCZtaW51cztGKC1GLzYlLUY5NiNRJ3RoZXRhcEYoRkktRi82JUY4Rj9GUS1GLzYlLUYvNidGNkZJRltvRklGVi1GQDYjUScmcGx1cztGKC1GLzYlRmhuRklGTC1GLzYlLUYvNiVGVkZJLUYvNiVGTkY/RkNGYW8tRjk2I1ElcGhpcEYoNyMtJkknVmVjdG9yR0YlNiNJJ2NvbHVtbkdGKDYjL0kkJWlkR0YoIjVFVDZLKGZwdVklPQ==
Man kann diese Schritte auch direkt zu einer Prozedur zusammenfassen:
Calc_JJw := proc(RR::Matrix,qq::Vector,qqp::Vector)::Matrix;
local k,qql,N,SSh,ww,JJh;
description "Calculate JJ(qq), SS(qq,qqp) and ww(qq,qqp) from given rotation matrix RR, vector qq of generalized coordinates and velocities qqp";
qql:=convert(qq,list);
N:=numelems(qql);
SSh:=ZeroMatrix(3,3);
for k from 1 to N do
SSh:=SSh+map(diff,RR,qql[k]).RR^+ *qqp[k];
od;
SSh:=simplify(SSh);
ww:=<SSh[3,2],SSh[1,3],SSh[2,1]>;
JJh:=Jacobian(ww,convert(qqp,list));
return [JJh, SSh, ww];
end proc:
hh:=Calc_JJw(RR_0_3,qq,qqp):
JJw_0_3:=hh[1];
SS_0_3:=hh[2];
ww_0_3:=hh[3];
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SShKSndfMF8zR0YoWCxJKWFueXRoaW5nR0YmRihGKFtnbCEiJSEhISMqIiQiJC1JI21uRzYkRiYvSSttb2R1bGVuYW1lR0YoRiQ2I1EiMEYoRjAtRjE2I1EiMUYoLUklbXJvd0dGMjYkLUkjbW9HRjI2I1EqJnVtaW51czA7RigtRjs2JS1JI21pR0YyNiRRJHNpbkYoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1GPjYjUTAmQXBwbHlGdW5jdGlvbjtGKC1JKG1mZW5jZWRHRjI2Iy1GRDYjUSYmcGhpO0YoLUY7NiUtRkQ2JFEkY29zRihGR0ZKRk1GMC1GOzYlRlMtRj42I1ExJkludmlzaWJsZVRpbWVzO0YoLUY7NiVGQ0ZKLUZONiMtRkQ2I1EoJnRoZXRhO0YoLUY7NiVGQUZaRmduLUY7NiVGVUZKRmluNyMtSSdNYXRyaXhHRiU2Iy9JJCVpZEdGKCI1OV81SyhmcHVZJT0=
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSdTU18wXzNHRihYLEkpYW55dGhpbmdHRiZGKEYoW2dsISIlISEhIyoiJCIkLUkjbW5HNiRGJi9JK21vZHVsZW5hbWVHRihGJDYjUSIwRigtSSVtcm93R0YyNiUtRjg2JS1JI21pR0YyNiNRJXBzaXBGKC1JI21vR0YyNiNRMSZJbnZpc2libGVUaW1lcztGKC1GODYlLUY9NiRRJGNvc0YoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1GQTYjUTAmQXBwbHlGdW5jdGlvbjtGKC1JKG1mZW5jZWRHRjI2Iy1GPTYjUSgmdGhldGE7RigtRkE2I1EnJnBsdXM7RigtRj02I1ElcGhpcEYoLUY4NiYtRkE2I1EqJnVtaW51czA7RigtRjg2Jy1GODYlLUY9NiRRJHNpbkYoRklGTEZPRkAtRjg2JUZeb0ZMLUZQNiMtRj02I1EmJnBoaTtGKEZARjwtRkE2I1EoJm1pbnVzO0YoLUY4NiUtRj02I1EndGhldGFwRihGQC1GODYlRkZGTEZjby1GODYmRmduRjpGaG9GWEYwLUY4NiUtRjg2J0Zcb0ZARmBwRkBGPEZoby1GODYlRl1wRkBGYW8tRjg2JUZqbkZVRltwLUY4NiZGZ25GZnBGVUZocEYwNyMtSSdNYXRyaXhHRiU2Iy9JJCVpZEdGKCI1SUk2SyhmcHVZJT0=
LV9JLFR5cGVzZXR0aW5nRzYkJSpwcm90ZWN0ZWRHSShfc3lzbGliRzYiSSxtcHJpbnRzbGFzaEdGKDYkNyM+SSd3d18wXzNHRihYKkkpYW55dGhpbmdHRiZGKEYoW2dsISMlISEhIiQiJC1JJW1yb3dHNiRGJi9JK21vZHVsZW5hbWVHRihGJDYlLUYxNictRjE2JS1JI21pR0YyNiRRJHNpbkYoL0kqZm9udHN0eWxlR0YoUSdub3JtYWxGKC1JI21vR0YyNiNRMCZBcHBseUZ1bmN0aW9uO0YoLUkobWZlbmNlZEdGMjYjLUY7NiNRKCZ0aGV0YTtGKC1GQjYjUTEmSW52aXNpYmxlVGltZXM7RigtRjE2JS1GOzYkUSRjb3NGKEY+RkEtRkY2Iy1GOzYjUSYmcGhpO0YoRkstRjs2I1ElcHNpcEYoLUZCNiNRKCZtaW51cztGKC1GMTYlLUY7NiNRJ3RoZXRhcEYoRkstRjE2JUY6RkFGUy1GMTYlLUYxNidGOEZLRl1vRktGWC1GQjYjUScmcGx1cztGKC1GMTYlRmpuRktGTi1GMTYlLUYxNiVGWEZLLUYxNiVGUEZBRkVGY28tRjs2I1ElcGhpcEYoNyMtJkknVmVjdG9yR0YlNiNJJ2NvbHVtbkdGKDYjL0kkJWlkR0YoIjVdSjZLKGZwdVklPQ==