Corrig du contrle continu 1

Elment de module: Robotique (GM2-2015)

Exercice 1:
1.1 Les axes X 2 ,X3 ,X 4 ,X5 et X 6 sont tracs sur la figure suivante. Les origines des repres
sont videntes sur cette reprsentation plane. (4points : -1 par axe faux)

X4
300

400

200

300

300

X3

X2
O2
200

X5

Z2

O3

Z3

O 4 Z4

O 5 Z5

Z6

O6

X6

Z1
X1

O1

1.2 Tableau des paramtres DH (4points: -1 par paramtre faux)

Axe i
1
2
3
4
5
6

a i (mm)
0
300
0
0
0
0

bi (mm)
200
0
0
200
300
300

i ()
90
180
90
90
90
0

O7

X7

Z7

Exercice 2:
X5

X4
X3

Z1

O5 ,O 6

O4

O7

Z5

Z4 ,Z6 ,Z7

Z3

O3

X6

Z2

X7

X2

O2

IRB 7600 3.5

X1
O1

2.1 Tableau des paramtres DH (4points: -1 par paramtre faux)

Axe i
1
2
3
4
5
6

a i (mm)
410
1075
165
0
0
0

bi (mm)
780
0
0
2012
0
250

i ()
-90
180
90
90
90
0

2.2 (2points)
i ()
-90
180
90
90
90
0

Axe i
1
2
3
4
5
6

A =1977840
B=0

i
0
-1
0
0
0
1

i
-1
0
1
1
1
0
R = 3.09581012

F=0
G = 2412

x C 2412
On a : y C = 0 ; C = 354750 ; H = 0
z C 2020 D = 4325800
I=0
E = 2292450 J = 0

S = 4.58051013
; T = 7.72851013

U = 3.35281013
V = 1.57171011

Lquation polynmiale de degr 4 : R34 + S33 + T32 + U3 + V = 0 admet les 4 solutions:

( 3 )1 =12.9298; ( 3 )2 =1.1032 ; ( 3 )3 = 0.7674 ; ( 3 )4 = 0.0046 . Do

( 3 )1 =171.15 ; ( 3 )2 = 95.618 ; ( 3 )3 = 75.005 ; ( 3 )4 = 0.5315 .
Par suite, on obtient:

( 1 )1 = 0 ; ( 1 )2 =180 ; ( 1 )3 =180 ; ( 1 )4 = 0 .
( 2 )1 = 26.99 ; ( 2 )2 = 149.6 ; ( 2 )3 = 163 ; ( 2 )4 = 90.53 .
2.3 (2points) On obtient :
* ( 1 , 2 , 3 ) = ( 0,26.99,171.15)
4 = 0 ; 5 =144.17 ; 6 = 0
* ( 1 , 2 , 3 ) = (180, 149.6,95.618)
4 = 0 ; 5 = 114.83 ; 6 =180
* ( 1 , 2 , 3 ) = (180, 163,75.005)
4 = 0 ; 5 = 121.99 ; 6 =180
* ( 1 , 2 , 3 ) = ( 0, 90.53, 0.5315)
4 = 0 ; 5 = 90 ; 6 = 0

2.4 (4points)
Ici le manipulateur est de type srie et il se compose de 7 corps. Les articulations sont toutes de
type rotode.
Lalgorithme de calcul de la dynamique inverse comprend deux tapes principales:
(a) calcul cinmatique; on calcule les torseurs cinmatiques et leurs drives en fonction des
angles daxes et de leurs deux premires drives. Ce calcul se fait conformment lalgorithme
de la diapositive 14 du cours 9.
(b) calcul dynamique; on dtermine les efforts de liaison et le torseur des forces extrieures. Ce
calcul suit lalgorithme de la diapositive 19 du cours 9.

