Université Moulay Ismail ENSAM de Meknes

Modèle inverse
Exemple 1 : Structure RRRRRR F7

o7
F6 a 6
o6
F5 a5
o5
F4 a 4
F3 a 3 o 4
o3
a2
F2
o2
z1
a1
o1 x1
Vitesse de rotation de la fin d’effecteur par rapport au repère de la base

Ω⃗(F /F ) = Ω⃗(F /F ) + Ω⃗(F /F ) + Ω⃗(F /F ) + Ω⃗(F /F ) + Ω⃗(F /F ) + Ω⃗(F /F )

Ω⃗(F /F ) = θ̇ Z⃗
Ω⃗(F /F ) = θ̇ Z⃗
Ω⃗(F /F ) = θ̇ Z⃗
Ω⃗(F /F ) = θ̇ Z⃗
Ω⃗(F /F ) = θ̇ Z⃗
Ω⃗(F /F ) = θ̇ Z⃗

Vitesse de déplacement de la fin d’effecteur par rapport au repère de la base

V⃗(O ∈ F /F ) = V⃗(O ∈ F /F ) + V⃗(O ∈ F /F ) + V⃗(O ∈ F /F ) + V⃗(O ∈ F /F ) + V⃗(O ∈ F /F ) + V⃗(O
∈ F /F )

V⃗(O ∈ F /F ) = V⃗(O ∈ F /F ) + O O ⃗ ∧ Ω⃗(F /F )

V⃗(O ∈ F /F ) = O O ⃗ ∧ Ω⃗(F /F )
V⃗(O ∈ F /F ) = Ω⃗(F /F ) ∧ O O ⃗
V⃗(O ∈ F /F ) = θ̇ Z⃗ ∧ O O ⃗

V⃗(O ∈ F /F ) = V⃗(O ∈ F /F ) + O O ⃗ ∧ Ω⃗(F /F )

V⃗(O ∈ F /F ) = O O ⃗ ∧ Ω⃗(F /F )
V⃗(O ∈ F /F ) = O O ⃗ ∧ Ω⃗(F /F )
V⃗(O ∈ F /F ) = θ̇ Z⃗ ∧ O O ⃗

V⃗(O ∈F /F ) = θ̇ Z⃗ ∧ O O⃗
V⃗(O ∈F /F ) = θ̇ Z⃗ ∧ O O⃗
V⃗(O ∈F /F ) = θ̇ Z⃗ ∧ O O⃗
V⃗(O ∈F /F ) = θ̇ Z⃗ ∧ O O⃗

Robotique Industrielle : 4 ième Année Page 1

Université Moulay Ismail ENSAM de Meknes

Expression matricielle de la vitesse de rotation et la vitesse de déplacement de la fin d’effecteur en fonction des
variations des coordonnées articulaires
θ̇
⎡ ⎤
⎢ θ̇ ⎥
⃗
Ω(F /F ) Z⃗ Z⃗ Z⃗ Z⃗ Z⃗ Z⃗ ⎢θ̇ ⎥
= ⎢ ⎥
V⃗(O ∈ F /F ) Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ θ̇
⎢ ⎥
̇
⎢θ ⎥
⎣θ̇ ⎦

θ̇
⎡ ⎤
⎢ θ̇ ⎥
Ω⃗(F /F ) Z⃗ Z⃗ Z⃗ Z⃗ Z⃗ ⃗
Z ⎢θ̇ ⎥
= ⎢ ⎥
V⃗(O ∈ F /F ) Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ θ̇
⎢ ⎥
̇
⎢θ ⎥
⎣θ̇ ⎦
Par rapport à une petite variation de temps ∆t

∆θ
⎡∆θ ⎤
⎢ ⎥
∆Q Z⃗ Z⃗ Z⃗ Z⃗ Z⃗ Z⃗ ⎢∆θ ⎥
=
∆P Z⃗ ∧ O O ⃗ Z⃗ ∧ O O⃗ Z⃗ ∧ O O⃗ Z⃗ ∧ O O⃗ Z⃗ ∧ O O⃗ Z⃗ ∧ O O ⃗ ⎢∆θ ⎥
⎢∆θ ⎥
⎣∆θ ⎦
∆θ
⎡∆θ ⎤
⎢ ⎥
∆Q ⎢∆θ ⎥
= 𝐉
∆P ⎢∆θ𝟒 ⎥
⎢∆θ ⎥
⎣∆θ ⎦
J est la matrice jacobéenne. Les composantes de cette matrice sont exprimées dans le repère lié à la base

