Przyklady robotyczne

Kinematyka manipulatorow

Podwojne wahadlo

Kinematyka i jakobian manipulatora

Zdefiniujmy kinematyke dla podwojnego wahadla wyrazona we wspolrzednych polozenia efektora (ramiona o dlugosci 6 i 3)

kinem[x1_,x2_]={6 Cos[x1]+3 Cos[x1+x2],6 Sin[x1]+3 Sin[x1+x2]};
MF=MatrixForm;
kinem[x1,x2]//MF

Wyliczmy jakobian analityczny dla zdefiniowanej kinematyki (jak ogolnie - niezaleznie od liczby stopni swobody - bedzie pozniej)

jakob[x1_,x2_] = {D[kinem[x1,x2],x1],D[kinem[x1,x2],x2]}//Transpose;
jakob[a,b]//MF

Zadanie kinematyki odwrotnej - metoda jakobianowa

Zdefiniujmy trajektorie zadana

yd[t_]={7 Cos[t],7 Sin[t] }(* wiersz to czy kolumna??? *)

%//MF

Jak wiec policzyc iloczyn wektora pionowego przez poziomy?

{(Subscript[a, 1]),(Subscript[a, 2])}.{(Subscript[b, 1]),(Subscript[b, 2])}

{(Subscript[a, 1]),(Subscript[a, 2])}{(Subscript[b, 1]),(Subscript[b, 2])}

{{(Subscript[a, 1])},{(Subscript[a, 2])}}.{{(Subscript[b, 1]),(Subscript[b, 2])}}

Rownanie jakobianowe ma postac