Annexe
Programme utilis pour le calcul des rotations daxes
clear all;
close all;
format long e;
q11=1; q12=0; q13=0; q21=0; q22=1; q23=0; q31=0; q32=0; q33=1;
%
xp=2412; yp=0; zp=2270;
a1=410; a2=1075; a3=165; a4=0; a5=0; a6=0;
b1=780; b2=0; b3=0; b4=2012; b5=0; b6=250;
al1=-90; al2=180; al3=90; al4=90; al5=90; al6=0;
lam1=cos(al1*pi/180); lam2=cos(al2*pi/180); lam3=cos(al3*pi/180);
lam4=cos(al4*pi/180); lam5=cos(al5*pi/180); lam6=cos(al6*pi/180);
mu1=sin(al1*pi/180); mu2=sin(al2*pi/180); mu3=sin(al3*pi/180);
mu4=sin(al4*pi/180); mu5=sin(al5*pi/180); mu6=sin(al6*pi/180);
xc=xp-(q11*a6+q12*b6*mu6+q13*b6*lam6);
yc=yp-(q21*a6+q22*b6*mu6+q23*b6*lam6);
zc=zp-(q31*a6+q32*b6*mu6+q33*b6*lam6);
A=2*a1*xc;
B=2*a1*yc;
C=2*a2*a3-2*b2*b4*mu2*mu3;
D=2*a3*b2*mu2+2*a2*b4*mu3;
E=a2^2+a3^2+b2^2+b3^2+b4^2-a1^2-xc^2-yc^2-(zcb1)^2+2*b2*b3*lam2+2*b2*b4*lam2*lam3+2*b3*b4*lam3;
F=yc*mu1;
G=-xc*mu1;
H=-b4*mu2*mu3;
I=a3*mu2;
J=b2+b3*lam2+b4*lam2*lam3-(zc-b1)*lam1;
ro2=xc^2+yc^2;
R=4*a1^2*(J-H)^2+mu1^2*(E-C)^2-4*ro2*a1^2*mu1^2;
S=4*(4*a1^2*I*(J-H)+mu1^2*D*(E-C));
T=2*(4*a1^2*(J^2-H^2+2*I^2)+mu1^2*(E^2-C^2+2*D^2)-4*ro2*a1^2*mu1^2);
U=4*(4*a1^2*I*(H+J)+mu1^2*D*(C+E));
V=4*a1^2*(J+H)^2+mu1^2*(E+C)^2-4*ro2*a1^2*mu1^2;
polynome=[R S T U V];
tau=roots(polynome);
tau1=tau(1); tau2=tau(2); tau3=tau(3); tau4=tau(4);
tet1d=2*atan(tau1)*180/pi; tet2d=2*atan(tau2)*180/pi;
tet3d=2*atan(tau3)*180/pi; tet4d=2*atan(tau4)*180/pi;
tet31=2*atan(tau1); tet32=2*atan(tau2);
tet33=2*atan(tau3); tet34=2*atan(tau4);
%
delta1=A*G-F*B;
c31=cos(tet31); s31=sin(tet31);
c11=(-G*(C*c31+D*s31+E)+B*(H*c31+I*s31+J))/delta1;
tet11=acos(c11);
s11=(F*(C*c31+D*s31+E)-A*(H*c31+I*s31+J))/delta1;
A11=a2+a3*c31+b4*mu3*s31;
A12=-a3*lam2*s31+b3*mu2+b4*lam2*mu3*c31+b4*mu2*lam3;
delt2=A11^2+A12^2;
c21=(A11*(xc*c11+yc*s11-a1)-A12*(-xc*lam1*s11+yc*lam1*c11+(zc-b1)*mu1))/delt2;
tet21=acos(c21)*180/pi;
s21=(A12*(xc*c11+yc*s11-a1)+A11*(-xc*lam1*s11+yc*lam1*c11+(zc-b1)*mu1))/delt2;
Q11=[c11 -lam1*s11 mu1*s11; s11 lam1*c11 -mu1*c11; 0 mu1 lam1];
Q21=[c21 -lam2*s21 mu2*s21; s21 lam2*c21 -mu2*c21; 0 mu2 lam2];
Q31=[c31 -lam3*s31 mu3*s31; s31 lam3*c31 -mu3*c31; 0 mu3 lam3];
R1=(Q11*Q21*Q31)'*[q11 q12 q13; q21 q22 q23; q31 q32 q33];
r11=R1(1,1); r12=R1(1,2); r13=R1(1,3); r21=R1(2,1); r22=R1(2,2);
r23=R1(2,3); r31=R1(3,1); r32=R1(3,2); r33=R1(3,3);
u=[q31; q32; q33];
e641=(Q11*Q21*Q31)'*u;
xi1=e641(1); et1=e641(2); ze1=e641(3);
%lam5-ze1*lam4-et1*mu4=0
tau41=0;
tet41=2*atan(tau41);

