%% Résolution du problème "bras plan" en algèbre spatiale
%
%
%
%% Définition des variables symboliques
% Ce sont les mêmes que les variables utilisées pour obtenir le modèle analytique

%paramètres géométriques
syms sym_l1 sym_l2 sym_a sym_b real
%paramètres inertiels
syms sym_m1 sym_m2 sym_I1 sym_I2 real

% Paramètres du mouvement
syms q1 q2 dq1 dq2 ddq1 ddq2 real

q=[q1 q2]';
dq=[dq1 dq2]';
ddq=[ddq1 ddq2]';
n=numel(q);
% Paramètres d'efforts

syms Fx Fy real

Fext=[Fx Fy 0]';
% Autres
syms g real %gravité
%% Matrices de transformation homogène

T01=[cos(q1) -sin(q1) 0 0
     sin(q1)  cos(q1) 0 0   
     0        0       1 0
     0        0      0 1];
 
 T12=[cos(q2) -sin(q2) 0 sym_l1
     sin(q2)  cos(q2) 0 0   
     0        0       1 0
     0        0      0 1];
 T02=simplify(T01*T12);

%% Positions des centres de masse

c1=[sym_a 0 0 1]';%dans le repère 1
c2=[sym_b 0 0 1]';%dans le repère 2

c01=T01*c1;
c02=T02*c2;

% point d'application de la force
O2=T02*[sym_l2 0 0  1]';
O2r=O2(1:3);

%
%
%% Calcul des vitesses articulaires
% On calcule d'abord les sous espaces de mouvement associés à chacune des liaisons de la chaine de soildes
%
%
% u_i est le vecteur axe de la liaison i (exprimé dans R_0 ici)
% p_i est le vecteur position de la liaison i (exprimé dans R_0 ici)



%
% D'où les vitesses spatiales
%



%
%% Calcul des accélérations spatiales
%
% il est nécessaire de définir une fonction "chapeau" qui génère l'opérateur produit vectoriel, pour que l'on se simplifie la vie !



%
%% Calcul des inerties spatiales
% Il est nécessaire de calculer les inerties complètes, donc de définir les inerties complètes des solides.

syms Ixx1 Iyy1 Ixx2 Iyy2 real

%
%% Résolution (pseudo Newton-Euler)
% On remet les efforts s'appliquant sur nos deux solides en O



% Equilibre d'un solide:
%
% On isole le solide 2



% On isole le solide 1



%
%% Calcul des couples articulaires
% On applique la transformation inverse pour retrouver le couple:
%