∆P = P − P ; P : Position desirée ; P : Position actuelle

On a la matrice de rotation désirée

𝑅 = 𝑅 ∗ ∆R
Donc
∆R = R ∗ 𝑅
∆R est la matrice exprimée par les trois angles d’Euler (ZYX) Repère désiré par rapport au repère actuel.
On calcule le vect(∆R) pour avoir un vecteur comportant les trois angles (Rolis, tangage et Lacet ) de rotation autour
de x, y et z du repère de base. Ce vecteur est exprimé dans le repère de base (Repère fixe XYZ )
∆Q = Vect(∆R)

Rotation désirée

R
P

∆R
P

Rotation actuelle
R

Base
Robotique Industrielle : 4 ième Année Page 2
Université Moulay Ismail ENSAM de Meknes

Sur Matlab, la function vect est declarée par:

function mat=vect(A)
mat=.5*[A(3,2)-A(2,3)
A(1,3)-A(3,1)
A(2,1)-A(1,2)];
End

Cette fonction donne les trois petits angles de rotation ∆θ , ∆θ et ∆θ autour des trois axes fixes x , y et z du repère
fixe.

Exemple : Script permettant de calculer le vect d’une matrice de rotation

syms a b g real
% a,b,g represente aplha, beta et gama sont les trois angles de rotation
% par rapport à l'axe z, y et x (angles d’Euler)

R=Rz(a)*Ry(b)*Rx(g)

R=simple(R)

V=vect(R)

V=simple(V)

Après calcul :

R=

[ cos(a)*cos(b) , cos(a)*sin(b)*sin(g) - cos(g)*sin(a) , sin(a)*sin(g) + cos(a)*cos(g)*sin(b) , 0]

[ cos(b)*sin(a) , cos(a)*cos(g) + sin(a)*sin(b)*sin(g) , cos(g)*sin(a)*sin(b) - cos(a)*sin(g) , 0]
[ -sin(b) , cos(b)*sin(g) , cos(b)*cos(g) , 0]
[ 0 , 0 , 0 , 1]

Vect (R) =

[ (cos(a)*sin(g))/2 + (cos(b)*sin(g))/2 - (cos(g)*sin(a)*sin(b))/2]

[ sin(b)/2 + (sin(a)*sin(g))/2 + (cos(a)*cos(g)*sin(b))/2]
[ (cos(b)*sin(a))/2 + (cos(g)*sin(a))/2 - (cos(a)*sin(b)*sin(g))/2]

Quand a, b et g sont tres petits (c’est le cas de la matrice R)

sin(a)  a et cos(a)  1
sin(b)  b et cos(b)  1
sin(g)  g et cos(g)  1

vect (R) = [ g , b , a]T .

Exemple 2 : Structure RRRPRR

Ω⃗(F /F ) = Ω⃗(F /F ) + Ω⃗(F /F ) + Ω⃗(F /F ) + Ω⃗(F /F ) + Ω⃗(F /F ) + Ω⃗(F /F )

Ω⃗(F /F ) = θ̇ Z⃗
Ω⃗(F /F ) = θ̇ Z⃗
𝛀⃗(𝐅𝟓 /𝐅𝟒 ) = 𝟎⃗
Ω⃗(F /F ) = θ̇ Z⃗
Ω⃗(F /F ) = θ̇ Z⃗
Ω⃗(F /F ) = θ̇ Z⃗

Robotique Industrielle : 4 ième Année Page 3

Université Moulay Ismail ENSAM de Meknes

V⃗(O ∈ F /F ) = V⃗(O ∈ F /F ) + V⃗(O ∈ F /F ) + 𝐕⃗(𝐎𝟕 ∈ 𝐅𝟓 /𝐅𝟒 ) + V⃗(O ∈ F /F ) + V⃗(O ∈ F /F ) + V⃗(O

∈ F /F )

V⃗(O ∈ F /F ) = θ̇ Z⃗ ∧ O O ⃗
V⃗(O ∈ F /F ) = θ̇ Z⃗ ∧ O O ⃗

V⃗(O ∈ F /F ) = V⃗(O ∈ F /F ) + O O ⃗ ∧ Ω⃗(F /F )