lstrona={x1'[t],x2'[t]};
pstrona=Inverse[jakob[x1[t],x2[t]]].(∂_t)yd[t];
rownanie=Apply[Equal,{lstrona,pstrona}//Transpose ,1]

Dodajemy warunki poczatkowe

(tosolve = rownanie ~Join~{x1[0]==-.5,x2[0]==1})//MF

i rozwiazujemy otrzymane rownania

tmax = 8 π;
rozwiazanie=NDSolve[tosolve,{x1,x2},{t,0,tmax}];

Zobaczmy co dostalismy

tmax=6.2819;

Plot[Evaluate[x1[t]/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","x1"}]
Plot[Evaluate[x2[t]/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","x2"}]

A w przestrzeni zadaniowej

trajy=ParametricPlot[Evaluate[kinem[x1[t],x2[t]]/.rozwiazanie],{t,0,tmax},AspectRatio->1]

trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},AspectRatio->1,PlotStyle->RGBColor[1,0,0]]

Show[trajy,trajzad]

Dolozmy slad ruchu manipulatora

ramie2[x1_,x2_]:=kinem[x1,x2];
ramie1[x1_,x2_] := {6 Cos[x1],6Sin[x1]};
PlotPendul[x1_,x2_,tmin_,tmax_,step_]:=
Module[{tmpx1,tmpx2,i},
      links={};
      For[i=tmin,i<=tmax,i+=step,
         tmpx1=x1/.t->i;tmpx2=x2/.t->i;
         AppendTo[links,Line[{{0,0},ramie1[tmpx1,tmpx2],ramie2[tmpx1,tmpx2]}]];
         ];
      Show[Graphics[links],PlotRange->All,AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]
      ];

manip=PlotPendul[x1[t]/.rozwiazanie[[1,1]],x2[t]/.rozwiazanie[[1,2]],0,tmax,0.2]

I wszystko razem

Show[trajy,trajzad,manip,PlotRange->All]

Animacja

Show[Graphics[links[[1]]],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

toanim[[30]]

Animate[toanim[[x]],{x,1,Length[toanim],1}]

Razem ze ścieżką ruchu

toanim=Map[Show[trajy,Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Okreslmy warunki poczatkowe

tmaxinit=10;

initcond={z1'[t]==(Inverse[jakob[z1[t],z2[t]]].(yd[0]-kinem[z1[t],z2[t]]))[[1]],z2'[t]==(Inverse[jakob[z1[t],z2[t]]].(yd[0]-kinem[z1[t],z2[t]]))[[2]]};
AppendTo[initcond,z1[0]==-1];
AppendTo[initcond,z2[0]==-1];
solinit=NDSolve[initcond,{z1,z2},{t,0,tmaxinit}];

Plot[z1[t]/.solinit,{t,0,tmaxinit},PlotRange->All,AxesLabel->{"t","z1"}]
Plot[z2[t]/.solinit,{t,0,tmaxinit},PlotRange->All,AxesLabel->{"t","z2"}]

(tosolve = rownanie ~Join~{x1[0]==z1[tmaxinit]/.solinit[[1]],x2[0]==z2[tmaxinit]/.solinit[[1]]})//MF

I wracamy do rozwiazujemy otrzymane rownanie

Bledy

Plot[Evaluate[yd[t]-kinem[x1[t],x2[t]]/.rozwiazanie[[1]]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","errq"}]

Plot[Evaluate[(Sqrt[(yd[t]-kinem[x1[t],x2[t]]).(yd[t]-kinem[x1[t],x2[t]])])/.rozwiazanie[[1]]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","erry"}]

Pierwiastek wpisany z klawiatury przez Esc sqrt Esc

√(a^2)+b

Zas "wyklikany" z palety (lub Ctrl+2)

(Sqrt[(a^2)+b])+(Sqrt[(c^2)])+d

A w ogole (Sqrt[a^2])≠a

(Sqrt[(a^2)])

Simplify[%,a>0]

Plot[Evaluate[Det[jakob[x1[t],x2[t]]/.rozwiazanie[[1]]]],{t,0,tmax}]

Plot[Evaluate[Det[jakob[x1[t],x2[t]]/.rozwiazanie[[1]]]],{t,6.25,6.32}]

Powtorzmy rzecz dla trajektorii zadanej (warunki poczatkowe dla tmaxinit 10 a potem 100)

yd[t_]={2+7 Cos[t],7 Sin[t]};
rownanie=Apply[Equal,({{x1'[t],x2'[t]},Inverse[jakob[x1[t],x2[t]]].(∂_t)yd[t]}//Transpose ),1];

tmaxinit=100;

A takze dla trajektorii zadanej (warunki poczatkowe tmaxinit 100 a potem 10)

yd[t_]={2+7 Cos[t-3],7 Sin[t-3]};
rownanie=Apply[Equal,({{x1'[t],x2'[t]},Inverse[jakob[x1[t],x2[t]]].(∂_t)yd[t]}//Transpose ),1];

tmaxinit=100;

Jakas inna trajektoria

yd[t_]={6+2 Cos[t],2 Sin[t]};
rownanie=Apply[Equal,({{x1'[t],x2'[t]},Inverse[jakob[x1[t],x2[t]]].(∂_t)yd[t]}//Transpose ),1];

Jakas inna trajektoria

yd[t_]={6+2 Cos[t],2 Sin[2 t]};
rownanie=Apply[Equal,({{x1'[t],x2'[t]},Inverse[jakob[x1[t],x2[t]]].(∂_t)yd[t]}//Transpose ),1];

Zadanie kinematyki odwrotnej - metoda Singularity Robust Inverse ((((J^T)((JJ^T)+ϵI))^(-1)))

ϵ=0.0001;
lstrona={x1'[t],x2'[t]};
pstrona=Transpose[jakob[x1[t],x2[t]]].Inverse[jakob[x1[t],x2[t]].Transpose[jakob[x1[t],x2[t]]]+ϵ IdentityMatrix[2]].(∂_t)yd[t];
rownanie=
Apply[Equal,({lstrona,pstrona}//Transpose ),1];

Dolaczamy warunki poczatkowe (wyliczone dokladnie tmaxinit=100, potem mniej tmaxinit=10) i rozwiazujemy (dla jakiegos przypadku osobliwego przy eps=10^-4, 10^-2 i porownujemy bledy polozenia efektora, takze dla przypadku "prawie osobliwego")

Funkcje do wyliczania kinematyki manipulatora

Definiujemy liste parametrow Denevita-Hartenberga dla podwojnego wahadla

DHParameters ={{q1,0,6,0},{q2,0,3,0}}

Podajmy definicje macierzy elementranych

TransX[q_,d_] :=
                {{ 1,      0,       0, d},
                 { 0, Cos[q], -Sin[q], 0},
                 { 0, Sin[q],  Cos[q], 0},
                 { 0,     0,       0,  1}}
TransZ[q_,d_] :=
                {{ Cos[q], -Sin[q], 0, 0},
                 { Sin[q],  Cos[q], 0, 0},
                 {     0,        0, 1, d},
                 {     0,        0, 0, 1}}

oraz funkcji wyliczajacej kinematyke

Kinematyka[DHParameters_List:{}] :=
	Module[{i,dof,kinematics},
	    kinematics =   IdentityMatrix[4];
		    dof = Length[DHParameters];
    For[i=1, i<= dof, i++,
         kinematics =  kinematics . 
                       TransZ[ DHParameters[[i,1]], DHParameters[[i,2]] ] .
                       TransX[ DHParameters[[i,4]], DHParameters[[i,3]] ] ;];
		kinematics
		];

Liczymy kinematyke

DHParameters//Kinematyka

(kinem=%//Simplify)//MatrixForm

Wybieramy czesc translacyjna i idziemy do czesci, gdzie badalismy zachowanie podwojnego wahadla

kinem[x1_,x2_] = Transpose[kinem][[4,{1,2}]]/.{q1->x1,q2->x2}

Nie dalo sie? Bo kinem ma wartosc

kinem

Zmieniamy wiec nazwe kinematyki w postaci macierzowej, czyscimy co trzeba, podstawiamy i teraz idziemy gdzie trzeba

mkinem=kinem;
kinem=.;
kinem[x1_,x2_] = Transpose[mkinem][[4,{1,2}]]/.{q1->x1,q2->x2}

kinem[a,b]//MatrixForm

Poczworne wahadlo

Kinematyka i jakobian manipulatora

Definiujemy liste parametrow Denevita-Hartenberga dla poczwornego wahadla, wektor polozen i wyliczamy kinematyke

DHParameters ={{q1,0,6,0},{q2,0,3,0},{q3,0,4,0},{q4,0,2,0}};
x[t_]={x1[t],x2[t],x3[t],x4[t]};
DHParameters//Kinematyka;
(mkinem=%//Simplify)//MatrixForm
kinem[x_] := Transpose[mkinem][[4,{1,2}]]/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]};
kinem[{a,b,c,d}]//MF

Operowanie wektorem polozen

x[t]
x[0]
x'[t]

Zadanie kinematyki odwrotnej - metoda jakobianowa

Zdefiniujmy trajektorie zadana

yd[t_]={7 Cos[t],7 Sin[t]}

yd[t_]={6+2 Cos[t],2 Sin[2 t]}

Zdefiniujmy jakobian analityczny

jakob[x_] := D[kinem[x],x];

Zobaczmy co dostalismy

?jakob

jakob[{a,b,c,d}]

Dlaczego

kinem[x]

x

Acha, no to moze tak

jakob[x_] := D[kinem[x[t]],x[t]];

?jakob

jakob[{a,b,c,d}]

?D

Wiec moze tak

jakob[x_] := D[kinem[x],{x}];

?jakob

j1=jakob[{a,b,c,d}]

%//MF

Moze jednak policzyc te pochodna "na piechote"? A moze najlepiej zdefiniowac wlasna funkcje rozniczkujaca po wektorze - zrobimy to pozniej przy liczeniu dynamiki, tam bedziemy rozniczkowac po q, q' wiec sie przyda

(jakob[x_]=Apply[D,Table[{kinem[x][[i]],x[[j]]},{i,2},{j,4}],{2}])//MF

j2=jakob[{a,b,c,d}]

Niby to samo ale "nasza definicja" liczy sie tylko raz

j1-j2

Uwaga - co powoduje pominiecie nawiasow

(mp1={{1,2},{3,4}})//MF
Inverse[mp1]//MF

mp1={{1,2},{3,4}}//MF
Inverse[mp1]//MF

jakob[{a,b,c,d}]//MF

jakob[x[t]]//MF

Transpose[jakob[x[t]]]//MF

Rownanie jakobianowe (x'=(((J^T)((JJ^T)))^(-1))y')

rownanie=Apply[Equal,({x'[t],Transpose[jakob[x[t]]].Inverse[jakob[x[t]].Transpose[jakob[x[t]]]].(∂_t)yd[t]}//Transpose ),1];

i warunki poczatkowe

tosolve = rownanie ~Join~{x1[0]==-.5,x2[0]==1,x3[0]==1,x4[0]==1};

Rozwiazujemy

tmax = 10π;
rozwiazanie=NDSolve[tosolve,{x1,x2,x3,x4},{t,0,tmax}];

Zobaczmy co dostalismy

Plot[Evaluate[x[t]/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","x[t]"}]

A w przestrzeni zadaniowej

trajy=ParametricPlot[Evaluate[kinem[x[t]]/.rozwiazanie],{t,0,tmax},AspectRatio->1]

trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},AspectRatio->1,PlotStyle->RGBColor[1,0,0]]

Show[trajy,trajzad,PlotRange->All]

Dolozmy slad ruchu manipulatora

Sposob wyliczania kinematyki w punkcie

rozwiazanie

x[t]/.rozwiazanie[[1]]/.t->1
x[1]/.rozwiazanie[[1]]

kinem[x[1]]/.rozwiazanie[[1]]

Wygenerujmy liste kinematyk czesciowych

DHParameters

Potrzebujemy liste list parametrow DH kolejno dla pierwszego stopnia, pierwszego i drugiego itp.
Czyli macierz trojkatna

Table[i,{j,4},{i,j}]

Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,0,4},{i,j}]]

%//MF

fullkinem[x_]:=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]}

fullkinem[x[1]]/.rozwiazanie[[1]]

Brakuje zera? - mozemy dolozyc recznie (np. funkcja Join) albo wykonac obliczenia jak powyzej dolaczajac na poczatku listy list parametrow DH manipulator o 0. liczbie stopni swobody (zamienimy w generowanej tabeli {j,4} na {j,0,4})

PlotPlanar[x_,tmin_,tmax_,step_]:=
Module[{i},(* korzysta ze zmiennej globalnej t!!!*)
      links={};
      For[t=tmin,t<=tmax,t+=step,
   AppendTo[links,Line[fullkinem[x]]];   (* bardzo nieefektywne czasowo *)
         ];  (* lepiej wygenerowac liste dobrej dlugosci (links=Table[{},{l_elem}]) i podmieniac elementy (links[[i]]={Line[...]}) *)
Clear[t];
      Show[Graphics[links],PlotRange->All,AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]
      ];

manip=PlotPlanar[x[t]/.rozwiazanie[[1]],0,10,0.2]

Show[trajy,trajzad,manip,PlotRange->All]

Animacja

toanim=Map[Show[trajy,Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1}]

Bledy

Plot[Evaluate[yd[t]-kinem[x[t]]/.rozwiazanie[[1]]],{t,0,tmax},PlotRange->All]

Dynamika manipulatorow

Podwojne wahadlo

Dynamika manipulatora (EDDA)

(* Elementy skladowe rownan dynamiki i same rownania *)
(Subscript[θ, 1])=3.1; (Subscript[θ, 2])=9.5; (Subscript[θ, 3])=0.24; (Subscript[θ, 4])=0.77;
(MQ[{q1_,q2_}]={{(Subscript[θ, 1])+0.6 (Subscript[θ, 4]) Cos[q2],(Subscript[θ, 3])+0.3 (Subscript[θ, 4])Cos[q2]},{(Subscript[θ, 3])+0.3 (Subscript[θ, 4])Cos[q2],(Subscript[θ, 3])}})//MF
(MC[{q1_,q2_},{qp1_,qp2_}]={{-qp1,-(qp1+qp2)},{qp1,0}} 0.3 (Subscript[θ, 4])Sin[q2])//MF
(MD[{q1_,q2_}]=1 {(Subscript[θ, 2])Cos[q1]+(Subscript[θ, 4])Cos[q1+q2],(Subscript[θ, 4])Cos[q1+q2]})//MF
(MR=0.6IdentityMatrix[2])//MF
q[t_]={q1[t],q2[t]}
rowdyn=Apply[Equal,{MQ[q[t]].q''[t]+MC[q[t],q'[t]].q'[t]+MD[q[t]]+MR.q'[t]//Expand,{u1[t],u2[t]}}//Transpose,1]

(* ??? *)
rowdyn={q1''[t]+Cos[q1[t]+q2[t]]==u1[t],q2''[t]+Cos[q1[t]+q2[t]]==u2[t]};

Det[MQ[{q1,q2}]]

%//Chop

Dolaczamy warunki poczatkowe, ustalamy sterowania i rozwiazujemy

(tosolve = (rowdyn/.{u1[t]->0,u2[t]->0}) ~Join~{q1[0]==0,q2[0]==0,q1'[0]==0,q2'[0]==0})//MF

Rozwiazujemy

tmax = 30;
rozwiazanie=NDSolve[tosolve,{q1,q2},{t,0,tmax},MaxSteps->10000];

tmax = 30;
rozwiazanie=NDSolve[tosolve,{q1,q2},{t,0,tmax},Method->ExplicitRungeKutta,MaxSteps->100000];

Zobaczmy co dostalismy

tmax=20;

Plot[Evaluate[q1[t]/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q1"}]
Plot[Evaluate[q2[t]/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q2"}]

Kinematyka potrzebna do wizualizacji

DHParameters ={{q1,0,6,0},{q2,0,3,0}};
fullDH=Map[Wybierz,Table[i,{j,0,2},{i,j}]];
fullkinem[x_]:=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]]}

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.05]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Wyliczenie dynamiki manipulatora z formalizmu E-L

Parametry manipulatora

q[t_]={q1[t],q2[t]};
DHParameters ={{q1,0,l1,0},{q2,0,l2,0}};

dlug=Transpose[DHParameters][[3]]

masy={m1,m2}

Kinematyki czesciowe by miec polozenia i predkosci mas do energii (mozna na piechote, ale skoro wiemy co to sa kinematyki czesciowe :-)

Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,2},{i,j}]];
fullkinem[x_]=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]}

fullkinem[{0,0}]

Polozenie nad poziomem morza i energia potencjalna

pol=fullkinem[q[t]][[{1,2},2]] 
(* alternatywnie *)
pol=fullkinem[q[t]][[All,2]] 
(* alternatywnie *)
pol =Transpose[fullkinem[q[t]]][[2]]

pol//MF

Ep = m g h - liczymy wiec

Ep=Plus @@(masy g pol)    (* @@ synonim Apply, g - przyspieszenie ziemskie *)

Predkosc i energia kinetyczna

pr=D[fullkinem[q[t]],t]

pr//MF

Podnosimy do kwadratu (ponizej przyklad dla listy skladowych predkosci czterech cial)

tmp={{ax,ay},{bx,by},{cx,cy},{dx,dy}}

(tmp2=tmp.Transpose[tmp])//MF

Table[tmp2[[i,i]],{i,4}]

Albo lepiej

tmp tmp

Plus @@Transpose[tmp tmp]

pr2=Plus @@Transpose[pr pr]

Ek = 1/2 m v^2 - liczymy wiec

Ek=Plus @@ (1/2 masy pr2)

I w końcu lagranżian

Lagr=Ek-Ep;

q[t]

Jego pochodne czastkowe

Dvec[wyrazenie_,wektor_]:=Apply[D,Table[{wyrazenie,wektor[[i]]},{i,Length[wektor]}],1]

Dvec[lagranzian[q[t],q'[t]],q[t]]

Dvec[lagranzian[q[t],q'[t]],q'[t]]

Rownanie E-L (d/(dt))((∂L)/(∂q'))-((∂L)/(∂q))=u

lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]]

%//Simplify

lstrona=%;

Jak majac lewe i prawe strony zrobic rownania?

ls={a,b,c,d}

Apply[Equal,Transpose[{ls ,{u1,u2,u3,u4}}],1]

Robimy wiec rownania dynamiki naszego robota, ustalamy wartości parametrów, dodajemy warunki poczatkowe i rozwiazujemy

l1=6;l2=3;m1=5;m2=4;g=10;
rowdyn=Apply[Equal,Transpose[{lstrona ,{u1[t],u2[t]}}],1];

tosolve = (rowdyn/.{u1[t]->0,u2[t]->0,u3[t]->0,u4[t]->0}) ~Join~{q1[0]==Pi/2,q2[0]==0,q1'[0]==0,q2'[0]==0};

tosolve

(* Obie metody dla q1[0]=0, q1[0]=-Pi/2, q1[0]=-Pi/2+0.1 i w koncu q1[0]=Pi/2 *)
tmax = 100;
rozwiazanie=NDSolve[tosolve,{q1,q2},{t,0,tmax},MaxSteps->10000];

tmax = 100;
rozwiazanie=NDSolve[tosolve,{q1,q2},{t,0,tmax},Method->RungeKutta,MaxSteps->100000];

Zobaczmy co dostalismy (tmax np. 20, ale tez dla calego)

tmax=20;

Plot[Evaluate[q1[t]/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q1"}]
Plot[Evaluate[q2[t]/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q2"}]

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.1]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Poczworne wahadlo

Wyliczenie dynamiki manipulatora z formalizmu E-L

Policzmy i zreasumujmy wczesniejsze prezentacje - kompletny przyklad, mozna uruchomic w nowym jadrze

Definicje macierzy elementranych

TransX[q_,d_] :=
                {{ 1,      0,       0, d},
                 { 0, Cos[q], -Sin[q], 0},
                 { 0, Sin[q],  Cos[q], 0},
                 { 0,     0,       0,  1}}
TransZ[q_,d_] :=
                {{ Cos[q], -Sin[q], 0, 0},
                 { Sin[q],  Cos[q], 0, 0},
                 {     0,        0, 1, d},
                 {     0,        0, 0, 1}}

oraz funkcji wyliczajacej kinematyke

Kinematyka[DHParameters_List:{}] :=
	Module[{i,dof,kinematics},
	    kinematics =   IdentityMatrix[4];
		    dof = Length[DHParameters];
    For[i=1, i<= dof, i++,
         kinematics =  kinematics . 
                       TransZ[ DHParameters[[i,1]], DHParameters[[i,2]] ] .
                       TransX[ DHParameters[[i,4]], DHParameters[[i,3]] ] ;];
		kinematics
		];

i pochodnej czastkowej funkcji wektorowej

Dvec[wyrazenie_,wektor_]:=Apply[D,Table[{wyrazenie,wektor[[i]]},{i,Length[wektor]}],1];

Funkcja wykreslajaca szkielet manipulatora (zmodyfikowana w stosunku do porzedniej wersji, by nie trzeba bylo dodawać punktu (0,0))

PlotPlanar[x_,tmin_,tmax_,step_]:=
Module[{i},(* korzysta ze zmiennej globalnej t!!!*)
      links={};
      For[t=tmin,t<=tmax,t+=step,
   AppendTo[links,Line[{{0,0}}~Join~fullkinem[x]]];
         ];
Clear[t];
      Show[Graphics[links],PlotRange->All,AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]
      ];

Parametry manipulatora

q[t_]={q1[t],q2[t],q3[t],q4[t]};
u[t_]={u1[t],u2[t],u3[t],u4[t]};
DHParameters ={{q1,0,6,0},{q2,0,3,0},{q3,0,4,0},{q4,0,2,0}};
masy={5,4,3,2};

Kinematyki czesciowe by miec polozenia i predkosci mas do energii

Off[Part::partd];
Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,Length[DHParameters]},{i,j}]];
fullkinem[x_]=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]};

Obejrzymy, co dostalismy

MF=MatrixForm;
fullkinem[q[t]]//MF
fullkinem[{0,0,0,0}]//MF

Polozenie "nad poziomem morza" i energia potencjalna

polh =Transpose[fullkinem[q[t]]][[2]];
Ep=Plus @@(10 masy polh);   (* @@ synonim Apply,10 - przyspieszenie ziemskie *)

Predkosc i energia kinetyczna

pr=D[fullkinem[q[t]],t];
pr2=Plus @@Transpose[pr pr];
Ek=Plus @@ (1/2 masy pr2);

Lagranżian

Lagr=Ek-Ep;

Rownania ruchu (rownanie E-L) (d/(dt))((∂L)/(∂q'))-((∂L)/(∂q))=u

lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]];

(* najpierw bez tarcia - kolejno z tarciem  *)
tarcie = 10 D[q[t], t];
lstrona += tarcie;

rowdyn=Apply[Equal,Transpose[{lstrona ,u[t]}],1];

Rozwiazmy dla przykladowych sterowan i konfiguracji poczatkowych

tosolve = (rowdyn/.{u1[t]->0,u2[t]->0,u3[t]->0,u4[t]->0}) ~Join~{q1[0]==Pi/2,q2[0]==0,q3[0]==0.01,q4[0]==-0.02,q1'[0]==0,q2'[0]==0,q3'[0]==0,q4'[0]==0};

tmax=50;
rozwiazanie=NDSolve[tosolve,{q1,q2,q3,q4},{t,0,tmax},Method->Automatic,MaxSteps->100000]; (* w Mathematice 9 dodatkowo opcja SolveDelayed->True *)
(* W Mathematice 10 dodatkowo Method->{"EquationSimplification"->"Residual"} *)

tmax=50;
rozwiazanie=NDSolve[tosolve,{q1,q2,q3,q4},{t,0,tmax},Method->ExplicitRungeKutta,MaxSteps->100000];

Zobaczmy co dostalismy

tmax=20;

Plot[Evaluate[{q1[t],q2[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q1"}]
Plot[Evaluate[{q3[t],q4[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q2"}]

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.02]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Algorytm sterowania przy pelnej znajomosci modelu typu obliczanego momentu - dokladna linearyzacja

Wezmy model manipulatora bez tarcia, ktory ma postac Q(q)(Overscript[q, ¨])+C(q,(OverDot[q])) (OverDot[q])+D(q)=u

lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]];
rowdyn=Apply[Equal,Transpose[{lstrona ,u[t]}],1];

Na wejscie ktorego podamy sterowanie  u=Q(q)v+C(q,(OverDot[q])) (OverDot[q])+D(q) gdzie v to nowy wektor sterowan. Do tego potrzebujemy macierz Q(q) oraz resztę   C(q,(OverDot[q])) (OverDot[q])+D(q).

Jak z modelu wydobyc macierz? Na przyklad tak

model={a x+b y,c x+d y}
Coefficient[model,#]&/@{x,y}//MF (* funkcja nienazwana (anonimowa - lambda) - pure function *)

Wydobywamy macierz inercji i sprawdzamy podstawowa jej wlasnosc (symetryczna)

mQ=Coefficient[lstrona,#]&/@Evaluate[q''[t]];
mQ-mQ//Transpose

Bierzemy tez reszte

reszta=lstrona/.Apply[Rule,{q''[t],Table[0,{Length[q[t]]}]}//Transpose,1]; (* lstrona ./ q''[t]->0 *)

Definiujemy nowy wektor sterowań v i wyliczamy sterowania  u=Q(q)v+C(q,(OverDot[q])) (OverDot[q])+D(q)

v[t_]={v1[t],v2[t],v3[t],v4[t]};
tmp=mQ.v[t]+reszta;

Co w sumie mozna uzyskac duzo latwiej, zastepujac w modelu (Overscript[q, ¨]) przez v

sterU=lstrona/.Apply[Rule,{q''[t],v[t]}//Transpose,1];

sterU-tmp//Simplify (* to samo ??? *)

Wstawiamy do rownan dynamiki

rowlin=rowdyn/.Apply[Rule,{u[t],sterU}//Transpose,1];

i rozwiazujemy

tosolve = (rowlin/.{v1[t]->0,v2[t]->0,v3[t]->0,v4[t]->0}) ~Join~{q1[0]==Pi/2,q2[0]==0,q3[0]==0.01,q4[0]==-0.02,q1'[0]==0,q2'[0]==0,q3'[0]==0,q4'[0]==0};
(* potem z q1[0]=Pi/4, potem dodatkowo q3'[0]=1 i dalej q4'[0]=-0.5 oraz v3=0.1 (z zerowym q3'[0]) *)

tmax=50;
rozwiazanie=NDSolve[tosolve,{q1,q2,q3,q4},{t,0,tmax},Method->Automatic,MaxSteps->100000];
(* jesli sie nie liczy (np. przez Infinite expression 1/0) mozna sprobowac z innym czasem symulacji *)

Zobaczmy co dostalismy

tmax=20;

Plot[Evaluate[{q1[t],q2[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 1]),(Subscript[q, 2])"}]
Plot[Evaluate[{q3[t],q4[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 3]),(Subscript[q, 4])"}]

Plot[Evaluate[{q1''[t],q2''[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[(Overscript[q, ¨]), 1]),(Subscript[(Overscript[q, ¨]), 2])"}]
Plot[Evaluate[{q3''[t],q4''[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[(Overscript[q, ¨]), 3]),(Subscript[(Overscript[q, ¨]), 4])"}]

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.05]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Jak wygladaja sterowania?

Plot[Evaluate[{lstrona[[1]],lstrona[[2]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 1]),(Subscript[u, 2])"}]
Plot[Evaluate[{lstrona[[3]],lstrona[[4]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 3]),(Subscript[u, 4])"}]

(* A jak wyglada rozwiazywane rownanie? *)
rowlin[[4]]//Simplify

Co, gdy model niedokladny...

masy={5,4,3,2.02};
Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,Length[DHParameters]},{i,j}]];
fullkinem[x_]=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]};
polh =Transpose[fullkinem[q[t]]][[2]];
Ep=Plus @@(10 masy polh);   (* @@ synonim Apply,10 - przyspieszenie ziemskie *)
pr=D[fullkinem[q[t]],t];
pr2=Plus @@Transpose[pr pr];
Ek=Plus @@ (1/2 masy pr2);
Lagr=Ek-Ep;
lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]];
rowdyn=Apply[Equal,Transpose[{lstrona ,u[t]}],1];

I wracamy do symulacji z linearyzacja (Wstawiamy do rownan dynamiki)

Albo jest tarcie...

masy={5,4,3,2};
Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,Length[DHParameters]},{i,j}]];
fullkinem[x_]=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]};
polh =Transpose[fullkinem[q[t]]][[2]];
Ep=Plus @@(10 masy polh);   (* @@ synonim Apply,10 - przyspieszenie ziemskie *)
pr=D[fullkinem[q[t]],t];
pr2=Plus @@Transpose[pr pr];
Ek=Plus @@ (1/2 masy pr2);
Lagr=Ek-Ep;
lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]];
tarcie = 1 D[q[t], t];
lstrona += tarcie;
rowdyn=Apply[Equal,Transpose[{lstrona ,u[t]}],1];

I wracamy do symulacji z linearyzacja (Wstawiamy do rownan dynamiki)

Zapnijmy sprzezenie zwrotne v=(Subscript[(Overscript[q, ¨]), d])-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

rowlin=rowdyn/.Apply[Rule,{u[t],sterU}//Transpose,1];
Kp=0.1IdentityMatrix[4];
Kd=0.4IdentityMatrix[4];
qd[t_]={Pi/4,0,0,0};
sterV=qd''[t]-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t]);
sprzezenie=Apply[Rule,{v[t],sterV}//Transpose,1]

i rozwiazujemy

tosolve = (rowlin/.sprzezenie) ~Join~{q1[0]==Pi/4,q2[0]==0,q3[0]==0.01,q4[0]==-0.02,q1'[0]==0,q2'[0]==0,q3'[0]==0,q4'[0]==0};

tmax=50;
rozwiazanie=NDSolve[tosolve,{q1,q2,q3,q4},{t,0,tmax},Method->Automatic,MaxSteps->100000];

Zobaczmy co dostalismy

tmax=20;

Plot[Evaluate[{q1[t],q2[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 1]),(Subscript[q, 2])"}]
Plot[Evaluate[{q3[t],q4[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 3]),(Subscript[q, 4])"}]

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.05]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Jak wygladaja sterowania?

Plot[Evaluate[{lstrona[[1]],lstrona[[2]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 1]),(Subscript[u, 2])"}]
Plot[Evaluate[{lstrona[[3]],lstrona[[4]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 3]),(Subscript[u, 4])"}]

A gdy robot cos trzyma w efektorze...

masy={5,4,3,2.5};
Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,Length[DHParameters]},{i,j}]];
fullkinem[x_]=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]};
polh =Transpose[fullkinem[q[t]]][[2]];
Ep=Plus @@(10 masy polh);   (* @@ synonim Apply,10 - przyspieszenie ziemskie *)
pr=D[fullkinem[q[t]],t];
pr2=Plus @@Transpose[pr pr];
Ek=Plus @@ (1/2 masy pr2);
Lagr=Ek-Ep;
lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]];
tarcie = 1 D[q[t], t];
lstrona += tarcie;
rowdyn=Apply[Equal,Transpose[{lstrona ,u[t]}],1];

I wracamy do symulacji ze sprzezeniem (Zapnijmy sprzezenie zwrotne)

Inna trajektoria bez trzymania czegos, ale z tarciem

masy={5,4,3,2};
Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,Length[DHParameters]},{i,j}]];
fullkinem[x_]=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]};
polh =Transpose[fullkinem[q[t]]][[2]];
Ep=Plus @@(10 masy polh);   (* @@ synonim Apply,10 - przyspieszenie ziemskie *)
pr=D[fullkinem[q[t]],t];
pr2=Plus @@Transpose[pr pr];
Ek=Plus @@ (1/2 masy pr2);
Lagr=Ek-Ep;
lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]];
tarcie = 1 D[q[t], t];
lstrona += tarcie;
rowdyn=Apply[Equal,Transpose[{lstrona ,u[t]}],1];
rowlin=rowdyn/.Apply[Rule,{u[t],sterU}//Transpose,1];
Kp=0.1IdentityMatrix[4];
Kd=0.4IdentityMatrix[4];
qd[t_]={0.5t,-t,-Pi/2,Pi/2};
sterV=qd''[t]-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t]);
sprzezenie=Apply[Rule,{v[t],sterV}//Transpose,1]

i rozwiazujemy

tosolve = (rowlin/.sprzezenie) ~Join~{q1[0]==Pi/4,q2[0]==0,q3[0]==0.01,q4[0]==-0.02,q1'[0]==0,q2'[0]==0,q3'[0]==1,q4'[0]==-0.5};

tmax=50;
rozwiazanie=NDSolve[tosolve,{q1,q2,q3,q4},{t,0,tmax},Method->Automatic,MaxSteps->100000];

Zobaczmy co dostalismy

tmax=20;

Plot[Evaluate[{q1[t],q2[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 1]),(Subscript[q, 2])"}]
Plot[Evaluate[{q3[t],q4[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 3]),(Subscript[q, 4])"}]

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.05]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Jak wygladaja sterowania?

Plot[Evaluate[{lstrona[[1]],lstrona[[2]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 1]),(Subscript[u, 2])"}]
Plot[Evaluate[{lstrona[[3]],lstrona[[4]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 3]),(Subscript[u, 4])"}]

A bledy?

Plot[Evaluate[(q[t]-qd[t])/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[e, 1]),(Subscript[e, 2]),(Subscript[e, 3]),(Subscript[e, 4])"}]
Plot[Evaluate[(((q[t]-qd[t]).(q[t]-qd[t]))//Sqrt)/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","|e|"}] (* alternatywnie Plus@@((q[t]-qd[t])^2)//Sqrt *)

A gdy model dokladny

masy={5,4,3,2};
Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,Length[DHParameters]},{i,j}]];
fullkinem[x_]=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]};
polh =Transpose[fullkinem[q[t]]][[2]];
Ep=Plus @@(10 masy polh);   (* @@ synonim Apply,10 - przyspieszenie ziemskie *)
pr=D[fullkinem[q[t]],t];
pr2=Plus @@Transpose[pr pr];
Ek=Plus @@ (1/2 masy pr2);
Lagr=Ek-Ep;
lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]];
tarcie = 0;
lstrona += tarcie;
rowdyn=Apply[Equal,Transpose[{lstrona ,u[t]}],1];
rowlin=rowdyn/.Apply[Rule,{u[t],sterU}//Transpose,1];
Kp=0.1IdentityMatrix[4];
Kd=0.4IdentityMatrix[4];
qd[t_]={0.5t,-t,-Pi/2,Pi/2};
sterV=qd''[t]-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t]);
sprzezenie=Apply[Rule,{v[t],sterV}//Transpose,1];

i rozwiazujemy

I na koniec cos fikusnego

qd[t_]={Cos[t/4],Cos[t/2],Cos[t],Cos[2t]};
sterV=qd''[t]-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t]);
sprzezenie=Apply[Rule,{v[t],sterV}//Transpose,1];

i rozwiazujemy

tosolve = (rowlin/.sprzezenie) ~Join~{q1[0]==1,q2[0]==1,q3[0]==1,q4[0]==1,q1'[0]==0,q2'[0]==0,q3'[0]==0,q4'[0]==0};
tmax=50;
rozwiazanie=NDSolve[tosolve,{q1,q2,q3,q4},{t,0,tmax},Method->Automatic,MaxSteps->100000];

Zobaczmy co dostalismy

Plot[Evaluate[{q1[t],q2[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 1]),(Subscript[q, 2])"}]
Plot[Evaluate[{q3[t],q4[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 3]),(Subscript[q, 4])"}]

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.05]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Jak wygladaja sterowania?

Plot[Evaluate[{lstrona[[1]],lstrona[[2]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 1]),(Subscript[u, 2])"}]
Plot[Evaluate[{lstrona[[3]],lstrona[[4]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 3]),(Subscript[u, 4])"}]

A bledy?

Plot[Evaluate[(q[t]-qd[t])/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[e, 1]),(Subscript[e, 2]),(Subscript[e, 3]),(Subscript[e, 4])"}]
Plot[Evaluate[(((q[t]-qd[t]).(q[t]-qd[t]))//Sqrt)/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","|e|"}] (* alternatywnie Plus@@((q[t]-qd[t])^2)//Sqrt *)

Algorytm sterowania przy nieznanym modelu - algorytm Qu-Dorseya

Zapnijmy sprzezenie zwrotne u=-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))

rowlin=rowdyn;
Kp=5000IdentityMatrix[4];
Kd=1000IdentityMatrix[4];
qd[t_]={0,0,0,0};
sterV=-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t]);
sprzezenie=Apply[Rule,{u[t],sterV}//Transpose,1]

i rozwiazujemy

tosolve = (rowlin/.sprzezenie) ~Join~{q1[0]==Pi/4,q2[0]==0,q3[0]==0.01,q4[0]==-0.02,q1'[0]==0,q2'[0]==0,q3'[0]==1,q4'[0]==-0.5};

tmax=50;
rozwiazanie=NDSolve[tosolve,{q1,q2,q3,q4},{t,0,tmax},Method->Automatic,MaxSteps->100000];

Zobaczmy co dostalismy

tmax=20;

Plot[Evaluate[{q1[t],q2[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 1]),(Subscript[q, 2])"}]
Plot[Evaluate[{q3[t],q4[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 3]),(Subscript[q, 4])"}]

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.05]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Jak wygladaja sterowania?

Plot[Evaluate[{lstrona[[1]],lstrona[[2]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 1]),(Subscript[u, 2])"}]
Plot[Evaluate[{lstrona[[3]],lstrona[[4]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 3]),(Subscript[u, 4])"}]

A mielismy...

rowlin=rowdyn/.Apply[Rule,{u[t],sterU}//Transpose,1];
Kp=0.1IdentityMatrix[4];
Kd=0.4IdentityMatrix[4];
qd[t_]={0,0,0,0};
sterV=qd''[t]-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t]);
sprzezenie=Apply[Rule,{v[t],sterV}//Transpose,1]

Pełny regulator PID

Zapnijmy sprzezenie zwrotne u=-(Subscript[K, d])((OverDot[q])-(Subscript[(OverDot[q]), d]))-(Subscript[K, p])(q-(Subscript[q, d]))-(Subscript[K, i])(∫_0^t)(q-(Subscript[q, d]))dt  bez linearyzacji (Qu-Dorsey)

rowlin=rowdyn;
Kp=5000IdentityMatrix[4];
Kd=1000IdentityMatrix[4];
Ki=1000 IdentityMatrix[4];
qd[t_]={0,0,0,0};
sterV=-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t])-Ki.Integrate[q[t]-qd[t],{t,0,t}];
sprzezenie=Apply[Rule,{u[t],sterV}//Transpose,1]

i rozwiazujemy

tosolve = (rowlin/.sprzezenie) ~Join~{q1[0]==Pi/4,q2[0]==0,q3[0]==0.01,q4[0]==-0.02,q1'[0]==0,q2'[0]==0,q3'[0]==1,q4'[0]==-0.5};

tmax=50;
rozwiazanie=NDSolve[tosolve,{q1,q2,q3,q4},{t,0,tmax},Method->Automatic,MaxSteps->100000];

Zobaczmy co dostalismy

tmax=20;

Plot[Evaluate[{q1[t],q2[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 1]),(Subscript[q, 2])"}]
Plot[Evaluate[{q3[t],q4[t]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[q, 3]),(Subscript[q, 4])"}]

manip=PlotPlanar[q[t]/.rozwiazanie[[1]],0,tmax,0.05]

toanim=Map[Show[Graphics[#],PlotRange->FullOptions[manip,PlotRange],AspectRatio->Automatic,
           Axes->True,AxesOrigin->{0,0},AxesLabel->{"y1","y2"}]&,links,1];

Animate[toanim[[x]],{x,1,Length[toanim],1},AnimationRate->25]

Jak wygladaja sterowania?

Plot[Evaluate[{lstrona[[1]],lstrona[[2]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 1]),(Subscript[u, 2])"}]
Plot[Evaluate[{lstrona[[3]],lstrona[[4]]}/.rozwiazanie],{t,0,tmax},PlotRange->All,AxesLabel->{"t","(Subscript[u, 3]),(Subscript[u, 4])"}]

A analogiczny PID  przy metodzie obliczanego momentu (dokładna linearyzacja)

rowlin=rowdyn/.Apply[Rule,{u[t],sterU}//Transpose,1];
Kp=0.1IdentityMatrix[4];
Kd=0.4IdentityMatrix[4];
Ki=0.5 IdentityMatrix[4];
qd[t_]={0,0,0,0};
sterV=qd''[t]-Kd.(q'[t]-qd'[t])-Kp.(q[t]-qd[t])-Ki.Integrate[q[t]-qd[t],{t,0,t}];
sprzezenie=Apply[Rule,{v[t],sterV}//Transpose,1]

A gdy robot cos trzyma w efektorze i jest tarcie...

masy={5,4,3,2.5};
Wybierz[x_]:=Part[DHParameters[[x]]]
fullDH=Map[Wybierz,Table[i,{j,Length[DHParameters]},{i,j}]];
fullkinem[x_]=(Map[Transpose[Kinematyka[#]][[4,{1,2}]]&,fullDH]//Simplify)/.{q1->x[[1]],q2->x[[2]],q3->x[[3]],q4->x[[4]]};
polh=Transpose[fullkinem[q[t]]][[2]];
Ep=Plus@@(10 masy polh);(* @@synonim Apply,10-przyspieszenie ziemskie*)pr=D[fullkinem[q[t]],t];
pr2=Plus@@Transpose[pr pr];
Ek=Plus@@(1/2 masy pr2);
Lagr=Ek-Ep;
lstrona=D[Dvec[Lagr,q'[t]],t]-Dvec[Lagr,q[t]];
tarcie=1 D[q[t],t];
lstrona+=tarcie;
rowdyn=Apply[Equal,Transpose[{lstrona,u[t]}],1];

I powtarzamy dokładną linearyzację i Qu-Dorseya

Potrojne wahadlo - dla slabszych komputerow :)

Wyliczenie dynamiki manipulatora z formalizmu E-L

Parametry manipulatora

q[t_]={q1[t],q2[t],q3[t]};
u[t_]={u1[t],u2[t],u3[t]};
DHParameters ={{q1,0,6,0},{q2,0,3,0},{q3,0,4,0}};
masy={5,4,3}

I idziemy do przykladu z poczwornym wahadlem :P