Materiály k 3. přednášce: Inverzní úloha kinematiky, Jacobián - FBMI
Materiály k 3. přednášce: Inverzní úloha kinematiky, Jacobián - FBMI
Materiály k 3. přednášce: Inverzní úloha kinematiky, Jacobián - FBMI
You also want an ePaper? Increase the reach of your titles
YUMPU automatically turns print PDFs into web optimized ePapers that Google loves.
Inovace výuky předmětu Robotika v lékařství<br />
TT32=[1 0 0 0; 0 cos(fi32) -sin(fi32) 0; 0 sin(fi32) cos(fi32) 0; 0 0 0 1];<br />
% rotace kolem x-ove osy o fi32<br />
TT43=[1 0 0 0; 0 cos(fi43) -sin(fi43) 0; 0 sin(fi43) cos(fi43) 0; 0 0 0 1];<br />
% rotace kolem x-ove osy o fi43<br />
TT54=[cos(fi54) -sin(fi54) 0 0; sin(fi54) cos(fi54) 0 0; 0 0 1 0; 0 0 0 1];<br />
% rotace kolem z-ove osy o fi54<br />
TT50=TT10*TT21*TT32*TT43*TT54; % výsledná transformacni matice z prostoru<br />
telesa 5 do prostoru telesa 0<br />
% poloha koncového bodu M vůči rámu v homogennim zobrazení<br />
rM0=TT50*r5M;<br />
% Rychlosti<br />
% diferencialni operatory zakladnich pohybu<br />
DTX = [0 0 0 1;0 0 0 0;0 0 0 0;0 0 0 0];<br />
DTY = [0 0 0 0;0 0 0 1;0 0 0 0;0 0 0 0];<br />
DTZ = [0 0 0 0;0 0 0 0;0 0 0 1;0 0 0 0];<br />
DRX = [0 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 0];<br />
DRY = [0 0 1 0;0 0 0 0;-1 0 0 0;0 0 0 0];<br />
DRZ = [0 -1 0 0;1 0 0 0;0 0 0 0;0 0 0 0];<br />
% rychlosti zakladnich pohybu<br />
V10S1=om10*DRZ;<br />
V21S2=om21*DRX;<br />
V32S3=om32*DRX;<br />
V43S4=om43*DRX;<br />
V54S5=om54*DRZ;<br />
% inverzni transformacni matice<br />
ITT10=inv(TT10);<br />
ITT21=inv(TT21);<br />
ITT32=inv(TT32);<br />
ITT43=inv(TT43);<br />
ITT54=inv(TT54);<br />
V10S5=ITT54*ITT43*ITT32*ITT21*V10S1*TT21*TT32*TT43*TT54;%transformace<br />
rychlosti V10S1 (v prostoru 1) na V10S5 (v prostoru 5)<br />
V21S5=ITT54*ITT43*ITT32*V21S2*TT32*TT43*TT54;<br />
V32S5=ITT54*ITT43*V32S3*TT43*TT54;<br />
V43S5=ITT54*V43S4*TT54;<br />
% vysledna matice rychlosti soustavy (z telesa 5 do telesa 0 v prostoru<br />
souradnic 5)<br />
V50S5=V10S5+V21S5+V32S5+V43S5+V54S5;<br />
simV50S5=simplify(V50S5); % zjednoduseni formalni uprava<br />
% rychlost bodu M vuci ramu<br />
Stránka 5 z 6