V(O ∈ F /F ) = V⃗(O ∈ F /F )
⃗ car 𝛀⃗(𝐅𝟓 /𝐅𝟒 ) = 𝟎⃗
⃗ ̇ ⃗
𝐕(𝐎𝟕 ∈ 𝐅𝟓 /𝐅𝟒 ) = 𝐝𝟒 𝐙𝟒

V⃗(O ∈ F /F ) = θ̇ Z⃗ ∧ O O ⃗
V⃗(O ∈ F /F ) = θ̇ Z⃗ ∧ O O ⃗
V⃗(O ∈ F /F ) = θ̇ Z⃗ ∧ O O ⃗
θ̇
⎡ ⎤
⎢ θ̇ ⎥
⃗
Ω(F /F ) Z⃗ Z⃗ Z⃗ 𝟎⃗ Z⃗ Z⃗ ⎢ θ̇ ⎥
= ⎢ ⎥
⃗
V(O ∈ F /F ) ⃗ ⃗ ⃗ ⃗ ⃗
Z ∧ O O Z ∧ O O Z ∧ O O 𝐙𝟒 ⃗ ⃗ Z ∧ O O Z ∧ O O 𝐝̇𝟒
⃗ ⃗ ⃗ ⃗
⎢ ⎥
̇
⎢θ ⎥
⎣ θ̇ ⎦
∆θ
⎡ ∆θ ⎤
⎢ ⎥
∆Q Z⃗ Z⃗ Z⃗ 𝟎⃗ Z⃗ Z⃗ ⎢ ∆θ ⎥
=
∆P Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ 𝐙⃗ Z⃗ ∧ O O ⃗ Z⃗ ∧ O O ⃗ ⎢∆𝐝𝟒 ⎥
⎢ ∆θ ⎥
⎣ ∆θ ⎦

∆θ
⎡ ∆θ ⎤
⎢ ⎥
∆Q ⎢ ∆θ ⎥
= 𝐉
∆P ⎢∆𝐝𝟒 ⎥
⎢ ∆θ ⎥
⎣ ∆θ ⎦

Expression des composantes de la matrice jacobienne J dans le repère {F1} lié à la base (Bâti)

Les matrices de rotation

𝑅 = 𝑅
𝑅 =R ∗ 𝑅 = 𝑅∗ 𝑅
𝑅 =R ∗ 𝑅 = 𝑅∗ 𝑅∗ 𝑅
𝑅 =R ∗ 𝑅 = 𝑅∗ 𝑅∗ 𝑅∗ 𝑅
𝑅 =R ∗ 𝑅 = 𝑅∗ 𝑅∗ 𝑅∗ 𝑅∗ 𝑅

Expression de Z⃗ dans le repère {F1} i = 1, 2, 3, 4, 5, 6

On definit le vecteur k⃗ = [0 , 0 , 1]

Z⃗ = k⃗
Z⃗ = 𝑅12 ∗ k⃗
Z⃗ = 𝑅13 ∗ k⃗
Z⃗ = 𝑅14 ∗ k⃗
Z⃗ = 𝑅15 ∗ k⃗

Robotique Industrielle : 4 ième Année Page 4

Université Moulay Ismail ENSAM de Meknes

Z⃗ = 𝑅16 ∗ k⃗

O O ⃗ = 𝒂𝟔⃗ Position de O7 dans le repère {F6} exprimée dans le repère {F6}

O O ⃗ = 𝒂𝟓⃗ + 𝑅∗O O⃗ Position de O7 dans le repère {F5} exprimée dans le repère {F5}
O O ⃗ = 𝒂𝟒⃗ + 𝑅∗O O⃗ Position de O7 dans le repère {F4} exprimée dans le repère {F4}
O O ⃗ = 𝒂𝟑⃗ + 𝑅∗O O⃗ Position de O7 dans le repère {F3} exprimée dans le repère {F3}
O O ⃗ = 𝒂𝟐⃗ + 𝑅∗O O⃗ Position de O7 dans le repère {F2} exprimée dans le repère {F2}
O O ⃗ = 𝒂𝟏⃗ + 𝑅∗O O⃗ Position de O7 dans le repère {F1} exprimée dans le repère {F1}

Expression des vecteurs dans le repère {F1}