c41=cos(tet41); s41=sin(tet41);
s51=((mu6*r12+lam6*r13)*c41+(mu6*r22+lam6*r23)*s41)/mu5;
c51=(lam4*(mu6*r12+lam6*r13)*s41-lam4*(mu6*r22+lam6*r23)*c41mu4*(mu6*r32+lam6*r33))/mu5;
w11=r11*c41+r21*s41;
w21=-lam4*(r11*s41-r21*c41)+mu4*r31;
w31=mu4*(r11*s41-r21*c41)+lam4*r31;
c61=w11*c51+w21*s51;
s61=-w11*lam5*s51+w21*lam5*c51+w31*mu5;
tet41d=tet41*180/pi;
tet51=acos(c51); tet51d=tet51*180/pi;
tet61=acos(c61); tet61d=tet61*180/pi;
%
c32=cos(tet32);
s32=sin(tet32);
c12=(-G*(C*c32+D*s32+E)+B*(H*c32+I*s32+J))/delta1;
tet12=acos(c12);
s12=(F*(C*c32+D*s32+E)-A*(H*c32+I*s32+J))/delta1;
A11=a2+a3*c32+b4*mu3*s32;
A12=-a3*lam2*s32+b3*mu2+b4*lam2*mu3*c32+b4*mu2*lam3;
delt2=A11^2+A12^2;
c22=(A11*(xc*c12+yc*s12-a1)-A12*(-xc*lam1*s12+yc*lam1*c12+(zc-b1)*mu1))/delt2;
tet22=acos(c22)*180/pi;
s22=(A12*(xc*c12+yc*s12-a1)+A11*(-xc*lam1*s12+yc*lam1*c12+(zc-b1)*mu1))/delt2;
Q12=[c12 -lam1*s12 mu1*s12; s12 lam1*c12 -mu1*c12; 0 mu1 lam1];
Q22=[c22 -lam2*s22 mu2*s22; s22 lam2*c22 -mu2*c22; 0 mu2 lam2];
Q32=[c32 -lam3*s32 mu3*s32; s32 lam3*c32 -mu3*c32; 0 mu3 lam3];
R2=(Q12*Q22*Q32)'*[q11 q12 q13; q21 q22 q23; q31 q32 q33];
r11=R2(1,1); r12=R2(1,2); r13=R2(1,3); r21=R2(2,1); r22=R2(2,2);
r23=R2(2,3); r31=R2(3,1); r32=R2(3,2); r33=R2(3,3);
u=[q31; q32; q33];
e642=(Q12*Q22*Q32)'*u;
xi2=e642(1); et2=e642(2); ze2=e642(3);
%lam5-ze2*lam4-et2*mu4=0
tau42=0;
tet42=2*atan(tau42);
c42=cos(tet42); s42=sin(tet42);
s52=((mu6*r12+lam6*r13)*c42+(mu6*r22+lam6*r23)*s42)/mu5;
c52=(lam4*(mu6*r12+lam6*r13)*s42-lam4*(mu6*r22+lam6*r23)*c42mu4*(mu6*r32+lam6*r33))/mu5;
w12=r11*c42+r21*s42;
w22=-lam4*(r11*s42-r21*c42)+mu4*r31;
w32=mu4*(r11*s42-r21*c42)+lam4*r31;
c62=w12*c52+w22*s52;
s62=-w12*lam5*s52+w22*lam5*c52+w32*mu5;
tet42d=tet42*180/pi;
tet52=acos(c52); tet52d=tet52*180/pi;
tet62=acos(c62); tet62d=tet62*180/pi;
%
c33=cos(tet33);
s33=sin(tet33);
c13=(-G*(C*c33+D*s33+E)+B*(H*c33+I*s33+J))/delta1;
tet13=acos(c13);
s13=(F*(C*c33+D*s33+E)-A*(H*c33+I*s33+J))/delta1;
A11=a2+a3*c33+b4*mu3*s33;
A12=-a3*lam2*s33+b3*mu2+b4*lam2*mu3*c33+b4*mu2*lam3;
delt2=A11^2+A12^2;
c23=(A11*(xc*c13+yc*s13-a1)-A12*(-xc*lam1*s13+yc*lam1*c13+(zc-b1)*mu1))/delt2;
tet23=acos(c23)*180/pi;
s23=(A12*(xc*c13+yc*s13-a1)+A11*(-xc*lam1*s13+yc*lam1*c13+(zc-b1)*mu1))/delt2;
Q13=[c13 -lam1*s13 mu1*s13; s13 lam1*c13 -mu1*c13; 0 mu1 lam1];
Q23=[c23 -lam2*s23 mu2*s23; s23 lam2*c23 -mu2*c23; 0 mu2 lam2];
Q33=[c33 -lam3*s33 mu3*s33; s33 lam3*c33 -mu3*c33; 0 mu3 lam3];
R3=(Q13*Q23*Q33)'*[q11 q12 q13; q21 q22 q23; q31 q32 q33];
r11=R3(1,1); r12=R3(1,2); r13=R3(1,3); r21=R3(2,1); r22=R3(2,2);
r23=R3(2,3); r31=R3(3,1); r32=R3(3,2); r33=R3(3,3);
u=[q31; q32; q33];
e643=(Q13*Q23*Q33)'*u;
xi3=e643(1); et3=e643(2); ze3=e643(3);

