Techniki komputerowe
     w robotyce

Wykład VIII

Mathematica – przykłady robotyczne
Kinematyka stacjonarnie

Robert Muszyński

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

Copyright 2009-2026 RM

Kinematyka manipulatorów

Podwójne wahadło

Kinematyka i jakobian manipulatora

y=k(x)
 y’=J(x)x’

vx={x1,x2};
kin[{x1_,x2_}]={6 Cos[x1]+4 Cos[x1+x2],6 Sin[x1]+4 Sin[x1+x2]};
MF=MatrixForm;
kin[vx]//MF

jak[{x1_,x2_}]=D[kin[vx],{vx}]; 
jak[vx]//MF
jak[{x1_,x2_}]={D[kin[vx],x1],D[kin[vx],x2]}//Transpose;
jak[vx]//MF

Zadanie kinematyki odwrotnej – metoda jakobianowa

Równania y’=J(x)x’ ⟶ x'=(J^(-1))(x)y'

x[t_]={x1[t],x2[t]};
yd[t_]={7 Cos[t],6 Sin[t]+2};    (* trajektoria zadana *)
lstrona=x'[t];
pstrona=Inverse[jak[x[t]]].(∂_t)yd[t];
rown=Thread[lstrona==pstrona]
LeafCount[rown]
rown//TreeForm

Jak to działa

ls={a,b,c}
ps={d,e,f}
Thread[ls==ps]//MF
(ls==ps)//MF   (* alternatywnie równanie wektorowe *)
MF/@(ls==ps)    (* wtedy wyświetlamy tak *)

Rozwiązujemy

init=x[0]=={-.5,1};
(tosolve ={ rown,init})//MF
(tosolve = rown~Join~{init})//MF
tmax = 20;
rozw=NDSolve[tosolve,vx,{t,0,tmax}]//First

Oglądamy

