Techniki komputerowe
     w robotyce

Wykład IX

Mathematica – przykłady robotyczne
Dynamika stacjonarnie

Robert Muszyński

Katedra Cybernetyki i Robotyki
Wydział Elektroniki, Fotoniki i Mikrosystemów
Politechnika Wrocławska

Copyright 2009-2026 RM

Dynamika manipulatorów

Podwójne wahadło w 2D

Funkcje pomocnicze – uniwersalne

Systemowe

SetOptions[Plot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
SetOptions[ParametricPlot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
MF=MatrixForm;

Wyliczanie modelu kinematyki

transX[α_,a_] :=(RotationTransform[α,{1,0,0}].TranslationTransform[{a,0,0}])//TransformationMatrix;
transZ[θ_,d_] :=(RotationTransform[θ,{0,0,1}].TranslationTransform[{0,0,d}])//TransformationMatrix;
kinematyki[DHParameters_List:{}]:=    (* @@@ – zastosuj na „drugim” poziomie *)FoldList[Dot,IdentityMatrix[4],transZ[#1,#2].transX[#4,#3]&@@@DHParameters];
jakobianAnalit[k_List,q_List]:=D[k,{q}];

Do wizualizacji

plotPlaski[szkielet_List,tmin_,tmax_,step_]:=Show[Graphics[Table[Line[szkielet],{t,tmin,tmax,step}]],PlotRange->All,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])"}]
animPlaski[szkielet_List,tmin_,tmax_,step_,dodatki_List:Graphics[]]:=Module[{szkielSampled,fullSlad,fullPlotRange}, szkielSampled=Table[szkielet,{t,tmin,tmax,step}];
fullSlad=Line/@szkielSampled[[1;;-1;;10]]; (* do wyznaczenia obszaru *)
fullPlotRange=AbsoluteOptions[Show[Graphics[fullSlad],Sequence@@dodatki,PlotRange->All],PlotRange];
With[{plotRangeLoc=fullPlotRange,dodatkiLoc=dodatki,t0=tmin,tk=tmax,s=step,indMax=Length[szkielSampled]},
DynamicModule[{szkielData=szkielSampled}, (* vvvvvv przeliczenie czasu na indeks *)Animate[With[{ind=Clip[Round[(t-t0)/s]+1,{1,indMax}]},
 Show[Sequence@@dodatkiLoc,Graphics[{Thick,Blue,Line[szkielData[[ind]]],Brown,PointSize[0.02],Point[szkielData[[ind]]]}],plotRangeLoc,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])"}]],{t,t0,tk,s},AnimationRate->1,AnimationRunning->False,SaveDefinitions->True,TrackedSymbols:>{t},Initialization:>(t=t0)]]]]

Definicja manipulatora

Wektor konfiguracyjny, parametry Denevita-Hartenberga

q[t_]={q1[t],q2[t]};
vq=Head/@q[t]
DHPar ={{q1,0,(Subscript[l, 1]),0},{q2,0,(Subscript[l, 2]),0}};   (* teraz na symbolach *)
masy={(Subscript[m, 1]),(Subscript[m, 2])};

Wyliczenie modeli: kinematyki częściowe,jakobiany analityczne częściowe

mKin=DHPar//kinematyki//Simplify;
Clear[kin,jak];                                        (* vvvv bierzemy bez bazy vvvv *)
Evaluate[kin[Pattern[#,_]&/@vq]]=mKin[[All,{1,2},4]]//Rest;  Evaluate[jak[Pattern[#,_]&/@vq]]=jakobianAnalit[kin[vq],vq];
MF/@kin[vq]                                                 (* ^^^^ za to tu wszystkie ^^^^ *)
MF/@jak[vq]

Wyliczenie modelu dynamiki

Energia potencjalna:  E = m g h

ePot=vg masy.kin[q[t]][[All,2]]  (* składowe y kinematyk częściowych *)

Energia kinetyczna:  K = (1/2) m (v^2)

Jak podnieść prędkości do kwadratu???

tmpv={{ax,ay},{bx,by},{cx,cy}};   (* nasze prędkości składowe *)
tmpv.Transpose[tmpv]//MF                           (* wersja 1 z Diagonal *)
%//Diagonal
tmpv tmpv                                                                  (* wersja 2 z Total *)
Total[%,{2}]
%%//Transpose                                                     (* wersja 3 z Plus *) 
Plus@@%

I jak wyliczyć energię???

tmpm={(Subscript[m, a]),(Subscript[m, b]),(Subscript[m, c])};                                     (* nasze masy składowe *)
1/2tmpm.Plus@@Transpose[tmpv^2]  (* wybieramy z powyżej rozwiązanie z Plus, ale... *)
1/2 tmpm tmpv^2
Total[%,∞]                                                      (* skalarnie z Total rządzi *)
%-%%%//Simplify

Mamy więc wyrażenie wyliczające całkowitą energię kinetyczną

Total[1/2 tmpm tmpv^2,∞]                               (* ostateczny wzór :) *)
1/2 tmpm tmpv^2//Flatten//Total            (* albo spłaszczając *)

Skąd wziąć prędkości???

pred=jak[q[t]].q'[t];   (* z jakobianów J(q(t)).q'(t) *)
MF/@pred                              (* albo... *)
pred=D[kin[q[t]],t];   (* z pochodnej kinematyk po czasie (∂_t)kin[q[t]] *)
MF/@pred
%%-%%%%//Simplify

Podsumowując

pred=D[kin[q[t]],t];        (* wyliczamy prędkości z kinematyk *)
wys=kin[vq][[All,2]];     (* „wyliczamy” wysokości „nad poziomem morza” *)
eKin=1/2 masy pred^2//Flatten//Total   (* kinetyczna 1/2 m v^2 *)
ePot= masy vg wys//Flatten//Total                  (* potencjalna m g h *)

La grande finale – la gran żian i cała reszta

Wyliczenie lagranżianu

pred=D[kin[q[t]],t]; 
wys=kin[q[t]][[All,2]];
eKin=1/2 masy pred^2//Flatten//Total;
ePot= masy vg wys//Flatten//Total;
lagr=eKin-ePot

Równania Eulera-Lagrange’a (d/(dt))((∂L)/(∂q'))-((∂L)/(∂q))=u

lstrona=D[D[lagr,{q'[t]}],t]-D[lagr,{q[t]}]//Simplify;
u[t_]={u1[t],u2[t]};                                                     (* wektor sterowania *)
rowDyn=Thread[lstrona==u[t]];
rowDyn//MF

rowDynVec=lstrona==u[t];     (* można też wektorowo *)
MF/@rowDynVec

Needs["VariationalMethods`"]                              (* albo systemowo *)
rowDynSyst=EulerEquations[lagr,q[t],t];
rowDynSyst//MF

Porównajmy

nasze=rowDyn/.(lhs_==rhs_)-> lhs-rhs;  (* przenosimy wszystko na lewo *)
zSyst=rowDynSyst/.(lhs_==rhs_)->lhs-rhs;
nasze+zSyst//Simplify
(prow={x^2+4t+7==z^2+z,y^2+5t==3z^3})//MF (* jak to działa *)
prow/.(lhs_==rhs_):>lhs-rhs //MF

Dodanie sterowań do modelu z „systemu”

rowDynSyst[[All,2]]=u[t];
rowDynSyst//MF

Macierz bezwładności, wektor sił odśrodkowych, Coriolisa i grawitacji 
i cały model Q(q)(Overscript[q, ¨])+C(q,(OverDot[q]))(OverDot[q])+D(q)==τ

mQ=D[eKin,{q'[t]},{q'[t]}]//Simplify;     (* hesjan energii kinetycznej *)
vD=D[ePot,{q[t]}]//Simplify;                            (* gradient energii potencjalnej *)
vC=(lstrona-(mQ.q''[t])-vD)//Simplify;  (* cała reszta *)
mQ//MF
vC//Expand//MF
vD//MF

Sprawdźmy

mQ.q''[t]+vC+vD-lstrona //Simplify
mQ-mQ//Transpose                  (* bo Q symetryczna *)

Symulacje

Numeryczne wartości parametrów, sterowań, warunki początkowe i rozwiązanie

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[m, 1])->2,(Subscript[m, 2])->4,vg->10};
ster=Thread[u[t]->{0,0}];     (* wybieramy sterowania *)
init={q[0]=={0,-π/2},q'[0]=={0,0}};  (* warunki początkowe *)
tmax=20;
rozw=First@NDSolve[{rowDyn/.numParam/.ster,init},vq,{t,0,tmax}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowDyn/.numParam/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;
plotPlaski[towiz,0,tmax,0.05]

animPlaski[towiz,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Det[jak[q[t]][[-1]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Podsumowanie – poczwórne wahadło

Funkcje pomocnicze – uniwersalne

Systemowe

SetOptions[Plot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
SetOptions[ParametricPlot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
MF=MatrixForm;

Wyliczanie modelu kinematyki

transX[α_,a_] :=(RotationTransform[α,{1,0,0}].TranslationTransform[{a,0,0}])//TransformationMatrix;
transZ[θ_,d_] :=(RotationTransform[θ,{0,0,1}].TranslationTransform[{0,0,d}])//TransformationMatrix;
kinematyki[DHParameters_List:{}]:=    (* @@@ – zastosuj na „drugim” poziomie *)FoldList[Dot,IdentityMatrix[4],transZ[#1,#2].transX[#4,#3]&@@@DHParameters];
jakobianAnalit[k_List,q_List]:=D[k,{q}];

Do wizualizacji

plotPlaski[szkielet_List,tmin_,tmax_,step_]:=Show[Graphics[Table[Line[szkielet],{t,tmin,tmax,step}]],PlotRange->All,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])"}]
animPlaski[szkielet_List,tmin_,tmax_,step_,dodatki_List:Graphics[]]:=Module[{szkielSampled,fullSlad,fullPlotRange}, szkielSampled=Table[szkielet,{t,tmin,tmax,step}];
fullSlad=Line/@szkielSampled[[1;;-1;;10]]; (* do wyznaczenia obszaru *)
fullPlotRange=AbsoluteOptions[Show[Graphics[fullSlad],Sequence@@dodatki,PlotRange->All],PlotRange];
With[{plotRangeLoc=fullPlotRange,dodatkiLoc=dodatki,t0=tmin,tk=tmax,s=step,indMax=Length[szkielSampled]},
DynamicModule[{szkielData=szkielSampled}, (* vvvvvv przeliczenie czasu na indeks *)Animate[With[{ind=Clip[Round[(t-t0)/s]+1,{1,indMax}]},
 Show[Sequence@@dodatkiLoc,Graphics[{Thick,Blue,Line[szkielData[[ind]]],Brown,PointSize[0.02],Point[szkielData[[ind]]]}],plotRangeLoc,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])"}]],{t,t0,tk,s},AnimationRate->1,AnimationRunning->False,SaveDefinitions->True,TrackedSymbols:>{t},Initialization:>(t=t0)]]]]

Definicja manipulatora

Wektor konfiguracyjny, parametry Denevita-Hartenberga

q[t_]={q1[t],q2[t],q3[t],q4[t]};
vq=Head/@q[t]
DHPar ={{q1,0,(Subscript[l, 1]),0},{q2,0,(Subscript[l, 2]),0},{q3,0,(Subscript[l, 3]),0},{q4,0,(Subscript[l, 4]),0}};
masy={(Subscript[m, 1]),(Subscript[m, 2]),(Subscript[m, 3]),(Subscript[m, 4])};

Wyliczenie modeli: kinematyki częściowe,jakobiany analityczne częściowe

mKin=DHPar//kinematyki//Simplify;
Clear[kin,jak];
Evaluate[kin[Pattern[#,_]&/@vq]]=mKin[[All,{1,2},4]]//Rest;  Evaluate[jak[Pattern[#,_]&/@vq]]=jakobianAnalit[kin[vq],vq];

Wyliczenie modelu dynamiki

pred=D[kin[q[t]],t]; 
wys=kin[q[t]][[All,2]];
eKin=1/2 masy pred^2//Flatten//Total;
ePot= masy vg wys//Flatten//Total;
lagr=eKin-ePot;

lstrona=D[D[lagr,{q'[t]}],t]-D[lagr,{q[t]}]//Simplify;
u[t_]={u1[t],u2[t],u3[t],u4[t]};
rowDyn=Thread[lstrona==u[t]];

Model z tarciem

tarcie = 20 q'[t];
tmp=lstrona+tarcie;
rowDynTar=Thread[tmp==u[t]];

Macierz bezwładności, wektor sił odśrodkowych, Coriolisa i grawitacji 
i cały model Q(q)(Overscript[q, ¨])+C(q,(OverDot[q]))(OverDot[q])+D(q)==τ

mQ=D[eKin,{q'[t]},{q'[t]}]//Simplify;     (* hesjan energii kinetycznej *)
vD=D[ePot,{q[t]}]//Simplify;                             (* gradient energii potencjalnej *)
vC=(lstrona-(mQ.q''[t])-vD)//Simplify;  (* cała reszta *)

Sprawdźmy

mQ.q''[t]+vC+vD-lstrona //Simplify
mQ-mQ//Transpose//MF

Symulacje

Numeryczne wartości parametrów, sterowań, warunki początkowe i rozwiązanie

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[l, 3])->3,(Subscript[l, 4])->2,(Subscript[m, 1])->2,(Subscript[m, 2])->4,(Subscript[m, 3])->5,(Subscript[m, 4])->2,vg->10};
ster=Thread[u[t]->{0,0,0,0}];     (* wybieramy sterowania *)
init={q[0]=={0,π/2,0,0},q'[0]=={0,0,0,0}};  (* warunki początkowe *)
tmax=20;
rozw=First@NDSolve[{rowDyn/.numParam/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowDyn/.numParam/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;
plotPlaski[towiz,0,tmax,0.02]

animPlaski[towiz,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Posterujmy

Algorytm sterowania przy pełnej znajomości modelu typu obliczanego momentu (algorytm dokładnej linearyzacji) w przestrzeni przegubowej

Q(q)(Overscript[q, ¨])+C(q,(OverDot[q]))(OverDot[q])+D(q)==u   ⟵ model dynamiki
u==Q(q)v+C(q,(OverDot[q]))(OverDot[q])+D(q)          ⟵ linearyzacja
v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))      ⟵ sprzężenie zwrotne

v – nowy wektor sterowania, 
(Subscript[K, d]),(Subscript[K, p]) – macierze wzmocnień

v[t_]={v1[t],v2[t],v3[t],v4[t]};
tmp=mQ.v[t]+vC+vD;                                           (* z definicji algorytmu *)
sterU=lstrona/.Thread[q''[t]->v[t]]; (* podstawiając v za q'' w modelu *)
sterU-tmp//Simplify                                            (* czy takie same??? *)
rowLin=rowDyn/.Thread[u[t]->sterU];   (* linearyzujemy!!! *)

Numeryczne wartości parametrów, sterowań,warunki początkowe i rozwiązanie

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[l, 3])->3,(Subscript[l, 4])->2,(Subscript[m, 1])->2,(Subscript[m, 2])->4,(Subscript[m, 3])->5,(Subscript[m, 4])->2,vg->10};
ster=Thread[v[t]->{0,0,0,0}];     (* wybieramy nowe sterowania *)
init={q[0]=={0,π/4,1,2},q'[0]=={0,0,0,0}};  (* warunki początkowe *)
tmax=20;
rozw=First@NDSolve[{rowLin/.numParam/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.numParam/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.numParam/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;     (* uzupełniamy szkielet o punkt w bazie *)
plotPlaski[towiz,0,tmax,0.05]

animPlaski[towiz,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Zapinamy pętlę sprzężenia      v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

Kp=3IdentityMatrix[4];
Kd=1IdentityMatrix[4];
qd0[t_]={π/4,0,1,2};
qd1[t_]={π/4,0,1,2+Sin[t]};
qd2[t_]={π/4,0,t/4,2+Sin[t]};
qd[t_]=qd0[t];
petla=qd''[t]-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t]);
ster=Thread[v[t]->petla];

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[l, 3])->3,(Subscript[l, 4])->2,(Subscript[m, 1])->2,(Subscript[m, 2])->4,(Subscript[m, 3])->5,(Subscript[m, 4])->2,vg->10};
init={q[0]=={0,π/4,1,2},q'[0]=={0,-1,0,0}};
tmax=20;
rozw=First@NDSolve[{rowLin/.numParam/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.numParam/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[Norm[(q[t]-qd[t])/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","e"},PlotLegends->MapIndexed[Subscript[e,#2[[1]]]&,vq]]
Plot[Evaluate[lstrona/.numParam/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;
plotPlaski[towiz,0,tmax,0.05]

animPlaski[towiz,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Odsprzęganie wejściowo-wyjściowe w przestrzeni zadaniowej

Algorytm sterowania przy pełnej znajomości modelu typu obliczanego momentu (algorytm dokładnej linearyzacji) w przestrzeni zadaniowej

Funkcje pomocnicze – uniwersalne

Systemowe

SetOptions[Plot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
SetOptions[ParametricPlot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
MF=MatrixForm;

Wyliczanie modelu kinematyki

transX[α_,a_] :=(RotationTransform[α,{1,0,0}].TranslationTransform[{a,0,0}])//TransformationMatrix;
transZ[θ_,d_] :=(RotationTransform[θ,{0,0,1}].TranslationTransform[{0,0,d}])//TransformationMatrix;
kinematyki[DHParameters_List:{}]:=    (* @@@ – zastosuj na „drugim” poziomie *)FoldList[Dot,IdentityMatrix[4],transZ[#1,#2].transX[#4,#3]&@@@DHParameters];
jakobianAnalit[k_List,q_List]:=D[k,{q}];

Odwrotności (zwykła: (A^(-1)),pseudo: (((A^T)((AA^T)))^(-1)),odporna – SRI: (((A^T)((AA^T)+εI))^(-1)))

mInv[mA_]:=Inverse[mA];
mInvPseudo[mA_]:=Transpose[mA].Inverse[mA.Transpose[mA]];
mInvSyst[mA_]:=PseudoInverse[mA];   (* systemowa - długo się liczy *)
mInvSRI[mA_]:=Transpose[mA].Inverse[mA.Transpose[mA]+ε IdentityMatrix[mA//Length]];ε=(10^(-3));

Do wizualizacji

plotPlaski[szkielet_List,tmin_,tmax_,step_]:=Show[Graphics[Table[Line[szkielet],{t,tmin,tmax,step}]],PlotRange->All,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])"}]
animPlaski[szkielet_List,tmin_,tmax_,step_,dodatki_List:Graphics[]]:=Module[{szkielSampled,fullSlad,fullPlotRange},
szkielSampled=Table[szkielet,{t,tmin,tmax,step}];
fullSlad=Line/@szkielSampled[[1;;-1;;10]]; (* do wyznaczenia obszaru *)
fullPlotRange=AbsoluteOptions[Show[Graphics[fullSlad],Sequence@@dodatki,PlotRange->All],PlotRange];
With[{plotRangeLoc=fullPlotRange,dodatkiLoc=dodatki,t0=tmin,tk=tmax,s=step,indMax=Length[szkielSampled]},
DynamicModule[{szkielData=szkielSampled}, (* vvvvvv przeliczenie czasu na indeks *)Animate[With[{ind=Clip[Round[(t-t0)/s]+1,{1,indMax}]},
 Show[Sequence@@dodatkiLoc,Graphics[{Thick,Blue,Line[szkielData[[ind]]],Brown,PointSize[0.02],Point[szkielData[[ind]]]}],plotRangeLoc,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])"}]],{t,t0,tk,s},AnimationRate->1,AnimationRunning->False,SaveDefinitions->True,TrackedSymbols:>{t},Initialization:>(t=t0)]]]]

Podwójne wahadło

Definicja manipulatora

q[t_]={q1[t],q2[t]};
u[t_]={u1[t],u2[t]};
vq=Head/@q[t];
DHPar ={{q1,0,(Subscript[l, 1]),0},{q2,0,(Subscript[l, 2]),0}};
masy={(Subscript[m, 1]),(Subscript[m, 2])};
mKin=DHPar//kinematyki//Simplify;  (* model kinematyki *)
Clear[kin,jak];
Evaluate[kin[Pattern[#,_]&/@vq]]=mKin[[All,{1,2},4]]//Rest;  Evaluate[jak[Pattern[#,_]&/@vq]]=jakobianAnalit[kin[vq],vq];
pred=D[kin[q[t]],t];                                    (* model dynamiki *)
wys=kin[q[t]][[All,2]];
eKin=1/2 masy pred^2//Flatten//Total;
ePot= masy vg wys//Flatten//Total;
lagr=eKin-ePot;
lstrona=D[D[lagr,{q'[t]}],t]-D[lagr,{q[t]}]//Simplify;
rowDyn=Thread[lstrona==u[t]];
mQ=D[eKin,{q'[t]},{q'[t]}]//Simplify;
vD=D[ePot,{q[t]}]//Simplify;
vC=(lstrona-(mQ.q''[t])-vD)//Simplify;

Algorytm sterowania

y==k(q)       ⟵ model kinematyki
Q(q)(Overscript[q, ¨])+C(q,(OverDot[q]))(OverDot[q])+D(q)==u      ⟵ model dynamiki
J=(∂_q)k                                              ⟵ jakobian
u==((QJ)^(-1))(v-(OverDot[J])(OverDot[q]))+Cq+D          ⟵ linearyzacja  
v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))       ⟵ sprzężenie zwrotne

v – nowy wektor sterowania 
(Subscript[K, d]),(Subscript[K, p]) – macierze wzmocnień

v[t_]={v1[t],v2[t]};
mJ[t_]=jak[q[t]][[-1]];
sterU=mQ.mInv[mJ[t]].(v[t]-mJ'[t].q'[t])+vC+vD;
rowLin=rowDyn/.Thread[u[t]->sterU];   (* linearyzujemy!!! *)
sterUSRI=mQ.mInvSRI[mJ[t]].(v[t]-mJ'[t].q'[t])+vC+vD;  (*  SRI *)
rowLinSRI=rowDyn/.Thread[u[t]->sterUSRI];

Symulacje

Numeryczne wartości parametrów, sterowań,warunki początkowe i rozwiązanie

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[m, 1])->2,(Subscript[m, 2])->4,vg->10};
ster0=Thread[v[t]->{0,0}];         (* wybieramy nowe sterowania *)
ster1=Thread[v[t]->{-0.1,0}];
ster2=Thread[v[t]->{-0.1Sin[t],-Cos[t]}];
ster=ster0;
init={q[0]=={0,π/4},q'[0]=={0,0}};  (* warunki początkowe *)
tmax=20;
rozw=First@NDSolve[{rowLin/.numParam/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.numParam/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.rozw/.numParam],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajy=ParametricPlot[Evaluate[kin[q[t]][[-1]]/.numParam/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;     (* uzupełniamy szkielet o punkt w bazie *)
plotPlaski[towiz,0,tmax,0.05]

animPlaski[towiz,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Det[jak[q[t]][[-1]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Zapinamy pętlę sprzężenia      v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

Kp=3IdentityMatrix[2];
Kd=1IdentityMatrix[2];
yd0[t_]={7 Cos[t],6 Sin[t]+2}; 
yd1[t_]={2 Cos[t]+8,4 Sin[t]};                          (* osobliwa *)
yd2[t_]={2 Cos[2t+π/2],1 Sin[ t]+5};         (* kokarda *)
yd3[t_]={3+1/5t Cos[t],3+ 1/5t Sin[t]}; (* spirala Archimedesa *)
yd4[t_]={3+Cos[t]^3,3+ Sin[t]^3};              (* asteroida *)

yd[t_]=yd0[t];                         (* trajektoria zadana *)
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,PlotRange->All]
petla=yd''[t]-Kd.((∂_t)kin[q[t]][[-1]]-yd'[t])-Kp.(kin[q[t]][[-1]]-yd[t]);
ster=Thread[v[t]->petla];

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[l, 3])->3,(Subscript[l, 4])->2,(Subscript[m, 1])->2,(Subscript[m, 2])->4,(Subscript[m, 3])->5,(Subscript[m, 4])->2,vg->10};
init={q[0]=={0,π/4},q'[0]=={0,0}};
tmax=30;
rozw=First@NDSolve[{rowLin/.ster/.numParam,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.ster/.numParam]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[Norm[(kin[q[t]][[-1]]-yd[t])/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","e"}]
Plot[Evaluate[lstrona/.numParam/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,AxesLabel->{"x","y"}]
trajy=ParametricPlot[Evaluate[kin[q[t]][[-1]]/.numParam/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;
slad=plotPlaski[towiz,0,tmax,0.1];
Show[trajy,trajzad,slad,PlotRange->All]

animPlaski[towiz,0,tmax,0.05,{trajy,trajzad}]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Det[jak[q[t]][[-1]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Potrójne wahadło

Definicja manipulatora

q[t_]={q1[t],q2[t],q3[t]};
u[t_]={u1[t],u2[t],u3[t]};
vq=Head/@q[t];
DHPar ={{q1,0,(Subscript[l, 1]),0},{q2,0,(Subscript[l, 2]),0},{q3,0,(Subscript[l, 3]),0}};
masy={(Subscript[m, 1]),(Subscript[m, 2]),(Subscript[m, 3])};
mKin=DHPar//kinematyki//Simplify;  (* model kinematyki *)
Clear[kin,jak];
Evaluate[kin[Pattern[#,_]&/@vq]]=mKin[[All,{1,2},4]]//Rest;  Evaluate[jak[Pattern[#,_]&/@vq]]=jakobianAnalit[kin[vq],vq];
pred=D[kin[q[t]],t];                                    (* model dynamiki *)
wys=kin[q[t]][[All,2]];
eKin=1/2 masy pred^2//Flatten//Total;
ePot= masy vg wys//Flatten//Total;
lagr=eKin-ePot;
lstrona=D[D[lagr,{q'[t]}],t]-D[lagr,{q[t]}]//Simplify;
rowDyn=Thread[lstrona==u[t]];
mQ=D[eKin,{q'[t]},{q'[t]}]//Simplify;
vD=D[ePot,{q[t]}]//Simplify;
vC=(lstrona-(mQ.q''[t])-vD)//Simplify;

Algorytm sterowania

u==((QJ)^(-1))(v-(OverDot[J])(OverDot[q]))+Cq+D                ⟵ mieliśmy  
u==((((JQ^(-1))))^(#))(v-(OverDot[J])(OverDot[q])+(JQ^(-1))(Cq+D))  ⟵ mamy  
v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))          ⟵ sprzężenie zwrotne

v – nowy wektor sterowania 
(A^((#)^□)) – pseudoodwrotność
(Subscript[K, d]),(Subscript[K, p]) – macierze wzmocnień

v[t_]={v1[t],v2[t]};
mJ[t_]=jak[q[t]][[-1]];
mD=mJ[t].Inverse[mQ];
mP=-mJ'[t].q'[t]+mJ[t].Inverse[mQ].(vC+vD);
{tpseudo,sterU}=Timing[mInvPseudo[mD].(v[t]+mP)];
rowLin=rowDyn/.Thread[u[t]->sterU];   (* linearyzujemy!!! *)

tpseudo

tsyst

(* z rzutowaniem na przestrzeń zerową
                                                            vvvvvvvvvvvvvvvv *)
u==((((JQ^(-1))))^(#))(v-(OverDot[J])(OverDot[q])+(JQ^(-1))(Cq+D))+(I-((((JQ^(-1))))^(#))((JQ^(-1))))(Subscript[v, 0])
(Subscript[v, 0])==(Subscript[k, 0])∇w(q)-(Subscript[k, t])(OverDot[q])   ⟵ funkcja celu w z tłumieniem

v – nowy wektor sterowania 
(Subscript[v, 0]) – sterowanie w przestrzeni zerowej
w(q)  – funkcja celu
(Subscript[k, 0]), (Subscript[k, t])  – wzmocnienie, tłumienie

v[t_]={v1[t],v2[t]};
mJ[t_]=jak[q[t]][[-1]];
mD=mJ[t].Inverse[mQ];
mP=-mJ'[t].q'[t]+mJ[t].Inverse[mQ].(vC+vD);
mZero=(u[t]//Length//IdentityMatrix)-mInvPseudo[mD].mD;
sterU=mInvPseudo[mD].(v[t]+mP);
sterUZero=sterU-kt mZero .q'[t];              (* tylko tłumienie *)
rowLinZero=rowDyn/.Thread[u[t]->sterUZero];
fw=Sqrt[Det[mJ[t].Transpose[mJ[t]]]]; (* plus odpychanie od osobliwości *)
sterUZeroOso=sterU+ mZero .(k0 D[fw,{q[t]}]-kt q'[t]);
rowLinZeroOso=rowDyn/.Thread[u[t]->sterUZeroOso];
LeafCount[rowLin]
LeafCount[rowLinZero]
LeafCount[rowLinZeroOso]

Symulacje

Numeryczne wartości parametrów, sterowań,warunki początkowe i rozwiązanie

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[l, 3])->3,(Subscript[m, 1])->2,(Subscript[m, 2])->4,(Subscript[m, 3])->2,vg->10,k0->50,kt->50};
ster0=Thread[v[t]->{0,0}];                        (* wybieramy nowe sterowania *)
ster1=Thread[v[t]->{-0.1,0}];
ster2=Thread[v[t]->{-0.1Sin[t],-Cos[t]}];
ster=ster0;
init={q[0]=={0,π/4,0},q'[0]=={0,0,0}}; (* warunki początkowe *)
tmax=20;
rozw=First@NDSolve[{rowLin/.numParam/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLinZeroOso/.numParam/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.numParam/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajy=ParametricPlot[Evaluate[kin[q[t]][[-1]]/.numParam/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;     (* uzupełniamy szkielet o punkt w bazie *)
plotPlaski[towiz,0,tmax,0.1]

animPlaski[towiz,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Zapinamy pętlę sprzężenia      v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

Kp=3IdentityMatrix[2];
Kd=1IdentityMatrix[2];

ydp[t_]={5,5}; 
yd0[t_]={7 Cos[t],6 Sin[t]+2}; 
yd1[t_]={2 Cos[t]+11,4 Sin[t]};                          (* osobliwa *)
yd2[t_]={2 Cos[2t+π/2],1 Sin[ t]+5};         (* kokarda *)
yd3[t_]={3+1/5t Cos[t],3+ 1/5t Sin[t]}; (* spirala Archimedesa *)
yd4[t_]={3+Cos[t]^3,3+ Sin[t]^3};              (* asteroida *)

yd[t_]=yd0[t];                         (* trajektoria zadana *)
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,PlotRange->All]
petla=yd''[t]-Kd.((∂_t)kin[q[t]][[-1]]-yd'[t])-Kp.(kin[q[t]][[-1]]-yd[t]);
ster=Thread[v[t]->petla];

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[l, 3])->3,(Subscript[l, 4])->2,(Subscript[m, 1])->2,(Subscript[m, 2])->4,(Subscript[m, 3])->5,(Subscript[m, 4])->2,vg->10,k0->50,kt->200};
init={q[0]=={0,π/4,0},q'[0]=={0,0,0}};
tmax=30;
rozw=First@NDSolve[{rowLin/.ster/.numParam,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.ster/.numParam]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[Norm[(kin[q[t]][[-1]]-yd[t])/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","e"}]
Plot[Evaluate[lstrona/.numParam/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,AxesLabel->{"x","y"}]
trajy=ParametricPlot[Evaluate[kin[q[t]][[-1]]/.numParam/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;
slad=plotPlaski[towiz,0,tmax,0.1];
Show[trajy,trajzad,slad,PlotRange->All]

animPlaski[towiz,0,tmax,0.05,{trajy,trajzad}]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Wyznacznik,przyspieszenia,prędkości,błąd w wybranych chwilach

przedzialy={{0,0.001},{2π-0.01,2π+0.01},{4π-0.001,4π+0.001},
{4.65,4.75},{0.1,5},{5,tmax},
{20,tmax-0.1}};
{tp,tk}=przedzialy[[1]];
Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]] 
Plot[Evaluate[q'[t]/.rozw],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]   
Plot[Evaluate[Norm[(kin[q[t]][[-1]]-yd[t])/.numParam/.rozw]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","e"}]

Poczwórne wahadło

Definicja manipulatora

q[t_]={q1[t],q2[t],q3[t],q4[t]};
u[t_]={u1[t],u2[t],u3[t],u4[t]};
vq=Head/@q[t];
DHPar ={{q1,0,(Subscript[l, 1]),0},{q2,0,(Subscript[l, 2]),0},{q3,0,(Subscript[l, 3]),0},{q4,0,(Subscript[l, 4]),0}};
masy={(Subscript[m, 1]),(Subscript[m, 2]),(Subscript[m, 3]),(Subscript[m, 4])};
mKin=DHPar//kinematyki//Simplify;  (* model kinematyki *)
Clear[kin,jak];
Evaluate[kin[Pattern[#,_]&/@vq]]=mKin[[All,{1,2},4]]//Rest;  Evaluate[jak[Pattern[#,_]&/@vq]]=jakobianAnalit[kin[vq],vq];
pred=D[kin[q[t]],t];                                    (* model dynamiki *)
wys=kin[q[t]][[All,2]];
eKin=1/2 masy pred^2//Flatten//Total;
ePot= masy vg wys//Flatten//Total;
lagr=eKin-ePot;
lstrona=D[D[lagr,{q'[t]}],t]-D[lagr,{q[t]}]//Simplify;
rowDyn=Thread[lstrona==u[t]];
mQ=D[eKin,{q'[t]},{q'[t]}]//Simplify;
vD=D[ePot,{q[t]}]//Simplify;
vC=(lstrona-(mQ.q''[t])-vD)//Simplify;

Algorytm sterowania

u==((QJ)^(-1))(v-(OverDot[J])(OverDot[q]))+Cq+D                ⟵ mieliśmy  
u==((((JQ^(-1))))^(#))(v-(OverDot[J])(OverDot[q])+(JQ^(-1))(Cq+D))  ⟵ mamy  
v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))          ⟵ sprzężenie zwrotne

v – nowy wektor sterowania 
(A^((#)^□)) – pseudoodwrotność
(Subscript[K, d]),(Subscript[K, p]) – macierze wzmocnień

v[t_]={v1[t],v2[t]};
mJ[t_]=jak[q[t]][[-1]];
mD=mJ[t].Inverse[mQ];
mP=-mJ'[t].q'[t]+mJ[t].Inverse[mQ].(vC+vD);
{tpseudo,sterU}=Timing[mInvPseudo[mD].(v[t]+mP)];
rowLin=rowDyn/.Thread[u[t]->sterU];   (* linearyzujemy!!! *)
tpseudo

(* z rzutowaniem na przestrzeń zerową
                                                                vvvvvvvvvvvvvvvv *)
u==((((JQ^(-1))))^(#))(v-(OverDot[J])(OverDot[q])+(JQ^(-1))(Cq+D))+(I-((((JQ^(-1))))^(#))((JQ^(-1))))(Subscript[v, 0])
(Subscript[v, 0])==(Subscript[k, 0])∇w(q)-(Subscript[k, t])(OverDot[q])   ⟵ funkcja celu w z tłumieniem

v – nowy wektor sterowania 
(Subscript[v, 0]) – sterowanie w przestrzeni zerowej
w(q)  – funkcja celu
(Subscript[k, 0]), (Subscript[k, t])  – wzmocnienie, tłumienie

v[t_]={v1[t],v2[t]};
mJ[t_]=jak[q[t]][[-1]];
mD=mJ[t].Inverse[mQ];
mDInv=mInvPseudo[mD];
mP=-mJ'[t].q'[t]+mJ[t].Inverse[mQ].(vC+vD);
mZero=(u[t]//Length//IdentityMatrix)-mDInv.mD;
sterU=mDInv.(v[t]+mP);
sterUZero=sterU-kt mZero .q'[t];              (* tylko tłumienie *)
rowLinZero=rowDyn/.Thread[u[t]->sterUZero];
fw=Sqrt[Det[mJ[t].Transpose[mJ[t]]]]; (* plus odpychanie od osobliwości *)
sterUZeroOso=sterU+ mZero .(k0 D[fw,{q[t]}]-kt q'[t]);
rowLinZeroOso=rowDyn/.Thread[u[t]->sterUZeroOso];
LeafCount[rowLin]
LeafCount[rowLinZero]
LeafCount[rowLinZeroOso]

Symulacje

Numeryczne wartości parametrów, sterowań,warunki początkowe i rozwiązanie

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[l, 3])->3,(Subscript[l, 4])->2,(Subscript[m, 1])->2,(Subscript[m, 2])->4,(Subscript[m, 3])->5,(Subscript[m, 4])->2,vg->10,k0->50,kt->200};
ster0=Thread[v[t]->{0,0}];         (* wybieramy nowe sterowania *)
ster1=Thread[v[t]->{-0.1,0}];
ster2=Thread[v[t]->{-0.1Sin[t],-Cos[t]}];
ster=ster2;
init={q[0]=={0,π/4,0,0},q'[0]=={0,0,0,0}};  (* warunki początkowe *)
tmax=20;
{trozw,rozw}=Timing[First@NDSolve[{rowLin/.numParam/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}]];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.numParam/.ster]
trozw

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.numParam/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajy=ParametricPlot[Evaluate[kin[q[t]][[-1]]/.numParam/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;     (* uzupełniamy szkielet o punkt w bazie *)
plotPlaski[towiz,0,tmax,0.1]

animPlaski[towiz,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Zapinamy pętlę sprzężenia      v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

tmax=20;
Kp=3IdentityMatrix[2];
Kd=1IdentityMatrix[2];

ydp[t_]={5,5}; 
yd0[t_]={7 Cos[t],6 Sin[t]+2}; 
yd1[t_]={2 Cos[t]+11,4 Sin[t]};                          (* osobliwa *)
yd2[t_]={2 Cos[2t+π/2],1 Sin[ t]+5};         (* kokarda *)
yd3[t_]={3+1/5t Cos[t],3+ 1/5t Sin[t]}; (* spirala Archimedesa *)
yd4[t_]={3+Cos[t]^3,3+ Sin[t]^3};              (* asteroida *)

yd[t_]=yd0[t];                         (* trajektoria zadana *)
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,PlotRange->All]
petla=yd''[t]-Kd.((∂_t)kin[q[t]][[-1]]-yd'[t])-Kp.(kin[q[t]][[-1]]-yd[t]);
ster=Thread[v[t]->petla];

numParam={(Subscript[l, 1])->6,(Subscript[l, 2])->4,(Subscript[l, 3])->3,(Subscript[l, 4])->2,(Subscript[m, 1])->2,(Subscript[m, 2])->4,(Subscript[m, 3])->5,(Subscript[m, 4])->2,vg->10,k0->50,kt->200};
init={q[0]=={0,π/4,0,0},q'[0]=={0,0,0,0}};
tmax=30;
{trozw,rozw}=Timing[First@NDSolve[{rowLinZeroOso/.ster/.numParam,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}]];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLinZeroOso/.ster/.numParam]
trozw

(* tylko z tłumieniem *)

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[Norm[(kin[q[t]][[-1]]-yd[t])/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","e"}]
Plot[Evaluate[lstrona/.numParam/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,AxesLabel->{"x","y"}]
trajy=ParametricPlot[Evaluate[kin[q[t]][[-1]]/.numParam/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
towiz=kin[q[t]]~Prepend~Table[0,kin[vq][[1]]//Length]/.rozw/.numParam;
slad=plotPlaski[towiz,0,tmax,0.1];
Show[trajy,trajzad,slad,PlotRange->All]

animPlaski[towiz,0,tmax,0.05,{trajy,trajzad}]

(* tylko z tłumieniem *)

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Wyznacznik,przyspieszenia,prędkości,błąd w wybranych chwilach

przedzialy={{0,0.00001},{2π-0.01,2π+0.01},{4π-0.001,4π+0.001},
{4.65,4.75},{1,5},{5,tmax},
{5,tmax-0.1}};
{tp,tk}=przedzialy[[1]];
Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]] 
Plot[Evaluate[q'[t]/.rozw],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]   
Plot[Evaluate[Norm[(kin[q[t]][[-1]]-yd[t])/.numParam/.rozw]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","e"}]

Zróbmy to w 3D

Funkcje pomocnicze – uniwersalne

Systemowe

SetOptions[Plot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
SetOptions[ParametricPlot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
MF=MatrixForm;

Wyliczanie modelu kinematyki

transX[α_,a_] :=(RotationTransform[α,{1,0,0}].TranslationTransform[{a,0,0}])//TransformationMatrix;
transZ[θ_,d_] :=(RotationTransform[θ,{0,0,1}].TranslationTransform[{0,0,d}])//TransformationMatrix;
kinematyki[DHParameters_List:{}]:=    (* @@@ – zastosuj na „drugim” poziomie *)FoldList[Dot,IdentityMatrix[4],transZ[#1,#2].transX[#4,#3]&@@@DHParameters];
kinematykiFull[DHParameters_List: {}]:=  (* z punktami pośrednimi *)Module[{mPosrednie},mPosrednie=Flatten[{transZ[#1,#2],transX[#4,#3]}&@@@DHParameters,1];
FoldList[Dot,IdentityMatrix[4],mPosrednie]]
jakobianAnalit[k_List,q_List]:=D[k,{q}];

Odwrotności (zwykła: (A^(-1)),pseudo: (((A^T)((AA^T)))^(-1)),odporna – SRI: (((A^T)((AA^T)+εI))^(-1)))

mInv[mA_]:=Inverse[mA];
mInvPseudo[mA_]:=Transpose[mA].Inverse[mA.Transpose[mA]];
mInvSyst[mA_]:=PseudoInverse[mA];   (* systemowa - długo się liczy *)
mInvSRI[mA_]:=Transpose[mA].Inverse[mA.Transpose[mA]+ε IdentityMatrix[mA//Length]];ε=(10^(-3));

Do wizualizacji

plotManip[szkielet_List,tmin_,tmax_,step_]:= (* usunięte co 2. kropki *)Show[Graphics3D[Table[{Line[szkielet],Brown,PointSize[0.02],Point[szkielet[[1;;-1;;2]]] },{t,tmin,tmax,step}]],PlotRange->All,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])","(Subscript[y, 3])"}]
animManip[szkielet_List,tmin_,tmax_,step_,dodatki_List:Graphics[],rozm_:400]:=Module[{szkielSampled,fullSlad,fullPlotRange}, szkielSampled=Table[szkielet,{t,tmin,tmax,step}];fullSlad=Line/@szkielSampled[[1;;-1;;10]]; (* do wyznaczenia obszaru *)
fullPlotRange=AbsoluteOptions[Show[Graphics3D[fullSlad],Sequence@@dodatki,PlotRange->All],PlotRange];
With[{plotRangeLoc=fullPlotRange,dodatkiLoc=dodatki,rozmLoc=rozm,t0=tmin,tk=tmax,s=step,indMax=Length[szkielSampled]},
DynamicModule[{szkielData=szkielSampled},(* vvvvvv przeliczenie czasu na indeks *)Animate[With[{ind=Clip[Round[(t-t0)/s]+1,{1,indMax}]},Show[Sequence@@dodatkiLoc,Graphics3D[{Thick,Blue,Line[szkielData[[ind]]],Brown,PointSize[0.02],Point[szkielData[[ind,1;;-1;;2]]]}],plotRangeLoc,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])","(Subscript[y, 3])"},ImageSize->rozmLoc]],{t,t0,tk,s},AnimationRate->1,AnimationRunning->False,SaveDefinitions->True,TrackedSymbols:>{t},Initialization:>(t=t0)]]]]

Podwójne dwuwahadło

Definicja manipulatora

Wektor konfiguracyjny, parametry Denevita-Hartenberga

(* numerycznie od początku *)
numParam={(Subscript[d, 1])->2.,(Subscript[a, 1])->6.,(Subscript[d, 2])->3.,(Subscript[a, 2])->4.,(Subscript[d, 3])->1.,(Subscript[a, 3])->3.,(Subscript[d, 4])->1.,(Subscript[a, 4])->2.,(Subscript[m, 1])->2.,(Subscript[m, 2])->4.,(Subscript[m, 3])->5.,(Subscript[m, 4])->2.,vg->10.};
q[t_]={q1[t],q2[t],q3[t],q4[t]};
vq=Head/@q[t]
DHPar ={{q1,(Subscript[d, 1]),(Subscript[a, 1]),0},{q2,(Subscript[d, 2]),(Subscript[a, 2]),π/2},{q3,(Subscript[d, 3]),(Subscript[a, 3]),0},{q4,(Subscript[d, 4]),(Subscript[a, 4]),0}};
masy={(Subscript[m, 1]),(Subscript[m, 2]),(Subscript[m, 3]),(Subscript[m, 4])};
DHParT ={{q1,0,(Subscript[a, 1]),0},{π/2,0,q2,0},{q3,0,(Subscript[a, 3]),0},{π/2,0,q4,0}};

Wyliczenie modeli: kinematyki częściowe,jakobian analityczny

mKin=DHPar/.numParam//kinematyki//Simplify;
mKinFull=DHPar/.numParam//kinematykiFull//Simplify;
Clear[kin,kinFull,jak];
Evaluate[kin[Pattern[#,_]&/@vq]]=mKin[[All,{1,2,3},4]]//Rest;   
Evaluate[kinFull[Pattern[#,_]&/@vq]]=mKinFull[[All,{1,2,3},4]];   
Evaluate[jak[Pattern[#,_]&/@vq]]=jakobianAnalit[kin[vq][[-1]],vq];
MF/@kin[vq]
plotManip[kinFull[{0,0,0,0}],0,0,0.2]
plotManip[kinFull[{0,π/4,π/3,π/2}],0,0,0.2]

Wyliczenie modelu dynamiki

pred=D[kin[q[t]],t]; 
wys=kin[q[t]][[All,3]];
eKin=1/2 masy pred^2//Flatten//Total;
ePot= masy vg wys//Flatten//Total;
lagr=(eKin-ePot)/.numParam//Simplify;
lstrona=D[D[lagr,{q'[t]}],t]-D[lagr,{q[t]}];
u[t_]={u1[t],u2[t],u3[t],u4[t]};
rowDyn=Thread[lstrona==u[t]];

Model z tarciem

tarcie = 20 q'[t];
tmp=lstrona+tarcie;
rowDynTar=Thread[tmp==u[t]];

Macierz bezwładności, wektor sił odśrodkowych, Coriolisa i grawitacji 
i cały model Q(q)(Overscript[q, ¨])+C(q,(OverDot[q]))(OverDot[q])+D(q)==τ

mQ=D[eKin,{q'[t]},{q'[t]}]/.numParam//Simplify;     (* hesjan energii kinetycznej *)
vD=D[ePot,{q[t]}]/.numParam//Simplify;                             (* gradient energii potencjalnej *)
vC=(lstrona-(mQ.q''[t])-vD)//Simplify;  (* cała reszta *)

Sprawdźmy

mQ.q''[t]+vC+vD-lstrona //Simplify
mQ-mQ//Simplify//Transpose//MF

Układ swobodny

Numeryczne wartości parametrów, sterowań, warunki początkowe i rozwiązanie

ster=Thread[u[t]->{0,0,0,0}];     (* wybieramy sterowania *)
init={q[0]=={0,π/2,0,0},q'[0]=={0,0,0,0}};  (* warunki początkowe *)
tmax=20;
rozw=First@NDSolve[{rowDyn/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowDyn/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
plotManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

animManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Odsprzęganie wejściowo-wyjściowe w przestrzeni przegubowej

Algorytm sterowania przy pełnej znajomości modelu typu obliczanego momentu (algorytm dokładnej linearyzacji) w przestrzeni przegubowej

Q(q)(Overscript[q, ¨])+C(q,(OverDot[q]))(OverDot[q])+D(q)==u   ⟵ model dynamiki
u==Q(q)v+C(q,(OverDot[q]))(OverDot[q])+D(q)          ⟵ linearyzacja
v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))      ⟵ sprzężenie zwrotne

v – nowy wektor sterowania, 
(Subscript[K, d]),(Subscript[K, p]) – macierze wzmocnień

v[t_]={v1[t],v2[t],v3[t],v4[t]};
sterU=lstrona/.Thread[q''[t]->v[t]]; (* podstawiamy v za q'' w modelu *)
rowLin=rowDyn/.Thread[u[t]->sterU];   (* linearyzujemy!!! *)

Numeryczne wartości sterowań,warunki początkowe i rozwiązanie

ster=Thread[v[t]->{0,0,0,0}];     (* wybieramy nowe sterowania *)
init={q[0]=={0,π/4,1,2},q'[0]=={0,1,0,0}};  (* warunki początkowe *)
tmax=20;
rozw=First@NDSolve[{rowLin/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
plotManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

animManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Zapinamy pętlę sprzężenia      v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

Kp=3IdentityMatrix[4];
Kd=1IdentityMatrix[4];
qd0[t_]={π/4,0,0,0};
qd1[t_]={π/4,0,1,2+Sin[t]};
qd2[t_]={π/4,0,t/4,2+Sin[t]};
qd[t_]=qd0[t];
petla=qd''[t]-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t]);
ster=Thread[v[t]->petla];

init={q[0]=={0,π/4,1,2},q'[0]=={0,-1,0,0}};
tmax=20;
rozw=First@NDSolve[{rowLin/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.ster]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[Norm[(q[t]-qd[t])/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","e"},PlotLegends->MapIndexed[Subscript[e,#2[[1]]]&,vq]]
Plot[Evaluate[lstrona/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
plotManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

animManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det (JJ^t)"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Odsprzęganie wejściowo-wyjściowe w przestrzeni zadaniowej

Algorytm sterowania

u==((QJ)^(-1))(v-(OverDot[J])(OverDot[q]))+Cq+D                ⟵ nieredundantny  
u==((((JQ^(-1))))^(#))(v-(OverDot[J])(OverDot[q])+(JQ^(-1))(Cq+D))  ⟵ redundantny  
v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))          ⟵ sprzężenie zwrotne

v – nowy wektor sterowania 
(A^((#)^□)) – pseudoodwrotność
(Subscript[K, d]),(Subscript[K, p]) – macierze wzmocnień

Nieredundantny

Wybieramy współrzędne położenie plus orientacja względem osi Y – pochylenie

Clear[kinNR,jakNR];  
Evaluate[kinNR[Pattern[#,_]&/@vq]]=mKin[[-1,{1,2,3},4]]~Join~{q3+q4};   
Evaluate[jakNR[Pattern[#,_]&/@vq]]=jakobianAnalit[kinNR[vq],vq];
kinNR[vq]//MF

v[t_]={v1[t],v2[t],v3[t],v4[t]};
mJ[t_]=jakNR[q[t]];
sterU=mQ.mInv[mJ[t]].(v[t]-mJ'[t].q'[t])+vC+vD;
rowLin=rowDyn/.Thread[u[t]->sterU];   (* linearyzujemy!!! *)
sterUSRI=mQ.mInvSRI[mJ[t]].(v[t]-mJ'[t].q'[t])+vC+vD;  (*  SRI *)
rowLinSRI=rowDyn/.Thread[u[t]->sterUSRI];

Numeryczne wartości sterowań,warunki początkowe i rozwiązanie

ster0=Thread[v[t]->{0,0,0,0}];         (* wybieramy nowe sterowania *)
ster1=Thread[v[t]->{-0.1,0,0,0}];
ster2=Thread[v[t]->{0,-.3Sin[t],0,-.3Cos[t]}];
ster=ster2;
init={q[0]=={0,π/4,0,0},q'[0]=={0,0,0,0}};  (* warunki początkowe *)
tmax=20;
rozw=First@NDSolve[{rowLin/.numParam/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.numParam/.ster]

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.rozw/.numParam],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajy=ParametricPlot3D[Evaluate[kin[q[t]][[-1]]/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
plotManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

animManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

Plot[Evaluate[Det[jakNR[q[t]]/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Zapinamy pętlę sprzężenia      v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

numParam={(Subscript[d, 1])->2.,(Subscript[a, 1])->6.,(Subscript[d, 2])->3.,(Subscript[a, 2])->4.,(Subscript[d, 3])->1.,(Subscript[a, 3])->3.,(Subscript[d, 4])->1.,(Subscript[a, 4])->2.,(Subscript[m, 1])->2.,(Subscript[m, 2])->4.,(Subscript[m, 3])->5.,(Subscript[m, 4])->2.,vg->10.};
tmax=30;
Kp=3IdentityMatrix[4];
Kd=1IdentityMatrix[4];
yd0[t_]={2 Cos[t]+10,3 Sin[t],5,0};  
yd1[t_]={2 Cos[t]+10,3 Sin[t],5,Sin[2t]};
yd2[t_]={2 Cos[t]+10,3 Sin[t],5+Sin[2 t],0};
yd3[t_]={2 Cos[t]+10,3 Sin[t],5+Sin[2 t],-Sin[4t]};
yd4[t_]={2Cos[3t]+10,3 Sin[t],5+Sin[7t+π/3],0}; 
yd5[t_]={2 Cos[t]^3+10.,3 Sin[t]^3,5,Sin[4t]};               (* asteroida *)

yd[t_]=yd5[t];                         (* trajektoria zadana *)
trajzad=ParametricPlot3D[Evaluate[yd[t][[{1,2,3}]]],{t,0,tmax},PlotStyle->Red,PlotRange->All]
petla=yd''[t]-Kd.((∂_t)kinNR[q[t]]-yd'[t])-Kp.(kinNR[q[t]]-yd[t]);
ster=Thread[v[t]->petla];
init={q[0]=={0,π/4,0,0},q'[0]=={0,0,0,0}};  (* warunki początkowe *)
rozw=First@NDSolve[{rowLin/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.ster/.numParam]

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajy=ParametricPlot3D[Evaluate[kin[q[t]][[-1]]/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
plotManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

animManip[kinFull[q[t]]/.rozw,0,tmax,0.05,{trajy,trajzad}]

Błąd, wyznacznik jakobianu, przyspieszenia w przegubach

blad[t_]=Norm[yd[t]-kinNR[q[t]]]/.rozw;
Plot[Evaluate[blad[t]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","ey"}]
Plot[Evaluate[Det[jakNR[q[t]]/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Redundantny

Algorytm sterowania

u==((QJ)^(-1))(v-(OverDot[J])(OverDot[q]))+Cq+D                ⟵ nieredundantny  
u==((((JQ^(-1))))^(#))(v-(OverDot[J])(OverDot[q])+(JQ^(-1))(Cq+D))  ⟵ redundantny  
v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))          ⟵ sprzężenie zwrotne

v – nowy wektor sterowania 
(A^((#)^□)) – pseudoodwrotność
(Subscript[K, d]),(Subscript[K, p]) – macierze wzmocnień

v[t_]={v1[t],v2[t],v3[t]};
mJ[t_]=jak[q[t]];
invmQ=Inverse[mQ];
mD=mJ[t].invmQ;
mP=(-mJ'[t].q'[t]+mJ[t].invmQ.(vC+vD));
{tpseudo,sterU}=Timing[mInvPseudo[mD].(v[t]+mP)];
rowLin=rowDyn/.Thread[u[t]->sterU];   (* linearyzujemy!!! *)
tpseudo

(* z rzutowaniem na przestrzeń zerową
                                                                vvvvvvvvvvvvvvvv *)
u==((((JQ^(-1))))^(#))(v-(OverDot[J])(OverDot[q])+(JQ^(-1))(Cq+D))+(I-((((JQ^(-1))))^(#))((JQ^(-1))))(Subscript[v, 0])
(Subscript[v, 0])==(Subscript[k, 0])∇w(q)-(Subscript[k, t])(OverDot[q])   ⟵ funkcja celu w z tłumieniem

v – nowy wektor sterowania 
(Subscript[v, 0]) – sterowanie w przestrzeni zerowej
w(q)  – funkcja celu
(Subscript[k, 0]), (Subscript[k, t])  – wzmocnienie, tłumienie

v[t_]={v1[t],v2[t],v3[t]};
mJ[t_]=jak[q[t]];
mD=mJ[t].Inverse[mQ];
mDInv=mInvPseudo[mD];
mP=-mJ'[t].q'[t]+mJ[t].Inverse[mQ].(vC+vD);
mZero=(u[t]//Length//IdentityMatrix)-mDInv.mD;
sterU=mDInv.(v[t]+mP);
sterUZero=sterU-kt mZero .q'[t];              (* tylko tłumienie *)
rowLinZero=rowDyn/.Thread[u[t]->sterUZero];
fw=Sqrt[Det[mJ[t].Transpose[mJ[t]]]]; (* plus odpychanie od osobliwości *)
sterUZeroOso=sterU+ mZero .(k0 D[fw,{q[t]}]-kt q'[t]);
rowLinZeroOso=rowDyn/.Thread[u[t]->sterUZeroOso];
LeafCount[rowLin]
LeafCount[rowLinZero]
LeafCount[rowLinZeroOso]

Symulacje

Numeryczne wartości sterowań,warunki początkowe i rozwiązanie

ster0=Thread[v[t]->{0,0,0}];         (* wybieramy nowe sterowania *)
ster1=Thread[v[t]->{-0.1,0,0}];
ster2=Thread[v[t]->{-0.1Sin[t],-Cos[t],0}];
ster=ster2;
init={q[0]=={0,π/4,0,0},q'[0]=={0,0,0,0}};  (* warunki początkowe *)
tmax=20;
{trozw,rozw}=Timing[First@NDSolve[{rowLin/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}]];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLin/.ster]
trozw

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajy=ParametricPlot3D[Evaluate[kin[q[t]][[-1]]/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
plotManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

animManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

Wyznacznik jakobianu, przyspieszenia w przegubach

Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Zapinamy pętlę sprzężenia      v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

tmax=20;
Kp=3IdentityMatrix[2];
Kd=1IdentityMatrix[2];
yd0[t_]={2 Cos[t]+10,3 Sin[t],5};  
yd1[t_]={2 Cos[t]+10,3 Sin[t],5};
yd2[t_]={2 Cos[t]+10,3 Sin[t],5+Sin[2 t]};
yd3[t_]={2 Cos[t]+10,3 Sin[t],5+Sin[2 t]};
yd4[t_]={2Cos[3t]+10,3 Sin[t],5+Sin[7t+π/3]}; 
yd5[t_]={2 Cos[t]^3+10.,3 Sin[t]^3,5};               (* asteroida *)

yd[t_]=yd0[t];                         (* trajektoria zadana *)
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,PlotRange->All]
petla=yd''[t]-Kd.((∂_t)kin[q[t]][[-1]]-yd'[t])-Kp.(kin[q[t]][[-1]]-yd[t]);
ster=Thread[v[t]->petla];

init={q[0]=={0,π/4,0,0},q'[0]=={0,0,0,0}};
tmax=30;
{trozw,rozw}=Timing[First@NDSolve[{rowLinZeroOso/.ster,init},vq,{t,0,tmax},Method->{"EquationSimplification"->"Residual"}]];
tkon=(q1/.rozw)["Domain"][[1,2]]
LeafCount[rowLinZeroOso/.ster]
trozw

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
Plot[Evaluate[q'[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]
Plot[Evaluate[lstrona/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","u"},PlotLegends->MapIndexed[Subscript[u,#2[[1]]]&,vq]]
trajy=ParametricPlot3D[Evaluate[kin[q[t]][[-1]]/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]
plotManip[kinFull[q[t]]/.rozw,0,tmax,0.05]

animManip[kinFull[q[t]]/.rozw,0,tmax,0.05,{trajy,trajzad}]

Błąd, wyznacznik jakobianu, przyspieszenia w przegubach

blad[t_]=Norm[yd[t]-kinNR[q[t]]]/.rozw;
Plot[Evaluate[blad[t]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","ey"}]
Plot[Evaluate[Det[jakNR[q[t]]/.rozw]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]

Wyznacznik,przyspieszenia,prędkości,błąd w wybranych chwilach

przedzialy={{0,0.00001},{2π-0.01,2π+0.01},{4π-0.001,4π+0.001},
{4.65,4.75},{1,5},{5,tmax},
{5,tmax-0.1}};
{tp,tk}=przedzialy[[1]];
Plot[Evaluate[Sqrt@Det[jak[q[t]][[-1]].Transpose[jak[q[t]][[-1]]]/.numParam/.rozw]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]] 
Plot[Evaluate[q'[t]/.rozw],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]   
Plot[Evaluate[Norm[(kin[q[t]][[-1]]-yd[t])/.numParam/.rozw]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","e"}]

Dynamic[Refresh[Module[{naglowki,menuElementy},naglowki=Cells[EvaluationNotebook[],CellStyle->{"Chapter","Section","Subsection","Subsubsection"}];
If[naglowki==={},"Brak nagłówków do wyświetlenia",menuElementy=Table[With[{k=n},With[{typRaw=CurrentValue[k,CellStyle],rawContent=Quiet@NotebookRead[k]},With[{typ=If[ListQ[typRaw],First[typRaw],typRaw]},Row[{Spacer[Switch[typ,"Chapter",0,"Section",20,"Subsection",40,"Subsubsection",60,_,0]],Button[Row[{Style[Switch[typ,"Chapter","● ","Section","■ ","Subsection","○ ","Subsubsection","- ",_,""],FontWeight->If[MemberQ[{"Chapter","Section"},typ],Bold,Plain],FontColor->Switch[typ,"Chapter",RGBColor[0.45,0.23,0.11],"Section",RGBColor[0.31,0.25,0.19],"Subsection",RGBColor[0.43,0.50,0.35],"Subsubsection",RGBColor[0.55,0.48,0.42],_,Black],FontSize->Switch[typ,"Chapter",15,"Section",14,"Subsection",13,_,12]],Style[(*POPRAWKA:Prawidłowe sprawdzanie nagłówka i wyświetlanie tekstu*)If[Head[rawContent]===Cell,rawContent[[1]]/.{TextData[lista_]:>Row[lista/.Cell[BoxData[b_],___]:>RawBoxes[b]],BoxData[b_]:>RawBoxes[b]}," "],FontWeight->If[MemberQ[{"Chapter","Section"},typ],Bold,Plain],FontColor->Switch[typ,"Chapter",RGBColor[0.45,0.23,0.11],"Section",RGBColor[0.31,0.25,0.19],"Subsection",RGBColor[0.43,0.50,0.35],"Subsubsection",RGBColor[0.55,0.48,0.42],_,Black],FontSize->Switch[typ,"Chapter",15,"Section",14,"Subsection",13,_,12],FontFamily->"Arial"]}],SelectionMove[k,All,Cell],Appearance->"Frameless",Alignment->Left]}]]]],{n,naglowki}];
Framed[OpenerView[{Style[" ☰ NAWIGACJA PO SLAJDACH",13,Bold,RGBColor[0.35,0.25,0.15]],Column[menuElementy,Spacings->0.3]},True  (*czy otwarte*)],Background->RGBColor[0.97,0.96,0.93],FrameMargins->10,FrameStyle->RGBColor[0.85,0.82,0.76],RoundingRadius->5]]],UpdateInterval->7]]