%% Modelisation d un robot mobile
clc
clear
close
tic
%% Donn�es du probleme
% Parametres Inertielles
M_Body = 7.50;
M_Wheel = 2;
M_Leg = 1;
M_Bar_1 = 1;
M_Bar_2 = 1;
I_Body = 2;
I_Wheel = 0.1;
I_Leg = 0.06;
I_Bar_1 = 0.06;
I_Bar_2 = 0.06;
% Parametres Geometriques
global L_1 L_2 L_3 R_W l_B h_B hh L_B L_C L_E L_D phi_E phi_D
L_1 = 0.527;
L_2 = 0.409;
L_3 = 0.368;
R_W = 0.2;
hh = 0.05;
l_B = 0.580;
h_B = 0.378;
L_B = 0.319 - L_1*0.5;
L_C = L_1 * 0.5;
L_E = 0.079;
L_D = 0.179;
phi_E = deg2rad(9.5);
phi_D = deg2rad(237);
% Damping and Stiffness
% KK_BW = 10000;
% CC_BW = 1000;
% KK_WG = 50000;
% CC_WG = 5000;
nb = 5;
% PID Parameter of stabilization of turret
PID_p = 0;
PID_i = 0;
PID_d = 0;
%
Kp = 1000;%8000000;
Kd = 100;%1380;
Ki = 0.5;%6200;
% Matrice Masse du systeme
M = diag([M_Body M_Body I_Body ...
M_Wheel M_Wheel I_Wheel ...
M_Leg M_Leg I_Leg ...
M_Bar_1 M_Bar_1 I_Bar_1 ...
M_Bar_2 M_Bar_2 I_Bar_2 ]);
g = -9.81;
mu1 = 0.9;
mu2 = 0.9;
e = 0.00002;
treshold = 0.0001;
% Obstacle points // Ground form
polyy = sin_To_Polyline(8.00,0.4,150,16,76);
Poly_Ground1 = [-2 0;25 0;25 0.2;25.4 0.2;25.4 0;25.7 0;25.7 0.2;26.1 0.2;26.1
0;26.4 0;26.4 0.2;26.8 0.2;26.8 0;27.1 0;27.1 0.2;27.5 0.2;27.5 0;27.8 0;27.8
0.2;28.2 0.2;28.2 0;28.5 0;38.5 -1;48.5 0;53.5 0;55 0.4;57 -0.2;58 -0.1;64.2 -
1;75.8 0.6;82.65 -0.56;88.5 0.6;115 -0.4;116 0;500 0];
Poly_Ground2 = [-2 0;30 0;30.4 0.2;30.8 0;50 0;50 0.2;50.5 0.2;50.5 0;70 0;70 -
0.25;71.4 -0.25;71.4 0;2000 0];
Poly_Ground3 = [-2 0;10 0;10 0.2;10.4 0.2;10.4 0;50 0];
Poly_Ground4 = [-4 0;16 0;polyy;76 0;2000 0];
Poly_Ground = Poly_Ground1;
%
% % Terrain : route plane puis descente
% Poly_Ground = [
% -10 0; % D�but du terrain (x = -10, y = 0)
% 10 0; % Fin de la route plane (x = 10, y = 0)
% 20 -5; % Descente (x = 20, y = -5)
% 30 -5; % Route plate apr�s descente (x = 30, y = -5)
% 40 0; % Remont�e (optionnelle, x = 40, y = 0)
% 50 0; % Continuit� de la route plane (x = 50, y = 0)
% 60 -2; % Nouvelle descente optionnelle
% 70 -2; % Continuit� en bas
% 80 0; % transition
% ]
ti = 0;
tf = 2;
delta = 0.001;
T_simul = ti:delta:tf;
dt=0.001;
% Vitesse roues
%tau1 = -400;
%tau2 = -400;
V_Car_km_h = 10;
V_Car = V_Car_km_h/3.6;
K_Car = 2;
previous_error = 0;
%% Conditions initiales
% q = [ x_Body y_Body theta_Body
% x_Wheel y_Wheel theta_Wheel
% x_Leg y_Leg theta_Leg
% x_Bar_1 y_Bar_1 theta_Bar_1
% x_Bar_2 y_Bar_2 theta_Bar_2]
% Positions
theta1(1) = 0.0 ;
theta2(1) = 0.0 ;
theta3(1) = deg2rad(48.54);
theta4(1) = - deg2rad(180 - 141.71);
theta5(1) = - deg2rad(180 - 140.31);
x1(1) = L_1 * cos(theta3(1)) - L_3 * cos(theta5(1)) - L_E * cos(phi_E);
y1(1) = R_W + L_1 * sin(theta3(1)) - L_3 * sin(theta5(1)) + L_E * sin(phi_E);
x2(1) = 0.0 ; y2(1) = R_W ;
x3(1) = 0.5 * L_1 * cos(theta3(1)) ; y3(1) = R_W + 0.5 * L_1 * sin(theta3(1));
x4(1) = x3(1) + L_B * cos(theta3(1)) - 0.5 * L_2 * cos(theta4(1));
y4(1) = y3(1) + L_B * sin(theta3(1)) - 0.5 * L_2 * sin(theta4(1));
x5(1) = L_1 * cos(theta3(1)) - 0.5 * L_3 * cos(theta5(1));
y5(1) = R_W + L_1 * sin(theta3(1)) - 0.5 * L_3 * sin(theta5(1));
% Vitesse
x1_p(1) = 0.0 ; y1_p(1) = 0.0; theta1_p(1) = 0.0;
x2_p(1) = 0.0 ; y2_p(1) = 0.0; theta2_p(1) = 0.0;
x3_p(1) = 0.0 ; y3_p(1) = 0.0; theta3_p(1) = 0.0;
x4_p(1) = 0.0 ; y4_p(1) = 0.0; theta4_p(1) = 0.0;
x5_p(1) = 0.0 ; y5_p(1) = 0.0; theta5_p(1) = 0.0;
%% MAIN LOOP
for i = 1 : length(T_simul)
% Vecteur des coordonn�es generalis�es
q = [x1(i) y1(i) theta1(i) ...
x2(i) y2(i) theta2(i) ...
x3(i) y3(i) theta3(i) ...
x4(i) y4(i) theta4(i) ...
x5(i) y5(i) theta5(i)]';
% Vecteur des vitesses generalis�es
q_p = [x1_p(i) y1_p(i) theta1_p(i) ...
x2_p(i) y2_p(i) theta2_p(i) ...
x3_p(i) y3_p(i) theta3_p(i) ...
x4_p(i) y4_p(i) theta4_p(i) ...
x5_p(i) y5_p(i) theta5_p(i)]';
% Les contraintes des liaisons et Jaccobienne des contraintes des liaisons
st1 = sin(theta1(i)) ; ct1 = cos(theta1(i));
st2 = sin(theta2(i)) ; ct2 = cos(theta2(i));
st3 = sin(theta3(i)) ; ct3 = cos(theta3(i));
st4 = sin(theta4(i)) ; ct4 = cos(theta4(i));
st5 = sin(theta5(i)) ; ct5 = cos(theta5(i));
[J,phi] = constraints_Jacobian(q);
phi1(i) = phi(1) ;
phi2(i) = phi(2) ;
phi3(i) = phi(3) ;
phi4(i) = phi(4) ;
phi5(i) = phi(5) ;
phi6(i) = phi(6) ;
phi7(i) = phi(7) ;
phi8(i) = phi(8) ;
phi9(i) = phi(9) ;
phi10(i) = phi(10) ;
%%
target_position=0;
error = target_position - theta1(i);
% Calcul des termes du PID
PID_p = Kp * error; % Terme proportionnel
PID_i = PID_i + Ki * error * dt; % Terme int�gral (accumulation de
l'erreur)
PID_d = (error - previous_error) / dt; % Terme d�riv� (changement de
l'erreur)
% Calcul de la sortie PID
output = PID_p + PID_i + PID_d; % Somme des termes PID
% Mettre � jour la position du moteur (en fonction de la sortie)
theta1(i) = theta1(i) + output * dt; % Ajuster la position du moteur
% Sauvegarder la position pour l'affichage
position_history(i) = theta1(i);
% Mettre � jour l'erreur pr�c�dente pour le prochain calcul
previous_error = error;
R4 = [ct4 -st4 0;st4 ct4 0;0 0 1];
tau1(i) = -K_Car * (V_Car - x1_p(i));
F_Body = [0 ;M_Body * g ;0] ;
F_Wheel = [0 ;M_Wheel * g ;0] + [ 0 ; 0 ; tau1(i)];
F_Leg = [10 ;M_Leg * g ;0] ;
F_Bar_1 = [20 ;M_Bar_1 * g ;20] +[ 0 ; 0 ; output];
F_Bar_2 = [0 ;M_Bar_2 * g ;0] ;
Fg = [F_Body ;F_Wheel;F_Leg;F_Bar_1;F_Bar_2];
h = M * q_p + delta * Fg;
% Stocker les forces
forces(i, :) = Fg;
% Stocker les vitesses
velocities(i, :) = q_p';
% Calculer et stocker les acc�l�rations (a = dq/dt)
if i > 1
accelerations(i, :) = (velocities(i, :) - velocities(i - 1, :)) / delta;
end
% Detection de contact
======================================================================
[yes_nott,ncc,nc,n,nbc,rG,err,Cnn] = Contact_Detection(q,Poly_Ground);
if yes_nott == 1
[Wn,D,E,b] = Wrenches_Matrix_2D(nb,ncc,nc,n,rG,nbc,err,e,treshold,delta);
mu = mu1 * eye(ncc);
H = [Wn D zeros(3*nb,ncc)];
N = [zeros(ncc,4*ncc);zeros(2*ncc,ncc*(1+2)) E;mu -E' zeros(ncc,ncc)];
invM_H=M\H;
invM_Jt = M\J';
invM_h=M\h;
G = H' * invM_H - H' * invM_Jt * inv(J * invM_Jt) * J * invM_H ;
s = b + H' * invM_h - H' * invM_Jt * inv(J * invM_Jt) *(J * invM_h);
A = G + N;
C = s;
xx = pathlcp(A,C);
impulse = - inv( J * inv(M) * J') * J * inv(M) * (H * xx + h);% + inv(J
* inv(M) * J') * gamma;
q_p = M \ (J' * impulse + H * xx + h);
end
if yes_nott == 0
K = [M -J';J zeros(10,10)];
B = [h;zeros(10,1)];
q_p = K\B;
end
x1_p(i+1) = q_p(1);
y1_p(i+1) = q_p(2);
theta1_p(i+1) = q_p(3);
x2_p(i+1) = q_p(4);
y2_p(i+1) = q_p(5);
theta2_p(i+1) = q_p(6);
x3_p(i+1) = q_p(7);
y3_p(i+1) = q_p(8);
theta3_p(i+1) = q_p(9);
x4_p(i+1) = q_p(10);
y4_p(i+1) = q_p(11);
theta4_p(i+1) = q_p(12);
x5_p(i+1) = q_p(13);
y5_p(i+1) = q_p(14);
theta5_p(i+1) = q_p(15);
q_new(1) = x1(i) + delta * x1_p(i+1);
q_new(2) = y1(i) + delta * y1_p(i+1);
q_new(3) = theta1(i) + delta * theta1_p(i+1);
q_new(4) = x2(i) + delta * x2_p(i+1);
q_new(5) = y2(i) + delta * y2_p(i+1);
q_new(6) = theta2(i) + delta * theta2_p(i+1);
q_new(7) = x3(i) + delta * x3_p(i+1);
q_new(8) = y3(i) + delta * y3_p(i+1);
q_new(9) = theta3(i) + delta * theta3_p(i+1);
q_new(10) = x4(i) + delta * x4_p(i+1);
q_new(11) = y4(i) + delta * y4_p(i+1);
q_new(12) = theta4(i) + delta * theta4_p(i+1);
q_new(13) = x5(i) + delta * x5_p(i+1);
q_new(14) = y5(i) + delta * y5_p(i+1);
q_new(15) = theta5(i) + delta * theta5_p(i+1);
[J_new,Phi_new] = constraints_Jacobian(q_new);
dq_new = -(J_new'*inv(J_new*J_new'))*Phi_new;
x1(i+1) = q_new(1) + dq_new(1);
y1(i+1) = q_new(2) + dq_new(2);
theta1(i+1) = q_new(3) + dq_new(3);
x2(i+1) = q_new(4) + dq_new(4);
y2(i+1) = q_new(5) + dq_new(5);
theta2(i+1) = q_new(6) + dq_new(6);
x3(i+1) = q_new(7) + dq_new(7);
y3(i+1) = q_new(8) + dq_new(8);
theta3(i+1) = q_new(9) + dq_new(9);
x4(i+1) = q_new(10) + dq_new(10);
y4(i+1) = q_new(11) + dq_new(11);
theta4(i+1) = q_new(12) + dq_new(12);
x5(i+1) = q_new(13) + dq_new(13);
y5(i+1) = q_new(14) + dq_new(14);
theta5(i+1) = q_new(15) + dq_new(15);
end
toc
%% Animation
===================================================================================
=
hold on
grid on
axis([-4 4 -4 4]);
v = VideoWriter('V30_T625_A020_W.avi');
open(v);
for i = 1 :150: length(T_simul)-3
cla
axis([x1(i)-1 x1(i)+1 -0.5 2]);
hold on
RR1 = [cos(theta1(i)) -sin(theta1(i));
sin(theta1(i)) cos(theta1(i))];
RR3 = [cos(theta3(i)) -sin(theta3(i));
sin(theta3(i)) cos(theta3(i))];
RR4 = [cos(theta4(i)) -sin(theta4(i));
sin(theta4(i)) cos(theta4(i))];
RR5 = [cos(theta5(i)) -sin(theta5(i));
sin(theta5(i)) cos(theta5(i))];
G1 = [x1(i) y1(i)]';
b11 = G1 + RR1*[0.5 * l_B 0.5 * h_B]';
b12 = G1 + RR1*[-0.5 * l_B 0.5 * h_B]';
b13 = G1 + RR1*[-0.5 * l_B -0.5 * h_B]';
b14 = G1 + RR1*[0.5 * l_B -0.5 * h_B]';
G3 = [x3(i) y3(i)]';
b31 = G3 + RR3*[0.5 * L_1 0.5 * hh]';
b32 = G3 + RR3*[-0.5 * L_1 0.5 * hh]';
b33 = G3 + RR3*[-0.5 * L_1 -0.5 * hh]';
b34 = G3 + RR3*[0.5 * L_1 -0.5 * hh]';
G4 = [x4(i) y4(i)]';
b41 = G4 + RR4*[0.5 * L_2 0.5 * hh]';
b42 = G4 + RR4*[-0.5 * L_2 0.5 * hh]';
b43 = G4 + RR4*[-0.5 * L_2 -0.5 * hh]';
b44 = G4 + RR4*[0.5 * L_2 -0.5 * hh]';
G5 = [x5(i) y5(i)]';
b51 = G5 + RR5*[0.5 * L_3 0.5 * hh]';
b52 = G5 + RR5*[-0.5 * L_3 0.5 * hh]';
b53 = G5 + RR5*[-0.5 * L_3 -0.5 * hh]';
b54 = G5 + RR5*[0.5 * L_3 -0.5 * hh]';
Poly_Body = [b11';b12';b13';b14';b11'];
Poly_Leg = [b31';b32';b33';b34';b31'];
Poly_Bar_1 = [b41';b42';b43';b44';b41'];
Poly_Bar_2 = [b51';b52';b53';b54';b51'];
drawPolyline(Poly_Ground,'k', 'lineWidth', 2);
drawPolyline(Poly_Body,'g', 'lineWidth', 2);
drawPolyline(Poly_Leg,'b', 'lineWidth', 2);
drawPolyline(Poly_Bar_1,'b', 'lineWidth', 2);
drawPolyline(Poly_Bar_2,'b', 'lineWidth', 2);
%drawCircle(x2(i),y2(i),R_W,4,6);
drawCircle(x2(i),y2(i),R_W);
drawPoint(x2(i),y2(i));
plot([x2(i) x2(i)+R_W*cos(theta2(i))],[y2(i) y2(i)
+R_W*sin(theta2(i))],'r','Linewidth',3);
frame = getframe(gcf);
writeVideo(v,frame);
pause(0.01)
end
close(v);
% % Tracer les forces
% figure;
% plot(T_simul, forces(:, 1), 'r', 'LineWidth', 1.5);
% hold on;
% plot(T_simul, forces(:, 2), 'b', 'LineWidth', 1.5);
% xlabel('Temps (s)');
% ylabel('Forces (N)');
% legend('Force x1', 'Force y1');
% title('Forces g�n�ralis�es');
% grid on;
%
% % Tracer les vitesses
% figure;
% plot(T_simul, velocities(:, 1), 'r', 'LineWidth', 1.5);
% hold on;
% plot(T_simul, velocities(:, 2), 'b', 'LineWidth', 1.5);
% xlabel('Temps (s)');
% ylabel('Vitesses (m/s)');
% legend('Vitesse x1', 'Vitesse y1');
% title('Vitesses g�n�ralis�es');
% grid on;
%
% % Tracer les acc�l�rations
% figure;
% plot(T_simul, accelerations(:, 1), 'r', 'LineWidth', 1.5);
% hold on;
% plot(T_simul, accelerations(:, 2), 'b', 'LineWidth', 1.5);
% xlabel('Temps (s)');
% ylabel('Acc�l�rations (m/s�)');
% legend('Acc�l�ration x1', 'Acc�l�ration y1');
% title('Acc�l�rations g�n�ralis�es');
% grid on;
%
%