O O⃗ = O O⃗ Position de O7 dans le repère {F1} exprimée dans le repère {F1}

O O⃗ = 𝑅 ∗ O O⃗ Position de O7 dans le repère {F2} exprimée dans le repère {F1}
O O⃗ = 𝑅 ∗ O O⃗ Position de O7 dans le repère {F3} exprimée dans le repère {F1}
O O⃗ = 𝑅 ∗ O O⃗ Position de O7 dans le repère {F4} exprimée dans le repère {F1}
O O⃗ = 𝑅 ∗ O O⃗ Position de O7 dans le repère {F5} exprimée dans le repère {F1}
O O⃗ = 𝑅 ∗ O O⃗ Position de O7 dans le repère {F6} exprimée dans le repère {F1}

Algorithme1 : calcul de la matrice jacobienne

1. Paramètres D-H du robot

2. Construire les matrices de transformation
homogène 𝐓 ; (position et rotation du repère
𝐹 par rapport 𝐹 ) ; 𝑖 = 1 : 6 tel que 𝐓 =
𝐑 𝐩
0 1
3. Construire les matrices de rotation R ; 𝑖 = 1 : 6
tel que 𝐑 = 𝐑
et 𝐑 = 𝐑 𝐑 … 𝐑
4. Exprimer les vecteurs k du repère 𝐹 dans le
repère de base 𝐹 :
𝐞 = 𝐤 = [0 0 1]
et 𝐞 = 𝐑 𝐤 𝑖 = 2 : 6 ;
5. Construire les vecteurs positions 𝒓
(Position de la fin de l’effecteur exprimée dans le
repère 𝐹 ) ; tel que :
𝒓 =𝐩
𝒓 =𝐩 +𝐑 𝒓
𝒓 = 𝐩 + 𝐑 𝒓( ) 𝑖 = 4,3,2,1
6. Exprimer les vecteurs positions 𝒓
dans le repère de base 𝐹 ; tel que :
𝒓 =𝒓
𝒓 =𝐑 𝒓
𝒓 = 𝐑 ( ) 𝒓 𝑖 = 3,4,5,6
7. Construire la matrice Jacobienne 𝐉 (6x6 ) a partir
des 6 matrices colonnes 𝐉 (6x1) ; tel que :
𝐞
𝐉 = 𝐞 ×𝒓
𝐉 = [𝐉 … 𝐉 ]

Robotique Industrielle : 4 ième Année Page 5

Université Moulay Ismail ENSAM de Meknes

Algorithme 2-1 : Méthode de calcul des coordonnées

articulaires pour une trajectoire

1. Données :
𝛉: configuration initiale des coordonnées articulaire
du robot
𝐓 : matrice de transformation homogène (position
et orientation désirées)
𝐷 : durée de la tache (temps de passage du point
initial au point desiré)
∆𝑡 : pas de la discrétisation du temps
2. Calculer la position et l’orientation initiale en
utilisant le modèle géométrique direct tel que :
𝑻 = MGD(𝛉)
3. Calculer le nombre de configurations :
𝐷
𝑁= +1
∆𝑡
4. Discrétisation de la trajectoire en (𝑁 − 1)
éléments et calcul des coordonnées cartésienne
et orientations de la fin de l’effecteur aux 𝑁
nœuds:
5. Appel a la fonction MATLAB reach.m, 𝑁 fois pour
calculer les 𝑁postures ( chaque posture
correspond a une configuration des coordonnées
articulaire du robot).

Robotique Industrielle : 4 ième Année Page 6

Université Moulay Ismail ENSAM de Meknes

Algorithme 2-2 : Méthode de calcul des coordonnées

articulaires en un point a partir d’un
point initial connu (fonction reach.m)