%lam5-ze3*lam4-et3*mu4=0
tau43=0;
tet43=2*atan(tau43);
c43=cos(tet43); s43=sin(tet43);
s53=((mu6*r12+lam6*r13)*c43+(mu6*r22+lam6*r23)*s43)/mu5;
c53=(lam4*(mu6*r12+lam6*r13)*s43-lam4*(mu6*r22+lam6*r23)*c43mu4*(mu6*r32+lam6*r33))/mu5;
w13=r11*c43+r21*s43;
w23=-lam4*(r11*s43-r21*c43)+mu4*r31;
w33=mu4*(r11*s43-r21*c43)+lam4*r31;
c63=w13*c53+w23*s53;
s63=-w13*lam5*s53+w23*lam5*c53+w33*mu5;
tet43d=tet43*180/pi;
tet53=acos(c53); tet53d=tet53*180/pi;
tet63=acos(c63); tet63d=tet63*180/pi;
%
c34=cos(tet34); s34=sin(tet34);
c14=(-G*(C*c34+D*s34+E)+B*(H*c34+I*s34+J))/delta1;
tet14=acos(c14);
s14=(F*(C*c34+D*s34+E)-A*(H*c34+I*s34+J))/delta1;
A11=a2+a3*c34+b4*mu3*s34;
A12=-a3*lam2*s34+b3*mu2+b4*lam2*mu3*c34+b4*mu2*lam3;
delt2=A11^2+A12^2;
c24=(A11*(xc*c14+yc*s14-a1)-A12*(-xc*lam1*s14+yc*lam1*c14+(zc-b1)*mu1))/delt2;
tet24=acos(c24)*180/pi;
s24=(A12*(xc*c14+yc*s14-a1)+A11*(-xc*lam1*s14+yc*lam1*c14+(zc-b1)*mu1))/delt2;
Q14=[c14 -lam1*s14 mu1*s14; s14 lam1*c14 -mu1*c14; 0 mu1 lam1];
Q24=[c24 -lam2*s24 mu2*s24; s24 lam2*c24 -mu2*c24; 0 mu2 lam2];
Q34=[c34 -lam3*s34 mu3*s34; s34 lam3*c34 -mu3*c34; 0 mu3 lam3];
R4=(Q14*Q24*Q34)'*[q11 q12 q13; q21 q22 q23; q31 q32 q33];
r11=R4(1,1); r12=R4(1,2); r13=R4(1,3); r21=R4(2,1); r22=R4(2,2);
r23=R4(2,3); r31=R4(3,1); r32=R4(3,2); r33=R4(3,3);
u=[q31; q32; q33];
e644=(Q14*Q24*Q34)'*u;
xi4=e644(1); et4=e644(2); ze4=e644(3);
%lam5-ze4*lam4-et4*mu4=0
tau44=0;
tet44=2*atan(tau44);
c44=cos(tet44)
s44=sin(tet44)
s54=((mu6*r12+lam6*r13)*c44+(mu6*r22+lam6*r23)*s44)/mu5;
c54=(lam4*(mu6*r12+lam6*r13)*s44-lam4*(mu6*r22+lam6*r23)*c44mu4*(mu6*r32+lam6*r33))/mu5;
w14=r11*c44+r21*s44;
w24=-lam4*(r11*s44-r21*c44)+mu4*r31;
w34=mu4*(r11*s44-r21*c44)+lam4*r31;
c64=w14*c54+w24*s54;
s64=-w14*lam5*s54+w24*lam5*c54+w34*mu5;
tet44d=tet44*180/pi;
tet54=acos(c54); tet54d=tet54*180/pi;
tet64=acos(c64); tet64d=tet64*180/pi;