SetOptions[Plot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
SetOptions[ParametricPlot,{AxesStyle->Directive[FontSize->15],FrameStyle->Directive[FontSize->15]}];
Plot[Evaluate[x[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","x"},PlotLegends->{"(Subscript[x, 1])","(Subscript[x, 2])"}]
trajy=ParametricPlot[Evaluate[kin[{x1[t],x2[t]}]/.rozw],{t,0,tmax},AxesLabel->{"x","y"}]

trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red];
Show[trajy,trajzad,PlotRange->All]

Ślad ruchu manipulatora

plotPendul[{cx1_,cx2_},tmin_,tmax_,step_]:=Module[{ramie,manip,links,i,tmpx1,tmpx2},
ramie[2,{x1_,x2_}]:=kin[{x1,x2}];
     ramie[1,{x1_,x2_}]:={6 Cos[x1],6Sin[x1]};manip[x1_,x2_]:=Line[{{0,0},ramie[1,{x1,x2}],ramie[2,{x1,x2}]}];
links={};
For[i=tmin,i≤tmax,i+=step,                                  (* Błąd w sztuce *)
tmpx1=cx1/.t->i;
tmpx2=cx2/.t->i;
AppendTo[links,manip[tmpx1,tmpx2]]];
Show[Graphics[links],PlotRange->All,Axes->True,AxesLabel->{"y1","y2"}]] 
slad=plotPendul[x[t]/.rozw,0,tmax,0.2]
czasFor=plotPendul[x[t]/.rozw,0,tmax,0.002]//Timing//First
ClearSystemCache[];
pamiecFor=MaxMemoryUsed[plotPendul[{x1[t],x2[t]},0,20,0.002]]
UnitConvert[Quantity[pamiecFor//N,"Bytes"],"Megabytes"]

plotPendul[{cx1_,cx2_},tmin_,tmax_,step_]:=   (*wersja z With *)
Module[{ramie,manip,links,i},
ramie[2,{x1_,x2_}]:=kin[{x1,x2}];
     ramie[1,{x1_,x2_}]:={6 Cos[x1],6Sin[x1]};manip[x1_,x2_]:=Line[{{0,0},ramie[1,{x1,x2}],ramie[2,{x1,x2}]}];
links={};
For[i=tmin,i≤tmax,i+=step,
With[{tmpx1=cx1/.t->i,tmpx2=cx2/.t->i},AppendTo[links,manip[tmpx1,tmpx2]]]];
Show[Graphics[links],PlotRange->All,Axes->True,AxesLabel->{"y1","y2"}]] 
slad=plotPendul[x[t]/.rozw,0,tmax,0.2]
czasWith=plotPendul[x[t]/.rozw,0,tmax,0.002]//Timing//First
ClearSystemCache[];
pamiecWith=MaxMemoryUsed[plotPendul[{x1[t],x2[t]},0,20,0.002]]
UnitConvert[Quantity[pamiecWith//N,"Bytes"],"Megabytes"]

plotPendul[{cx1_,cx2_},tmin_,tmax_,step_]:=   (*wersja z Table *)
Module[{ramie,manip,links},
ramie[2,{x1_,x2_}]:=kin[{x1,x2}];
     ramie[1,{x1_,x2_}]:={6 Cos[x1],6Sin[x1]};manip[x1_,x2_]:=Line[{{0,0},ramie[1,{x1,x2}],ramie[2,{x1,x2}]}];links=Table[With[{tmpx1=cx1/. t->i,tmpx2=cx2/. t->i},manip[tmpx1,tmpx2]],{i,tmin,tmax,step}];
Show[Graphics[links],
PlotRange->All,Axes->True,AxesLabel->{"y1","y2"}]] 
slad=plotPendul[x[t]/.rozw,0,tmax,0.2]
czasTable=plotPendul[x[t]/.rozw,0,tmax,0.002]//Timing//First
ClearSystemCache[];
pamiecTable=MaxMemoryUsed[plotPendul[{x1[t],x2[t]},0,20,0.002]]
UnitConvert[Quantity[pamiecTable//N,"Bytes"],"Megabytes"]

plotPendul[{cx1_,cx2_},tmin_,tmax_,step_]:=   (*wersja z Apply *)
Module[{ramie,manip,links},
ramie[2,{x1_,x2_}]:=kin[{x1,x2}];
ramie[1,{x1_,x2_}]:={6 Cos[x1],6 Sin[x1]};
manip[x1_,x2_]:=Line[{{0,0},ramie[1,{x1,x2}],ramie[2,{x1,x2}]}];
links=Table[manip@@({cx1,cx2}/.t->i),{i,tmin,tmax,step}];
Show[Graphics[links],PlotRange->All,Axes->True,AxesLabel->{"y1","y2"}]]
slad=plotPendul[x[t]/.rozw,0,tmax,0.2]
czasApply=plotPendul[x[t]/.rozw,0,tmax,0.002]//Timing//First
ClearSystemCache[];
pamiecApply=MaxMemoryUsed[plotPendul[{x1[t],x2[t]},0,20,0.002]]
UnitConvert[Quantity[pamiecApply//N,"Bytes"],"Megabytes"]

plotPendul[{cx1_,cx2_},tmin_,tmax_,step_]:=   (*wersja bez links *)
Module[{ramie,manip},
ramie[2,{x1_,x2_}]:=kin[{x1,x2}];
ramie[1,{x1_,x2_}]:={6 Cos[x1],6 Sin[x1]};
manip[x1_,x2_]:=Line[{{0,0},ramie[1,{x1,x2}],ramie[2,{x1,x2}]}];
Show[Graphics[Table[manip@@({cx1,cx2}/.t->i),{i,tmin,tmax,step}]],
PlotRange->All,Axes->True,AxesLabel->{"y1","y2"}]]
slad=plotPendul[x[t]/.rozw,0,tmax,0.2]
czasBezlinks=plotPendul[x[t]/.rozw,0,tmax,0.002]//Timing//First
ClearSystemCache[];
pamiecBezlinks=MaxMemoryUsed[plotPendul[{x1[t],x2[t]},0,20,0.002]]
UnitConvert[Quantity[pamiecBezlinks//N,"Bytes"],"Megabytes"]

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

Animacja ruchu manipulatora

animPendul[{cx1_,cx2_},tmin_,tmax_,step_]:=
Module[{ramie,manip,fullPlotRange},
ramie[2,{x1_,x2_}]:=kin[{x1,x2}];
ramie[1,{x1_,x2_}]:={6 Cos[x1],6 Sin[x1]};
manip[x1_,x2_]:=Line[{{0,0},ramie[1,{x1,x2}],ramie[2,{x1,x2}]}];
fullPlotRange=  (* wyznaczamy obszar animacji *)AbsoluteOptions[                                          (* generujemy tylko część vvvvvv *)Graphics[Table[manip@@({cx1,cx2}/.t->i),{i,tmin,tmax,10 step}]],PlotRange];
Animate[Graphics[{Thick,Blue,manip@@({cx1,cx2}/.t->i)},
fullPlotRange,Axes->True,AxesLabel->{"y1","y2"}],{i,tmin,tmax,step},AnimationRate->1, AnimationRunning->False]]
animPendul[x[t]/.rozw,0,tmax,0.05]

animPendul[{cx1_,cx2_},tmin_,tmax_,step_]:=   (* wersja z links *)
Module[{ramie,manip,fullPlotRange,links},
ramie[2,{x1_,x2_}]:=kin[{x1,x2}];
ramie[1,{x1_,x2_}]:={6 Cos[x1],6 Sin[x1]};
manip[x1_,x2_]:=Line[{{0,0},ramie[1,{x1,x2}],ramie[2,{x1,x2}]}];
links=Table[manip@@({cx1,cx2}/.t->i),{i,tmin,tmax,step}];
fullPlotRange= AbsoluteOptions[Graphics[links],PlotRange]; (* wyznaczamy obszar animacji *)
Animate[Graphics[{Thick,Blue,links[[i]]},
fullPlotRange,Axes->True,AxesLabel->{"y1","y2"}],{i,1,Length[links],1},AnimationRate->20, AnimationRunning->False]]
animPendul[x[t]/.rozw,0,tmax,0.05]

Obserwacja:
Skoro generujemy wszystkie klatki by wyznaczyć zakres wyświetlania grafiki, to może lepiej zapamiętać je a potem już tylko wyświetlać w Animate? Mniej obciąża procesor w trakcie samej animacji, ale wymaga więcej pamięci. A może zrezygnować z automatycznego wyliczania zakresu? I wtedy jak? Z listą links czy bez niej?

Pytanie:
I jeszcze kwestia zmiennych lokalnych, które powodują, że po wczytaniu notebooka z pliku bez ponownego wyliczenia Animate nie wyświetla animacji poprawnie – tu pomoże „wstrzykiwanie” kodu za pomocą funkcji With (ale stosujemy takie rozwiązanie raczej tylko, gdy ta cecha jest ważna, np. przy przygotowywaniu prezentacji:).

animPendul[{cx1_,cx2_},tmin_,tmax_,step_,dodatki_:Graphics[]]:=Module[{manipFunction,fullPlotRange},
manipFunction=Function[{x1,x2},Line[{{0,0},{6 Cos[x1],6 Sin[x1]},kin[{x1,x2}]}]];
fullPlotRange=AbsoluteOptions[Graphics[Table[manipFunction@@({cx1,cx2}/. t->i),{i,tmin,tmax,10 step}]],PlotRange];
With[{manipLoc=manipFunction,plotRangeLoc=fullPlotRange,dodatkiLoc=dodatki},Animate[Show[Graphics[{Thick,Blue,manipLoc@@({cx1,cx2}/. t->i)}],dodatkiLoc,plotRangeLoc,Axes->True,AxesLabel->{"y1","y2"}],
{i,tmin,tmax,step},AnimationRate->1,AnimationRunning->False,SaveDefinitions->True,TrackedSymbols:>{i},Initialization:>(i=0)]]]
animPendul[x[t]/.rozw,0,tmax,0.05,{trajy,trajzad}]

Błąd, wyznacznik jakobianu, przyspieszenia w przegubach

blad[t_]=Norm[yd[t]-kin[x[t]]]/.rozw;
Plot[Evaluate[blad[t]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","ey"}]
{maxVal,maxTime}=NMaximize[{blad[t],0≤t≤tmax},t]
{minVal,minTime}=NMinimize[{blad[t],0≤t≤tmax},t]
maxVal-minVal

Plot[Evaluate[Det[jak[x[t]/.rozw]]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[x''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","x''"},PlotLegends->{"(Subscript[x, 1])''","(Subscript[x, 2])''"}]

Początkowe przyspieszenia

Plot[Evaluate[x''[t]/.rozw],{t,0,0.001},PlotRange->All,AxesLabel->{"t","x''"},PlotLegends->{"(Subscript[x, 1])''","(Subscript[x, 2])''"}] 
Plot[Evaluate[x[t]/.rozw],{t,0,0.001},PlotRange->All,AxesLabel->{"t","x"},PlotLegends->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

Algorytm Newtona

y’=J(x)x’ ⟶ x'=(J^(-1))(x)(y'+α((Subscript[y, d])-k(x)))

yd[t_]={7 Cos[t],6 Sin[t]+2};    (* trajektoria zadana *)
pstrona=Inverse[jak[x[t]]].((∂_t)yd[t]+α(yd[t]-kin[x[t]]))/.α->1;
rown=Thread[x'[t]==pstrona]  (* i wracamy do tosolve *)

2D na czysto

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;

Do wizualizacji

plotPendul[{cx1_,cx2_},tmin_,tmax_,step_]:=Module[{ramie,manip,links},
ramie[2,{x1_,x2_}]:=kin[{x1,x2}];
     ramie[1,{x1_,x2_}]:={6 Cos[x1],6Sin[x1]};manip[x1_,x2_]:=Line[{{0,0},ramie[1,{x1,x2}],ramie[2,{x1,x2}]}];links=Table[With[{tmpx1=cx1/. t->i,tmpx2=cx2/. t->i},manip[tmpx1,tmpx2]],{i,tmin,tmax,step}];
Show[Graphics[links],PlotRange->All,Axes->True,AxesLabel->{"y1","y2"}]] 
animPendul[{cx1_,cx2_},tmin_,tmax_,step_,dodatki_:Graphics[]]:=Module[{manipFunction,fullPlotRange},
manipFunction=Function[{x1,x2},Line[{{0,0},{6 Cos[x1],6 Sin[x1]},kin[{x1,x2}]}]];
fullPlotRange=AbsoluteOptions[Graphics[Table[manipFunction@@({cx1,cx2}/. t->i),{i,tmin,tmax,10 step}]],PlotRange];
With[{manipLoc=manipFunction,plotRangeLoc=fullPlotRange,dodatkiLoc=dodatki},Animate[Show[Graphics[{Thick,Blue,manipLoc@@({cx1,cx2}/. t->i)}],dodatkiLoc,plotRangeLoc,Axes->True,AxesLabel->{"y1","y2"}],
{i,tmin,tmax,step},AnimationRate->1,AnimationRunning->False,SaveDefinitions->True,TrackedSymbols:>{i},Initialization:>(i=0)]]]

Definicja manipulatora

Wektor konfiguracyjny,kinematyka, jakobian

vx={x1,x2};
x[t_]={x1[t],x2[t]};
kin[{x1_,x2_}]={6 Cos[x1]+4 Cos[x1+x2],6 Sin[x1]+4 Sin[x1+x2]};
jak[{x1_,x2_}]=D[kin[vx],{vx}];

Symulacje

Trajektoria zadana

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

Inne trajektorie zadane

yd1[t_]={2 Cos[t]+8,4 Sin[t]};   (* osobliwa *)
yd2[t_]={2 Cos[t],4 Sin[t]+6};   (* osobliwa *)
yd3[t_]={2 Cos[2t+π/2],3 Sin[ t]+7};   (* osobliwa *)
yd4[t_]={2 Cos[t]+8.1,4 Sin[t]};  (* nieosiągalna *)
yd[t_]=yd1[t];

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[2]];ε=(10^(-3));

Algorytm Newtona   x'=(J^(#))(x)(y'+α((Subscript[y, d])-k(x))), rozwiązanie

pstrona=mInv[jak[x[t]]].((∂_t)yd[t]+α(yd[t]-kin[x[t]]))/.α->3;
rown=Thread[x'[t]==pstrona];
init=x[0]=={-.5,1};
tmax=20;
rozw=NDSolve[{ rown,init},{x1,x2},{t,0,tmax}]//First;
tkon=(x1/.rozw)["Domain"][[1,2]]
LeafCount[rown]

Wizualizacja

tmax=tkon;
Plot[Evaluate[x[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","x1"},PlotLegends->{"(Subscript[x, 1])''","(Subscript[x, 2])''"}]
trajy=ParametricPlot[Evaluate[kin[{x1[t],x2[t]}]/.rozw],{t,0,tmax},AxesLabel->{"x","y"}];
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},AxesLabel->{"x","y"},PlotStyle->Red];
slad=plotPendul[x[t]/.rozw,0,tmax,0.2];
Show[trajy,trajzad,slad,PlotRange->All]

animPendul[x[t]/.rozw,0,tmax,0.05,{trajy,trajzad}]

Błąd, wyznacznik jakobianu, przyspieszenia w przegubach

blad[t_]=Norm[yd[t]-kin[{x1[t],x2[t]}]];
Plot[Evaluate[blad[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","ey"}]
{maxVal,maxTime}=NMaximize[{blad[t]/.rozw,0≤t≤tmax},t]
{minVal,minTime}=NMinimize[{blad[t]/.rozw,0≤t≤tmax},t]
maxVal-minVal

Plot[Evaluate[Det[jak[x[t]/.rozw]]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[x''[t]/.rozw],{t,0,tmax},AxesLabel->{"t","x''"},PlotLegends->{"(Subscript[x, 1])''","(Subscript[x, 2])''"}]   
Plot[Evaluate[x''[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","x''"},PlotLegends->{"(Subscript[x, 1])''","(Subscript[x, 2])''"}]   
NMaximize[{Abs[x2''[t]/.rozw],0≤t≤tmax},t]

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

przedzialy={{0,0.001},{0.1,tmax},{2π-0.001,2π+0.001},
{4π-0.001,4π+0.001},{4.65,4.75},{5,tmax},
{5,tmax-0.1}};
{tp,tk}=przedzialy[[1]];
Plot[Evaluate[Det[jak[x[t]/.rozw]]],{t,tp,tk},AxesLabel->{"t","Det J"}]
Plot[Evaluate[x''[t]/.rozw],{t,tp,tk},PlotRange->All,AxesLabel->{"t","x''"},PlotLegends->{"(Subscript[x, 1])''","(Subscript[x, 2])''"}] 
NMaximize[{Abs[x2''[t]/.rozw],tp≤t≤tk},t]
Plot[Evaluate[x'[t]/.rozw],{t,tp,tk},PlotRange->All,AxesLabel->{"t","x'"},PlotLegends->{"(Subscript[x, 1])'","(Subscript[x, 2])'"}]   
Plot[Evaluate[blad[t]]/.rozw,{t,tp,tk},PlotRange->All,AxesLabel->{"t","ey"}]

NMaximize szuka maksimum na własnej siatce, użyjemy siatki z NDSolve

d2x2[t_]=Derivative[2][x2/.rozw][t];              (* wyliczamy przyspieszenia *)
siatka=(x2/.rozw)["Grid"]//Flatten;               (* pobieramy siatkę NDSolve *)
{pocz,koniec}={0,tmax};                                                         (* wybierz fragment *)
siatka=Select[siatka,pocz≤#≤koniec&];
accNaSiatce=Table[{t,Abs[d2x2[t]]},{t,siatka}];(* obliczamy przyspieszenia *)
MaximalBy[accNaSiatce,Last]                                                     (* szukamy maks *)

Algorytm Denavita-Hartenberga

DHPar=={{(Subscript[θ, 1,])(Subscript[d, 1]),(Subscript[a, 1]),(Subscript[α, 1])},{(Subscript[θ, 2,])(Subscript[d, 2]),(Subscript[a, 2]),(Subscript[α, 2])},...{(Subscript[θ, n,])(Subscript[d, n]),(Subscript[a, n]),(Subscript[α, n])}}  ⟵ lista parametrów 

(Subscript[A, i-1]^i)==Rot(Z,(Subscript[θ, i]))Trans(Z,(Subscript[d, i]))Trans(X,(Subscript[a, i]))Rot(X,(Subscript[α, i]))   ⟵ macierze transformacji
K(q) =(∑_(i=1)^n)(Subscript[(A^i), i-1])==(Subscript[A, 0]^1)(Subscript[A, 1]^2)...(Subscript[A, n-1]^n)                                    ⟵ kinematyka

q =((((Subscript[q, 1]),(Subscript[q, 2]),...,(Subscript[q, n])))^T)     ⟵ wektor konfiguracji
(Subscript[q, i])=(Subscript[θ, i])                            ⟵ przegub obrotowy
(Subscript[q, i])=(Subscript[d, i])                           ⟵ przegub przesuwny

Macierze bazowe

MF=MatrixForm;
tX=RotationTransform[q,{1,0,0}].TranslationTransform[{d,0,0}]
vv={x,y,z};
tX[vv]//MF

transX[q_,d_] :=RotationTransform[q,{1,0,0}].TranslationTransform[{d,0,0}]//TransformationMatrix;
transZ[q_,d_] :=(RotationTransform[q,{0,0,1}].TranslationTransform[{0,0,d}])//TransformationMatrix;
transX[a,b]//MF
transZ[a,b]//MF

Kinematyka

kinematyka[DHPar_List:{}] :=
	Module[{i,dof,mkinem},
	    mkinem =   IdentityMatrix[4];
		    dof = Length[DHPar];
    For[i=1, i<= dof, i++,                                  (* Błąd w sztuce *)
         mkinem =  mkinem . 
                       transZ[ DHPar[[i,1]], DHPar[[i,2]] ] .
                       transX[ DHPar[[i,4]], DHPar[[i,3]] ] ;];
		mkinem
		];

DHPar ={{q1,0,l1,0},{q2,0,l2,0}};
(mKin=DHPar//kinematyka//Simplify)//MF

Można tak

MF/@Map[transZ[#[[1]],#[[2]]].transX[#[[4]],#[[3]]]&,DHPar]   (* /@ – zastosuj na 1 poziomie *)
Fold[Dot,start,{m1,m2,m3}]       (* Co robi Fold i FoldList *)
FoldList[Dot,start,{m1,m2,m3}]

kinematyka[DHPar_List:{}] :=
	Module[{mAij},
mAij=Map[transZ[#[[1]],#[[2]]].transX[#[[4]],#[[3]]]&,DHPar];
(*lepiej zapisać: mAij=transZ[#[[1]],#[[2]]].transX[#[[4]],#[[3]]]&/@DHPar *)
Fold[Dot,IdentityMatrix[4],mAij]
		];

kinematyka[DHParameters_List:{}]:=    (* @@@ – zastosuj na „drugim” poziomie *)
Fold[Dot,IdentityMatrix[4],transZ[#1,#2].transX[#4,#3]&@@@DHParameters];
(mKin=DHPar//kinematyka)//MF

Jak to działa

{{(Subscript[x, 1]),(Subscript[x, 2])},{(Subscript[y, 1]),(Subscript[y, 2])}}//f    (* Postfix – po prostu funkcja *)
f/@{{(Subscript[x, 1]),(Subscript[x, 2])},{(Subscript[y, 1]),(Subscript[y, 2])}}    (* Map – zastosuj do składowych wyrażenia na 1 poziomie *)
f@@{{(Subscript[x, 1]),(Subscript[x, 2])},{(Subscript[y, 1]),(Subscript[y, 2])}}    (* Apply – zamień nagłówek wyrażenia *) 
f@@@{{(Subscript[x, 1]),(Subscript[x, 2])},{(Subscript[y, 1]),(Subscript[y, 2])}}  (* MapApply – zamień nagłówki składowych wyrażenia na 1 poziomie *)

Kinematyka we współrzędnych

mKin[[All,4]]//MF
vx={x1,x2};   (* współrzędne w przestrzeni wewnętrznej *)
kin[{q1_,q2_}]=mKin[[{1,2},4]]//Simplify   (* współrzędne w przestrzeni zewnętrznej *)
kin[{a,b}]//MF

Jakobian analityczny

Czy funkcja D robi co trzeba

pq={q1,q2}; pkin={k1[pq],k2[pq],k3[pq]};
D[pkin,{pq}]//MF
Attributes[D]
(∂_({pq}))pkin//MF

Nie wszystko co „wygląda” tak samo, robi to samo

(∂_({x,y})){f[x,y],g[x,y]}
(∂_({xy})){f[x,y],g[x,y]}
(∂_({x,y})){f[x,y],g[x,y]}   (* ,\["InvisibleComa"][Esc],[Esc]" *)

Zróbmy po swojemu

(* Chcemy mieć D[pkin,pq] *)
vecD[f_List,x_List]:=D[f,{x}]   (* alias *)
(* skalar po wektorze *)
vecD[pkin,pq]//MF
vecD[f_List,x_List]:=Table[D[f[[i]],{x}],{i,Length[f]}]
vecD[pkin,pq]//MF
vecD[f_List,x_List]:=Map[D[#,{x}]&,f]
vecD[pkin,pq]//MF
vecD[f_List,x_List]:=D[#,{x}]&/@f
vecD[pkin,pq]//MF
(* skalar po skalarze *)
vecD[f_List,x_List]:=Table[D[f[[i]],x[[j]]],{i,Length[f]},{j,Length[x]}]
vecD[pkin,pq]//MF
vecD[f_List,x_List]:=Function[skalarF,D[skalarF,#]&/@x]/@f
vecD[pkin,pq]//MF
vecD[f_List,x_List]:=Outer[D,f,x]
vecD[pkin,pq]//MF
Outer[f,{a,b,c},{d,e,f}]//MF

Definicja jakobianu analitycznego (korzystamy z wersji 1)

jakobianAnalit[k_List,x_List]:=D[k,{x}] 
jak[{x1_,x2_}]=jakobianAnalit[kin[vx],vx];
jak[vx]//MF
jak[{a,b}]//MF

Jak definiować funkcję jak uniwersalnie, na bazie wektora vx

DownValues[jak]                   (* HoldPattern – nie wyliczaj nagłówka *)
Clear[jak]                             (* „manipulując” nagłówkiem funkcji *)
Pattern[#,_]&/@vx       (* dodaje podkreślenia *) 
Evaluate[jak[Pattern[#,_]&/@vx]]=jakobianAnalit[kin[vx],vx];
DownValues[jak]
jak[{a,b}]//MF

DownValues[jak]
Clear[jak]                                     (* uniwersalnie, na bazie wektora vx bez „manipulacji” *)
pureJak=Function[Evaluate[vx],Evaluate[jakobianAnalit[kin[vx],vx]]]
jak[v_List]:=pureJak@@v  (* ale ze zmienną pomocniczą *)
DownValues[jak]
jak[{a,b}]//MF

DownValues[jak]
With[{tmpJak=jakobianAnalit[kin[vx],vx]},(* bez zmiennej pomocniczej, ale...*)
jak[v_List]:=tmpJak/.Thread[vx->v]]           (* wymagające podstawień reguł *)
DownValues[jak]                                                                  (* przy każdym wywołaniu *)
jak[{a,b}]//MF

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

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[2]];ε=(10^(-3));

Do wizualizacji

Obserwacja:
W funkcji plotPendul z użyciem With wstrzykiwaliśmy rozwiązanie modelu (funkcje interpolujące) wielokrotnie w każde miejsce wystąpienia zmiennych konfiguracyjnych w animowanym szkielecie, co może generować bardzo duże pliki notebooka. By temu zapobiec będziemy wyliczać szkielet we wskazanych punktach animacji i w takiej postaci wstrzykiwać go do Animate.

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])"}]
(* !!!! bez resamplingu !!!! *)
animPlaskiStandard[szkielet_List,tmin_,tmax_,step_,dodatki_List:Graphics[]]:=Module[{fullSlad,obszar,fullPlotRange},
  (* wyliczamy obszar animacji – tu tylko co 10 vvvvvv  *)
fullSlad=Table[Line[szkielet],{t,tmin,tmax,10 step}];
obszar=Show[Graphics[fullSlad],Sequence@@dodatki,PlotRange->All];
fullPlotRange=AbsoluteOptions[obszar,PlotRange];
With[{szkieletLoc=szkielet,plotRangeLoc=fullPlotRange,dodatkiLoc=dodatki,t0=tmin,tk=tmax},  (* wstrzykujemy co potrzeba *)Animate[Show[Sequence@@dodatkiLoc,Graphics[{Thick,Blue,Line[szkieletLoc/. t->i],Brown,PointSize[0.02],Point[szkieletLoc/. t->i]}],plotRangeLoc,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])"}],{i,t0,tk,step},AnimationRate->1,AnimationRunning->False,SaveDefinitions->True,TrackedSymbols:>{i},Initialization:>(i=t0)]]]
(* !!!! z resamplingiem !!!! *)
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)]]]]

Sequence – funkcja, która się „rozpuszcza", gdy poczuje się „otoczona” przez inną funkcję.

danel={a,b,c}
f[danel]    (* po prostu funkcja *)
f@danel     (* po prostu funkcja – zapis prefiksowy *)
f@@danel  (* Apply *)
f/@danel  (* Map *)

dane=Sequence[a,b,c]
f[dane]      (* po prostu funkcja *)
f@danel     (* po prostu funkcja – zapis prefiksowy *)
f@@danel  (* Apply *)
f/@danel  (* Map *)

Definicja manipulatora

Wektor konfiguracyjny, parametry Denevita–Hartenberga {(Subscript[θ, i]),(Subscript[d, i]),(Subscript[a, i]),(Subscript[α, i])}

q[t_]={q1[t],q2[t],q3[t],q4[t]}
vq=Head/@q[t]
DHPar ={{q1,0,6,0},{q2,0,4,0},{q3,0,3,0},{q4,0,2,0}};
DHParT ={{q1,0,6,0},{π/2,0,q2,0},{q3,0,3,0},{π/2,0,q2,0}};  (*Co to???*)

Wyliczenie modeli: kinematyki częściowe  (Subscript[A, 0]^1),(Subscript[A, 0]^2),...(Subscript[A, 0]^n)=K,
                                            kinematyki częściowe we współrzędnych  (Subscript[k, 0]^1),(Subscript[k, 0]^2),...(Subscript[k, 0]^n)=k,
                                            jakobian analityczny  J=((∂k)/(∂q))

mKin=DHPar//kinematyki//Simplify;
Clear[kin,jak];
Evaluate[kin[Pattern[#,_]&/@Head/@q[t]]]=mKin[[All,{1,2},4]];   
                                                                (* ^^^^^^^^^^ lepiej vq? *)
Evaluate[jak[Pattern[#,_]&/@vq]]=jakobianAnalit[kin[vq][[-1]],vq];
                                                                             (* -1 – ostatni element ^^^^^ *)
(* Evaluate[jak[Pattern[#,_]&/@vq]] daje jak[{q1_,q2_,q3_,q4}] *)
MF/@kin[vq]
jak[vq]//MF

Symulacje

Trajektoria zadana

yd0[t_]={7 Cos[t],6 Sin[t]+2}; 
yd1[t_]={2 Cos[t]+13,4 Sin[t]};   (* osobliwa *)
yd2[t_]={2 Cos[2t+π/2],3 Sin[ t]+7}; 
yd3[t_]={2 Cos[t]+14.,6 Sin[t]};  (* nieosiągalna *)
yd[t_]=yd0[t];

Algorytm Newtona   x'=(J^(#))(x)(y'+α((Subscript[y, d])-k(x))), rozwiązanie

pstrona=mInvPseudo[jak[q[t]]].((∂_t)yd[t]+α(yd[t]-kin[q[t]][[-1]]))/.α->3;
rown=Thread[q'[t]==pstrona];
init=q[0]=={-.5,1,1,1};
tmax=20;
rozw=NDSolve[{ rown,init},vq,{t,0,tmax}];
tkon=(q1/.rozw[[1]])["Domain"][[1,2]]
LeafCount[rown]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
trajy=ParametricPlot[Evaluate[kin[q[t]][[-1]]/.rozw],{t,0,tmax}]
trajzad=ParametricPlot[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red]
slad=plotPlaski[kin[q[t]]/.rozw[[1]],0,tmax,0.2];
Show[trajy,trajzad,slad,PlotRange->All]

animPlaski[kin[q[t]]/.rozw[[1]],0,tmax,0.05,{trajy,trajzad}]

Błąd, wyznacznik jakobianu, przyspieszenia w przegubach

blad[t_]=Norm[yd[t]-kin[q[t]][[-1]]]/.rozw[[1]];
Plot[Evaluate[blad[t]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","ey"}]
Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw[[1]]]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw[[1]]],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw[[1]]],{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},{5,tmax},{5,tmax-0.1}};
{tp,tk}=przedzialy[[1]];
Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw[[1]]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","Det \!\(\*SuperscriptBox[SuperscriptBox[\(JJ\), \(T\)], SuperscriptBox[\(\), \(\[Placeholder]\)]]\)"}]
Plot[Evaluate[q''[t]/.rozw[[1]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]] 
Plot[Evaluate[q'[t]/.rozw[[1]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]   
Plot[Evaluate[blad[t]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","ey"}]

To samo 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

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

plotManip[szkielet_List,tmin_,tmax_,step_]:=Show[Graphics3D[Table[{Line[szkielet],Brown,PointSize[0.02],Point[szkielet] },{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]]]}],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

Wektor konfiguracyjny, parametry Denevita-Hartenberga

q[t_]={q1[t],q2[t],q3[t],q4[t]};
vq=Head/@q[t]
DHPar ={{q1,2,6,0},{q2,3,4,π/2},{q3,1,3,0},{q4,1,2,0}};
DHParT ={{q1,0,6,0},{π/2,0,q2,0},{q3,0,3,0},{π/2,0,q2,0}};

Wyliczenie modeli: kinematyki częściowe,jakobian analityczny

tmax=20;
mKin=DHPar//kinematyki//Simplify;
Clear[kin,jak];
Evaluate[kin[Pattern[#,_]&/@vq]]=mKin[[All,{1,2,3},4]];   
Evaluate[jak[Pattern[#,_]&/@vq]]=jakobianAnalit[kin[vq][[-1]],vq];
MF/@kin[vq]
plotManip[kin[{0,0,0,0}],0,tmax,0.2]

Symulacje

Trajektoria zadana

yd0[t_]={2 Cos[t]+3,5 Sin[t],1}; 
yd1[t_]={2 Cos[t]+3,5 Sin[t],3+2 Sin[2 t]};
yd2[t_]={2 Cos[3t+π],3 Sin[ t]+7,3+2 Sin[7t+π/3]}; 
yd3[t_]={2 Cos[t]+3.,10 Sin[t],3+2 Sin[7t+π/3]}; 
yd[t_]=yd0[t];
trajzad=ParametricPlot3D[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,PlotRange->All]

Algorytm Newtona   x'=(J^(#))(x)(y'+α((Subscript[y, d])-k(x))), rozwiązanie

pstrona=mInvPseudo[jak[q[t]]].((∂_t)yd[t]+α(yd[t]-kin[q[t]][[-1]]))/.α->3;
rown=Thread[q'[t]==pstrona];
init={q[0]=={-.5,1,1,1}};
tmax=20;
rozw=NDSolve[{ rown,init},vq,{t,0,tmax}];
tkon=(q1/.rozw[[1]])["Domain"][[1,2]]
LeafCount[rown]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
trajy=ParametricPlot3D[Evaluate[kin[q[t]][[-1]]/.rozw],{t,0,tmax},PlotRange->All]
trajzad=ParametricPlot3D[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,PlotRange->All]
slad=plotManip[kin[q[t]]/.rozw[[1]],0,tmax,0.2];
Show[trajy,trajzad,slad,PlotRange->All]

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

Błąd, wyznacznik jakobianu, przyspieszenia w przegubach

blad[t_]=Norm[yd[t]-kin[q[t]][[-1]]]/.rozw[[1]];
Plot[Evaluate[blad[t]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","ey"}]
Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw[[1]]]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw[[1]]],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw[[1]]],{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.0001},{2π-0.01,2π+0.01},{4π-0.001,4π+0.001},
{4.65,4.75},{0,5},{5,tmax},
{5,tmax-0.1}};
{tp,tk}=przedzialy[[1]];
Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw[[1]]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","Det \!\(\*SuperscriptBox[SuperscriptBox[\(JJ\), \(T\)], SuperscriptBox[\(\), \(\[Placeholder]\)]]\)"}]
Plot[Evaluate[q''[t]/.rozw[[1]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]] 
Plot[Evaluate[q'[t]/.rozw[[1]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]   
Plot[Evaluate[blad[t]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","ey"}]

Poprawiony szkielet

DHPar ={{q1,2,6,0},{q2,3,4,π/2},{q3,1,3,0},{q4,1,2,0}};
kinematykiFull[DHParameters_List: {}]:=  (* z punktami pośrednimi *)Module[{mPosrednie},mPosrednie=Flatten[{transZ[#1,#2],transX[#4,#3]}&@@@DHParameters,1];
FoldList[Dot,IdentityMatrix[4],mPosrednie]]
mKinFull=DHPar//kinematykiFull//Simplify;
Clear[kinFull];
Evaluate[kinFull[Pattern[#,_]&/@vq]]=mKinFull[[All,{1,2,3},4]];   
MF/@kinFull[vq]

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])"}]
animManipStandard[szkielet_List,tmin_,tmax_,step_,dodatki_List: Graphics3D[],rozm_:400]:=Module[{fullSlad,obszar,fullPlotRange},
fullSlad=Table[Line[szkielet],{t,tmin,tmax,10step}];  (* co 10 klatkę tylko *) 
obszar=Show[Graphics3D[fullSlad],Sequence@@dodatki,PlotRange->All];
fullPlotRange=AbsoluteOptions[obszar,PlotRange];
With[{szkieletLoc=szkielet,plotRangeLoc=fullPlotRange,dodatkiLoc=dodatki,rozmLoc=rozm,t0=tmin,tk=tmax},Animate[Show[Sequence@@dodatkiLoc,Graphics3D[{Thick,Blue,Line[szkieletLoc],Brown,PointSize[0.02],Point[szkieletLoc] }],
plotRangeLoc,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])","(Subscript[y, 3])"},ImageSize->rozmLoc],{t,t0,tk,step},AnimationRate->1, AnimationRunning->False,SaveDefinitions->True,TrackedSymbols:>{t},Initialization:>(t=t0)]]]
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)]]]]

animManipOld[szkielet_List,tmin_,tmax_,step_,dodatki_List: Graphics3D[],rozm_:400]:=Module[{fullSlad,obszar,fullPlotRange},      (* tu tylko vvvvvv co 10 *)fullSlad=Table[Line[szkielet],{t,tmin,tmax,10step}];
obszar=Show[Graphics3D[fullSlad],Sequence@@dodatki,PlotRange->All];
fullPlotRange=AbsoluteOptions[obszar,PlotRange];
Animate[Show[Sequence@@dodatki,Graphics3D[{Thick,Blue,Line[szkielet],Brown,PointSize[0.02],Point[(szkielet[[1;;-1;;2]])] }],
fullPlotRange,Axes->True,AxesLabel->{"(Subscript[y, 1])","(Subscript[y, 2])","(Subscript[y, 3])"},ImageSize->rozm],{t,tmin,tmax,step},AnimationRate->1, AnimationRunning->False]]

plotManip[kinFull[q[t]]/.rozw[[1]],3,tmax/5,0.05]
animManip[kinFull[q[t]]/.rozw[[1]],0,tmax,0.05,{trajy,trajzad},500]
(*animManipStandard[kinFull[q[t]]/.rozw[[1]],0,tmax,0.05,{trajy,trajzad},500]*)

3D na czysto

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

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[2]];ε=(10^(-3));

Do wizualizacji

plotManip[szkielet_List,tmin_,tmax_,step_]:=Show[Graphics3D[Table[{Line[szkielet],Brown,PointSize[0.02],Point[szkielet] },{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]]]}],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)]]]]

FANUC LR Mate 200iC

Definicja manipulatora

q[t_]={q1[t],q2[t],q3[t],q4[t],q5[t],q6[t]};
vq=Head/@q[t]
DHPar ={{q1,0.33,0.075,-π/2},{q2-π/2,0,0.3,π},{q3,0,0.075,-π/2},{q4,-0.32,0,π/2},{q5,0,0,-π/2},{q6,-0.08,0,0}};

Wyliczenie modeli: kinematyki częściowe,jakobian analityczny

mKin=DHPar//kinematyki//Simplify;
mKinFull=DHPar//kinematykiFull//Simplify;
Clear[kin,kinFull,jak];
Evaluate[kin[Pattern[#,_]&/@vq]]=mKin[[All,{1,2,3},4]];   
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,0,0.2]
plotManip[kinFull[{0,π/4,π/3,π/2,π/3,π/4}],0,0,0.2]

Symulacje

Trajektoria zadana

tmax=20;
yd0[t_]={.2 Cos[t]+0.4,.5 Sin[t],0.2}; 
yd1[t_]={.1 Cos[t]+.2,.2 Sin[t],.2 Sin[2 t]+.1};
yd2[t_]={.05 Cos[3t+π]+0.3,.3 Sin[ t],.2 Sin[7t+π/3]+.3}; 
yd[t_]=yd0[t];
trajzad=ParametricPlot3D[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,PlotRange->All]

Algorytm Newtona   x'=(J^(#))(x)(y'+α((Subscript[y, d])-k(x))), rozwiązanie

pstrona=mInvPseudo[jak[q[t]]].((∂_t)yd[t]+α(yd[t]-kin[q[t]][[-1]]))/.α->3;
rown=Thread[q'[t]==pstrona];
init=q[0]=={-.5,1,1,1,1,1};
tmax=20;
rozw=NDSolve[{ rown,init},vq,{t,0,tmax}];
tkon=(q1/.rozw[[1]])["Domain"][[1,2]]
LeafCount[rown]

Wizualizacja

tmax=tkon;
Plot[Evaluate[q[t]/.rozw],{t,0,tmax},PlotRange->All,AxesLabel->{"t","q"},PlotLegends->MapIndexed[Subscript[q,#2[[1]]]&,vq]]
trajy=ParametricPlot3D[Evaluate[kin[q[t]][[-1]]/.rozw],{t,0,tmax},PlotRange->All]
trajzad=ParametricPlot3D[Evaluate[yd[t]],{t,0,tmax},PlotStyle->Red,PlotRange->All]
slad=plotManip[kinFull[q[t]]/.rozw[[1]],6,tmax,0.2];
Show[trajy,trajzad,slad,PlotRange->All]

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

Błąd, wyznacznik jakobianu, przyspieszenia w przegubach

blad[t_]=Norm[yd[t]-kin[q[t]][[-1]]]/.rozw[[1]];
Plot[Evaluate[blad[t]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","ey"}]
Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw[[1]]]],{t,0,tmax},PlotRange->All,AxesLabel->{"t","Det J"}]
Plot[Evaluate[q''[t]/.rozw[[1]]],{t,0,tmax},AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]]   
Plot[Evaluate[q''[t]/.rozw[[1]]],{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},
{5,tmax-0.1}};
{tp,tk}=przedzialy[[5]];
Plot[Evaluate[Sqrt@Det[jak[q[t]].Transpose[jak[q[t]]]/.rozw[[1]]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","Det (JJ^T)"}]
Plot[Evaluate[q''[t]/.rozw[[1]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q''"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"''"]&,vq]] 
Plot[Evaluate[q'[t]/.rozw[[1]]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","q'"},PlotLegends->MapIndexed[Superscript[Subscript[q,#2[[1]]],"'"]&,vq]]   
Plot[Evaluate[blad[t]],{t,tp,tk},PlotRange->All,AxesLabel->{"t","ey"}]

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]]