1. Données
𝐩 𝐑 : position et orientation désirées
𝛉: coordonnées articulaire initiales
n: nombre maximum d’itérations
e : erreur initiale (choisie grande pour
amorcer le calcul)
e0 : critère de convergence requise
𝑎 : amortissement du pas
2. Calculer la position 𝐩 et l’orientation 𝐑 de la fin de
l’effecteur à l’état initiale par le MGD
3. Calculer 𝐝𝐩 = 𝑎 (𝑝 − 𝐩) et
𝐝𝐪
𝐝𝐪 = 𝑣𝑒𝑐𝑡(𝐑 𝐑 ) Poser 𝐝𝐏 ≡
𝐝𝐩
4. Calculer la matrice Jacobienne 𝐉(𝛉)
5. Calculer 𝐝𝛉 = 𝐉 𝐝𝐏
6. Calculer la nouvelle configuration 𝛉 = 𝛉 + 𝐝𝛉
7. Calculer la nouvelle position 𝐩 et l’orientation 𝐑 de
la fin de l’effecteur pour la nouvelle 𝛉 en utilisant
le MGD
8. Calculer la nouvelle erreur 𝐝𝐩 = 𝐩 − 𝐩 et
𝐝𝐪 = 𝑣𝑒𝑐𝑡 𝐑 ∗ 𝐑 𝒇
𝐝𝐪
9. Calculer l’erreur e=norm( ) et décrémente n
𝐝𝐩
10. Si n > 0 et e > e0 recommencer le calcul a
partir de l’étape 3.
Sinon fin de calcul et retour des nouvelles valeurs
de 𝛉

Exemple : Fonction pour la discrétisation

% Pour un segment [P1P2]

% Premier point du segment

P1=[1;2;1];
% Deuxieme point du segment
P2=[2;3;4];

pas=0.04; % Pas de simulation (seconde)

T = 5; % Durée pour parcourir le segment (seconde)

t = [0:pas:T]; % Discrétisation du temps

s = (t/T)-sin(2*pi*t/T)/(2*pi); % Discrétisation de l'espace

figure(1)
plot(t,s,'s')
xlabel('temps(s)');
ylabel('Discrétisation de l espace ');

grid on
%--------------------------------------------
% Coordonnées des points sur le segment [P1,P2]
%--------------------------------------------

Robotique Industrielle : 4 ième Année Page 7

Université Moulay Ismail ENSAM de Meknes

P = P1*ones(size(s)) + (P2-P1)*s; % Discrétisation du segment

figure(2)
plot3(P(1,:,:),P(2,:,:),P(3,:,:),'*')
xlabel('x');
ylabel('y ');
zlabel('z ');
grid on

%----------------------------------------------------------
% Vitesse de la fin d'effecteur aus points du segment [P1,P2]
%----------------------------------------------------------

v(1)=0;
for i=2:length(s)-1

v(i)=norm(P(:,i+1)-P(:,i))/pas;

i=i+1;
end
v(length(v)+1)=0;
figure(3)
plot(t,v,'x')
xlabel('temps(s)');
ylabel('vitesse (mm/s) ');
grid on
%---------------------------------------------------------------
% acceleration de la fin d'effecteur aux points du segment [P1,P2]
%---------------------------------------------------------------

a(1)= 0;
for i=2:length(s)-1

a(i)=(v(i+1)-v(i))/pas;

i=i+1;3
end
a(length(a)+1)=0;
figure(4)
plot(t,a,'o')
xlabel('temps(s)');
ylabel('accéleration (mm/s^2) ');
grid on

Exemple : Calcul de la matrice Jacobienne

function Ja=Jacobienne(teta)

% Tableau D-H pour le Robot ABB_IRB4600_205_45 équipé de la pince

IRB1600_Pince.tool de longueur 233.37

table=[ teta(1) 495 175 -pi/2;

teta(2)-pi/2 0 900 0;
teta(3) 0 175 -pi/2;
teta(4) 960 0 pi/2 ;
teta(5) 0 0 -pi/2;
teta(6)-pi 135+233.37 0 0];
%
T1=TranElem(table,1);
T2=TranElem(table,2);

Robotique Industrielle : 4 ième Année Page 8

Université Moulay Ismail ENSAM de Meknes

T3=TranElem(table,3);
T4=TranElem(table,4);
T5=TranElem(table,5);
T6=TranElem(table,6);

R1=T1(1:3,1:3);
R2=T2(1:3,1:3);
R3=T3(1:3,1:3);
R4=T4(1:3,1:3);
R5=T5(1:3,1:3);
R6=T6(1:3,1:3);

R11=R1;
R12=R11*R2;
R13=R12*R3;
R14=R13*R4;
R15=R14*R5;
R16=R15*R6;

k=[0 0 1]';
%------------------------------------------
e1=k;
e2=R11*k;
e3=R12*k;
e4=R13*k;
e5=R14*k;
e6=R15*k;

