%Zadanie proste dla manipulatorów szeregowych

clear all
close all
clc



%1. Definicja projektu
n=17; %numer studenta
%2. Wgranie danych dla danego manipulatora
danymanipulatorhubert

for k=1:10:360
% theta(1)=k*pi/180
theta(3)=k*pi/180

% 3. obliczanie macierzy przekształceń jednorodnych

A1=matrixA(l(1),lambda(1),alpha(1),theta(1))

A2=matrixA(l(2),lambda(2),alpha(2),theta(2))

A3=matrixA(l(3),lambda(3),alpha(3),theta(3))




T30=A1*A2*A3

% wektor pozycji i macierz orientacji

p30=T30(1:3,4)

R30=T30(1:3,1:3)

%kąty orientacji wg konwencji roll-pitch-jaw

rpy = tr2rpy(R30)




roll=rpy(1) %przechylenie (X)

pitch=rpy(2) %pochylenie (Y)

yaw=rpy(3) %odchylenie (Z)
%rysunek manipulatora
rysunek=1
anim_manip_szer2
end