%-----------------------------------------
P6=T6(1:3,4);
P5=T5(1:3,4);
P4=T4(1:3,4);
P3=T3(1:3,4);
P2=T2(1:3,4);
P1=T1(1:3,4);
%--------------------------------------
r6p=P6;
r5p=P5+R5*r6p;
r4p=P4+R4*r5p;
r3p=P3+R3*r4p;
r2p=P2+R2*r3p;
r1p=P1+R1*r2p;

%-----------------------------------------

r6=R15*r6p;
r5=R14*r5p;
r4=R13*r4p;
r3=R12*r3p;
r2=R11*r2p;
r1=r1p;
%-------------------------------------------

Ja(:,1)=[e1;cross(e1,r1)];
Ja(:,2)=[e2;cross(e2,r2)];
Ja(:,3)=[e3;cross(e3,r3)];
Ja(:,4)=[e4;cross(e4,r4)];
Ja(:,5)=[e5;cross(e5,r5)];
Ja(:,6)=[e6;cross(e6,r6)];
%-------------------------------------------

Robotique Industrielle : 4 ième Année Page 9

Université Moulay Ismail ENSAM de Meknes

Fonction Reach

function t=reach(pf,qf,t)

% pf position final (desirée) 3x1

% qf rotation final (desirée) 3x3
% t matrice colonne 6x1 contenant les teta I de la position actuelle
%---------------------------------------------------------------

n=25; % Nombre maximum d'itération

e=1; % Erreur initiale tres grande (1 metre)
a=0.75; % Amortissement sur le pas

MGD=ModelGD(t); % Position Cartésienne actuelle

p=MGD(1:3,4); %position actuelle
q=MGD(1:3,1:3); % rotation actuelle

while(n>0 & e>0.0001) % Tant que l'erreur est plus de 0.1mm (et n>0)
dp=a*(pf-p); % Déplacement Cartésien requis
dq= vect(q' * qf); % rotation requise
dT=[dq;dp]; % torseur de vitesse rotation et vitesse de deplacement (dT = J . dt)
J=Jacobienne(t); % Évaluation de la Jacobienne
dt=pinv(J)*dT; % Calcul du déplacement theta approx.

t=t+dt; % Calcul la nouvelle position t

MGD=ModelGD(t); % Position Cartésienne actuelle

p=MGD(1:3,4); %Calcul de la nouvelle position p
q=MGD(1:3,1:3); % Calcul de la nouvelle rotation q

DeltaPos=pf-p;
DeltaRot=vect(qf *q');
Critere=[DeltaPos;DeltaRot]; % Matrice colonne 6x1

e=norm(Critere); % Erreur entre p et pf désirée et entre q et qf desiree

n=n-1; % Une itération en moins dedisp.

end
if n==0 % Affiche un message lorsque>0.1 mm
disp('Reach: Erreur '); e
end

Exemple de trajectoire entre deux points d’un segment

function trj=trajectoire2points(teta,posef)

%% teta : coordonnées articulaires du 1er point du segment

%% posef : matrice 4x4 comporte la position et l’orientation de la fin
d’effecteur désirées au 2ieme point du segment

Robotique Industrielle : 4 ième Année Page 10

Université Moulay Ismail ENSAM de Meknes

MGD = ModelGD(teta);

p=MGD(1:3,4); %% Approche du 1er point position p

q=MGD(1:3,1:3); % Approche du 1er point position rotation q
Qd=posef(1:3,1:3);
% Qd=[0 -1 0;0 0 1;-1 0 0] % Rotation désirée de la fin de l
% effecteur
P1 = p; % 1er Point du segment
P2 = posef(1:3,4); % 2e Point du segment

pas=0.04; % Pas de simulation (seconde)

T = 5; % Durée du segment (seconde)

iteration=(T/pas)+1; % Nombre d'itérations

t = [0:pas:T]; % Discrétisation du temps

s = (t/T)-sin(2*pi*t/T)/(2*pi); % Discrétisation de l'espace

P = P1*ones(size(s)) + (P2-P1)*s; % Discrétisation du segment

for i=1:iteration % Calcul des positions

teta = reach(P(:,i),Qd,teta);
trj(:,i)=teta;
%pxy(:,i)=position_outil(theta);
end
% trj
% etat=enregistre_trj(trj,pas,'robot2.trj') % Creer le fichier trj

Robotique Industrielle : 4 ième Année Page 11