--- jupyter: jupytext: text_representation: extension: .md format_name: markdown format_version: '1.3' jupytext_version: 1.14.1 kernelspec: display_name: Python 3 (ipykernel) language: python name: python3 --- # TP Planification: étude d'un pont roulant ![pont_roulant](images/pont_roulant.jpg) ```python hide_input=false nbgrader={"grade": false, "grade_id": "cell-2bc44daa972430c4", "locked": true, "schema_version": 3, "solution": false} %matplotlib inline import numpy as np import matplotlib.pyplot as plt from IPython.display import Markdown, display from validation.validation import info_etudiant def printmd(string): display(Markdown(string)) # test si numero étudiant spécifier try: NUMERO_ETUDIANT except NameError: NUMERO_ETUDIANT = None if type(NUMERO_ETUDIANT) is not int : printmd("**ERREUR:** numéro d'étudiant non spécifié!!!") NOM, PRENOM, NUMERO_ETUDIANT=info_etudiant() NOM='Toto' PRENOM='toto' # parametres spécifiques _uid_ = NUMERO_ETUDIANT _precis_ = 1.0e-5 printmd("**Login étudiant {} {} uid={}**".format(NOM,PRENOM,_uid_)) np.random.seed(_uid_) # parametres _XF_ = 4.0 + np.random.randint(4) _L0_ = 5.0 + np.random.randint(10) _L1_ = 2.0 + np.random.randint(10) _L2_ = _L1_ /2. _ALPHA_ = np.round(0.1+np.random.rand(),1) printmd("**parametres de la trajectoire**: Xf={} L0={} Lf={} Lm={} alpha={}". format(_XF_,_L0_,_L1_,_L2_,_ALPHA_)) ``` ## Modélisation ### objectif On veut déplacer, à l’aide d’un système de levage par câble (grue ou pont roulant), une charge d’un endroit à un autre en évitant les oscillations résiduelles à l’arrivée. L’objectif est de proposer une stratégie de commande simple et réaliste qui repose sur une structure de commande hiérarchisée, composée de régulateurs de bas niveau rapides, simples et découplés et d’une commande de haut niveau lente et prenant en compte les couplages. En outre, on mesure la position et la vitesse du chariot ainsi que la position et la vitesse du treuil, mais la position de la charge n’est pas mesurée. C’est pourquoi nous ne considérons que des bouclages qui ne dépendent que des positions et vitesses du chariot et du treuil. Le cas d'une trajectoire simple avec un cable de longueur constante $L_0$ sera traiter ensemble en début de TP et vous aurez à traiter ensuite les 2 cas suivants avec les paramètres individuels données plus haut. - trajectoire ascendente directe avec levage du cable de $L_0$ à $L_f$ (de A à B) - trajectoire avec un obstacle nécessitant un levage du cable à $L_m$ (de A à B en passant par C) (optionnel)

On vous demandera de rédiger un CR succinct sur ces 2 cas dans le fichier `monCR.tex` sur la stratégie de contrôle mise en place pour minimiser les oscillations de la charge en fin de trajectoire. ### modélisation ![chariot pendule](images/chariot-pendule.png) - modèle mécanique = masse ponctuelle P (masse) et chariot (C) pendule P de masse m de longueur $l(t)$ accroché à un chariot C de masse M se deplacant horizontalement $x_c(t)$ - système à 3 ddl $xc(t), \theta(t), l(t) $ - treuil de rayon $\rho$ - rayon $\rho$ (on néglige son inertie $J_\rho$) - angle $\phi = (l-l_0)/\rho$ ```python nbgrader={"grade": false, "grade_id": "cell-dd8b0b1aa89b736e", "locked": true, "schema_version": 3, "solution": false} slideshow={"slide_type": "-"} %matplotlib inline import numpy as np import sympy as sp import matplotlib.pyplot as plt plt.rc('font', family='serif', size='14') from sympy.physics.vector import init_vprinting init_vprinting(use_latex='mathjax', pretty_print=False) from sympy.physics.mechanics import dynamicsymbols, Point, ReferenceFrame from sympy.physics.mechanics import Particle, Lagrangian, LagrangesMethod ``` ### Définition des repères et ddl - définir les ddl avec `dynamicsymbols` - définir les paramètres avec `symbols` - définir les repères RO, RP `ReferencePoint` et les points O, P et C `Point` ```python slideshow={"slide_type": "-"} # définition des parametres theta, xc, l = dynamicsymbols("theta x_c l") thetap, xcp, lp = dynamicsymbols("theta x_c l",1) m,M,g = sp.symbols('m M g') t = sp.Symbol('t') ``` ```python nbgrader={"grade": true, "grade_id": "cell-0a806ed11baf4b72", "locked": false, "points": 1, "schema_version": 3, "solution": true} slideshow={"slide_type": "-"} # définition des pts et des répères O = 0 RO = 0 C = 0 RP = 0 P = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ### cinématique - calcul vitesse des points P et C - définition des masses ponctuelles Pc et Pa associées au chariot et à la masse m `Particle` - calcul de l'energie potentielle ```python nbgrader={"grade": true, "grade_id": "cell-72e505baa9ced9c7", "locked": false, "points": 1, "schema_version": 3, "solution": true} slideshow={"slide_type": "-"} # vitesse de C et P ### BEGIN SOLUTION ### END SOLUTION ``` ```python nbgrader={"grade": true, "grade_id": "cell-1624b2fd1a1463b2", "locked": false, "points": 1, "schema_version": 3, "solution": true} slideshow={"slide_type": "-"} # definition des masses ponctuelles et calcul de l'énergie potentiel Pa = 0 Pc = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ### Lagrangien - calcul du lagrangien du système chariot+masse dans La `Lagrangian` ```python nbgrader={"grade": true, "grade_id": "cell-03dd902faa9b3164", "locked": false, "points": 1, "schema_version": 3, "solution": true} slideshow={"slide_type": "-"} # calcul La La = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ### Bilan des forces - force motrice $F$ sur le chariot en C - force de tension $T$ dans le cable (ne travaille pas) - couple sur un treuil de rayon $\rho$ - couple $C$ - travail $C_t \rho \delta l $ - $\alpha = \frac{M}{m}$ rapport des masses - adimensionalisation: - $ F = F_c M = F_c\alpha m $ - $ C = C_t m \rho $ - définition d'un repère Rt lié au treuil - calcul angle $\phi_t$ en fct de $l(t)$ (enroulement autour du treuil) - $l_0$ longueur de référence ```python nbgrader={"grade": true, "grade_id": "cell-437fdb7a659a6fce", "locked": false, "points": 1, "schema_version": 3, "solution": true} slideshow={"slide_type": "-"} # definition Fc et Ct et des parametres Fc, Ct, rho, l0 = sp.symbols('F_c C_t rho l_0') phi_t = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ### Equations de Lagrange calcul des équations de Lagrange `LagrangesMethod` avec le travail des forces extérieures - force de traction du chariot Fc - couple sur le treuil Ct ```python nbgrader={"grade": true, "grade_id": "cell-fd0b9910ff2f4c06", "locked": false, "points": 1, "schema_version": 3, "solution": true} slideshow={"slide_type": "-"} # calcul des equations dans eq en introduisant LM = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ## Chariot mobile, longueur fixe - on applique une force motrice horizontale $F_c$ avec un cable de longueur $l_0$ fixe - adim: $ F_c M = F_c \alpha m $ - $\alpha = \frac{M}{m}$ - **objectif:** - déterminer la force $F_c$ pour avoir un ballant minimum - tests de différents contrôle - contrôle du chariot seule - asservissement de la trajectoire du chariot - couplage de haut niveau - calcul du nouveau lagrangien - calcul des equations de lagranges - introduction de $\alpha = M/m$ ```python nbgrader={"grade": true, "grade_id": "cell-6e857b4e6b340abd", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # calcul lagrangien LLa alpha = sp.Symbol('alpha') LLa = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ```python nbgrader={"grade": true, "grade_id": "cell-8e6b759d46b44548", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # calcul equation de Lagrange Fc = sp.symbols('F_c') LM = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ### 1ere methode: contrôle simple du chariot On contrôle uniquement le chariot $x_c(t)$ sans tenir compte de l'interaction avec la masse P: on suppose que $\theta \approx 0=cste$ L'équation du mouvement du chariot s'écrit alors $$ (1+\alpha)\ddot{x_c} = \alpha F_c$$ - on impose une trajectoire simple au chariot tq. $$ x_c(0)=0, \dot{x_c}(0)=0, x_c(t_f) = x_f, \dot{x_c}(t_f)=0$$ - d'où $x_c(t)$ polynôme de degré 3 $$ x_c(t) = x_f \frac{t^2}{t_f^2}(3-2\frac{t}{t_f})$$ - d'où la force $F_c$ sur le chariot - definition analytique - conversion fonction python avec `lambdify` ```python nbgrader={"grade": true, "grade_id": "cell-bd518c8e152843b5", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # calcul de xcc et Fcc xf,tf = sp.symbols('x_f t_f') xcc = 0 FC = 0 FCX = 0 ### BEGIN SOLUTION # fonction python FCX ### END SOLUTION ``` ### 2ieme méthode: asservissement simple - on applique un asservissement simple pour avoir la trajectoire précédente $$ x_i(t) = x_f \frac{t^2}{t_f^2}(3-2\frac{t}{t_f})$$ - asservissement $$ F_a(t) = -K_c \left( (x(t)-x_i(t)) + T_c(\dot x - \dot x_i) \right) $$ ```python nbgrader={"grade": true, "grade_id": "cell-527df3385ef1ec53", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # definition asservissement Fca et conversion en fonction Python FCA Kc,Tc = sp.symbols('K_c T_c') Fca = 0 FCA = 0 ### BEGIN SOLUTION # force sur le treuil ### END SOLUTION ``` ### 3ieme méthode: commande de haut niveau lente - linéarisation des équations - contrôle du positionnement de la charge P - calcul de la commande en fonction du mvt de la charge #### mouvement de la charge - calcul des coordonnées de la charge P: $\xi(t)$, $\eta(t)$ dans le repère fixe - relation entre $\theta$ $\xi$ et $\eta$ ```python # calcul de xi et eta display(P.pos_from(O).express(RO)) xi, eta = dynamicsymbols('xi eta') display(sp.Eq(xi,P.pos_from(O).dot(RO.x))) display(sp.Eq(eta,P.pos_from(O).dot(RO.y))) display(sp.Eq(sp.tan(theta),(-xi+xc)/eta)) ``` #### paramétrisation / trajectoire - équation d'équilibre de la charge en fonction de la tension T - réécrire les équations du mvt pour obtenir $\theta$ et $x_c$ en fct de $\xi$ et $\eta$ ```python # relation xc et xi et eta T = sp.Symbol('T') display(sp.Eq(m*xi.diff(t,t) , -T*sp.sin(theta))) display(sp.Eq(m*eta.diff(t,t), T*sp.cos(theta) - m*g)) display(sp.Eq(sp.tan(theta), -xi.diff(t,t)/(eta.diff(t,t)+g))) display(sp.Eq(xc, xi - eta*xi.diff(t,t)/(eta.diff(t,t)+g)) ) ``` #### paramétrisation si on connait $\xi(t)$ et $\eta(t)$ jusqu'à leurs dérivées d'ordre 4, peut donc déterminer le mouvement et calculer la force $F_c$ 'on doit calculer $\ddot{x_c}$ et $\ddot{\theta$}$ **idée** on va déterminer le mouvement de la masse M (i.e. $\xi(t)$ et $\eta(t)$) pour pouvoir en déduire la force $F_c$. Pour cela on va se placer dans le cas de petites oscillations (linéarisation) #### linéarisation $\theta$ petit - $\eta = - l_0$ - $\theta = -\frac{\ddot{\xi}}{g}$ - $x_c = \xi + \frac{l_0}{g} \ddot{\xi}$ - $\omega_0 ^2 = \frac{g}{l_0}$ calcul de la force $F_c$ dans ce cas en fct de $\xi(t)$ et $\omega_0$ ```python nbgrader={"grade": true, "grade_id": "cell-fd2b2aa391b87a3f", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # calcul force Fc dans Fc1 omega0 = sp.Symbol('omega_0') Fc1 = 0 ### BEGIN SOLUTION ### END SOLUTION ``` #### choix de la trajectoire $\xi(t)$ la plus régulière t.q. On veut que la masse M soit immobile au départ et à l'arrivée - trajectoire $\xi(t)$ idéale tq. $$\xi(0)=0, \frac{d\xi}{dt}(0)=0, \frac{d^2\xi}{dt^2}(0)=0, \frac{d^3\xi}{dt^3}(0)=0, \frac{d^4\xi}{dt^4}(0)=0$$ $$\xi(t_f)=x_f, \frac{d\xi}{dt}(t_f)=0, \frac{d^2\xi}{dt^2}(t_f)=0, \frac{d^3\xi}{dt^3}(t_f)=0, \frac{d^4\xi}{dt^4}(t_f)=0$$ - $\xi(t)$ polynome de degré 9 en t $$\xi(t) = x_f(\frac{t}{t_f})^5 (126-420(\frac{t}{t_f})+540(\frac{t}{tf})^2-315(\frac{t}{t_f})^3+70(\frac{t}{tf})^4)$$ ```python # calcul de xi dans x, Fcx, xcx et thx (theta) xf, tf = sp.symbols('x_f t_f') x = xf*(t/tf)**5*(126-420*(t/tf)+540*(t/tf)**2-315*(t/tf)**3+70*(t/tf)**4) display(sp.Eq(xi,x)) # verification print("dérivées en tf:",x.diff(t,1).subs({t:tf}),x.diff(t,2).subs({t:tf}),x.diff(t,3).subs({t:tf}),x.diff(t,4).subs({t:tf})) # contrôle Fc Fcx=Fc1.subs({xi.diff(t,4):x.diff(t,4),xi.diff(t,2):x.diff(t,2)}).doit().simplify() display(sp.Eq(Fc,Fcx)) # position chariot xc xcx= xi + xi.diff(t,2)/omega0**2 xcx= xcx.subs({xi:x}).doit().simplify() display(sp.Eq(xc,xcx)) # angle theta thx= -xi.diff(t,2)/(l0*omega0**2) thx= thx.subs({xi:x}).doit().simplify() display(sp.Eq(theta,thx)) ``` ```python nbgrader={"grade": true, "grade_id": "cell-e579559c64aebff5", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # fonctions numériques XI, XC, FX, THX XI = 0 XC = 0 FX = 0 THX = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ### Parametres de la simulation - définition des paramètres - **attention** valeur des parametres KC et TC $$\ddot{x_c} = -K_c\left((x_c - x_i) + T_c(\dot{x_c}- \dot{x_i})\right)$$ optimal si $\Delta=0$ i.e. $Tc^2 = 4/K_c$ ```python hide_input=true nbgrader={"grade": false, "grade_id": "cell-d9ccc38cde6b57b0", "locked": true, "schema_version": 3, "solution": false, "task": false} printmd("### parametres trajectoire: Xf={} L0={} Lf={} Lm={} alpha={}". format(_XF_,_L0_,_L1_,_L2_,_ALPHA_)) ``` ```python nbgrader={"grade": true, "grade_id": "cell-78698d35c73d3680", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # PARAMETRES NUMERIQUES # tester les 3 deplacement lent, rapide, + rapide TF = 8.0 TF = 6.0 TF = 4.0 GG = 9.81 KC = 100.0 TC = np.sqrt(4./KC) # valeur de XF ALPHA L0 et tt (temps simulation) XF = 0 ALPHA = 0 L0 = 0 XF = 0 ### BEGIN SOLUTION #ALPHA = 10.0 ### END SOLUTION print("Simulation: ",KC,TC,TF,2*np.pi/OMEGA0) Nt = 400 tt = np.linspace(0,TF,Nt) ``` ### tracée de la solution idéale $\xi(t)$ ```python # tracer de la solution optimal avec xi(t) XXI = XI(tt,XF,TF) XCX = XC(tt,XF,TF,OMEGA0) FFX = FX(tt,XF,TF,OMEGA0,ALPHA) TTHX= THX(tt,XF,TF,OMEGA0,L0) plt.figure(figsize=(10,8)) plt.subplot(3,1,1) plt.plot(tt,XXI,label="P") plt.plot(tt,XCX,'--',label="C") plt.legend() plt.title("Solution planifiée: position P et C") plt.subplot(3,1,2) plt.plot(tt,FFX) plt.ylabel("Force $F_c$") plt.subplot(3,1,3) plt.plot(tt,TTHX,'--') plt.ylabel("angle $\\theta$") plt.xlabel('t'); ``` ```python # tracer du mouvement associé plt.figure(figsize=(12,12)) for i in range(0,len(tt),20): X,Y = [XCX[i],XXI[i]],[0.,-L0] plt.plot(X,Y,'-o',ms=20) plt.axis('equal'); ``` ### Simulation avec les 3 contrôles - commande simple Fs - asservissement simple Fa - commande lente couplée F définition du second membre des equations de lagrange sous forme EDO du 1er ordre $$ \dot{Y} = F(Y,t)$$ ```python display("Equations Lagrange:",LM.form_lagranges_equations()) # calcul du 2nd membre en fonction de C display("F(Y)=",sp.simplify(LM.rhs()).doit()) smb = sp.simplify(sp.simplify(LM.rhs()).doit().subs({l:l0,g:omega0**2*l0})) display("F(Y)=",smb) smbF = sp.lambdify([theta,xc,thetap,xcp,Fc,l0,omega0,alpha],smb,'numpy') ``` ```python nbgrader={"grade": true, "grade_id": "cell-1b3ce7a4d261884c", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # definition des 3 seconds membres fct du contrôle def Fs(Y,t): """commande simple""" global L0,OMEGA0,ALPHA,TF,XF def Fa(Y,t): """asservissement simple""" global L0,OMEGA0,ALPHA,TF,XF,KC,TC def F(Y,t): """commande lente couplée""" global L0,OMEGA0,ALPHA ``` ```python # simulation avec les 3 controles from scipy.integrate import odeint # psition initiale idéale Y0 = [0.0, 0.0, 0., 0. ] print(L0,OMEGA0,ALPHA,TF,XF) # perturbation #Y0 = [0.01,0.01, 0.,0.] tt = np.linspace(0,TF,Nt) sol = odeint(F,Y0,tt) sols= odeint(Fs,Y0,tt) sola= odeint(Fa,Y0,tt) # solution avec couplage THETA = sol[:,0] XXC = sol[:,1] print("Erreur sur la position finale") print("commande 1 simple erreur xc={:.4f} \t theta={:.3f} deg".format(sols[-1,1]-XF,np.degrees(sols[-1,0]))) print("commande 2 asservie erreur xc={:.4f} \t theta={:.3f} deg".format(sola[-1,1]-XF,np.degrees(sola[-1,0]))) print("commande 3 couplée erreur xc={:.4f} \t theta={:.3f} deg".format(sol[-1,1]-XF ,np.degrees(sol[-1,0]))) ``` ```python # tracer des 3 solutions plt.figure(figsize=(12,8)) plt.subplot(2,1,1) plt.plot(tt,sols[:,0],label="cde1 simple") plt.plot(tt,sola[:,0],label="cde2 asservie") plt.plot(tt,THETA , label="cde3 couplée") plt.plot(tt,TTHX,'--',label="traj. opt") plt.legend() plt.title('angle $\\theta(t)$') plt.subplot(2,1,2) plt.plot(tt,sols[:,1],label="cde1 simple") plt.plot(tt,sola[:,1],label="cde2 asservie") plt.plot(tt,XXC, label="cde3 couplée") plt.plot(tt,XCX,'--', label="traj. opt") plt.legend() plt.title('Position $x_c(t)$'); ``` ### analyse solution commande simple ```python from validation.Chariot import Chariot pas = 5 chariots = Chariot(L0,sols[::pas,1],sols[::pas,0],tt[pas]) chariots.trace(titre="commande simple",pas=4) ``` ```python # animation anims = chariots.calculanim(-0.5, XF+0.5, -1.2*L0, 0.5) ``` ```python plt.rc('animation', html='jshtml') anims ``` ### analyse solution commande asservie ```python chariota = Chariot(L0,sola[::pas,1],sola[::pas,0],tt[pas]) chariota.trace(titre="asservissement simple",pas=4) ``` ```python # animation anima = chariota.calculanim(-0.5, XF+0.5, -1.2*L0, 0.5) ``` ```python plt.rc('animation', html='jshtml') anima ``` ### analyse solution commande couplée ```python # tracer du mouvement cde lente couplee chariotc = Chariot(L0,XXC[::pas],THETA[::pas],tt[pas]) chariotc.trace(titre="cde lente couplée",pas=4) ``` ```python # animation animc = chariotc.calculanim(-0.5, XF+0.5, -1.2*L0, 0.5) ``` ```python plt.rc('animation', html='jshtml') animc ``` ## Asservissement: regulateur bas niveau pour rendre la commande précédente robuste on ajoute un asservissement pour suivre la trajectoire idéale - trajectoire imposée $x_i(t)$ à partir de $\xi(t)$ $$x_i(t) = \xi + \frac{l_0}{g} \ddot{\xi}$$ - au lieu d'imposer directement la trajectoire, on ajoute un régulateur à la cde précédente $$ F_c = F_i -K_c ( (x_c-x_i) + T_c(\dot{x_c}-\dot{x_i})) $$ ```python nbgrader={"grade": true, "grade_id": "cell-e08c7ea9927f1d35", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} # definition de Fc Kc, Tc = sp.symbols('K_c T_c') Fca = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ```python # definition du 2nd membre de l'EDO et de la force de controle Fca smbF = sp.lambdify([theta,xc,thetap,xcp,l0,omega0,alpha,Fc],smb) FCa = sp.lambdify([t,xc,xcp,tf,xf,omega0,Kc,Tc],Fca) ``` ### simulation calculer la commande FC = cde idéale + asservissement ```python nbgrader={"grade": true, "grade_id": "cell-2a9bb7f686fe7fa8", "locked": false, "points": 1, "schema_version": 3, "solution": true, "task": false} def F(Y,t): ``` ```python # parametres KC = 200 TC = np.sqrt(4/KC) print("parametres:",KC,TC,TF) # simulation Y0 = [0.0, 0.0, 0., 0. ] sol = odeint(F,Y0,tt) THETA = sol[:,0] XXC = sol[:,1] print("Erreur sur la position finale") print("commande couplée erreur xc={:.3f} \t theta={:.3f} deg".format(sol[-1,1]-XF ,np.degrees(sol[-1,0]))) ``` ```python # comparaison avec trajectoire idéale plt.figure(figsize=(12,8)) plt.subplot(2,1,1) plt.plot(tt,XXC) plt.plot(tt,XCX,'--') plt.ylabel("$\\xi$") plt.title("comparaison avec traj. ideale") plt.subplot(2,1,2) plt.plot(tt,THETA) plt.plot(tt,TTHX,'--') plt.xlabel('t') plt.ylabel("$\\theta$"); ``` ```python chariot = Chariot(L0,XXC[::pas],THETA[::pas],tt[pas]) chariot.trace(titre="asservisement + commande lente couplée",pas=4) ``` ```python # animation anim = chariot.calculanim(-0.5, XF+0.5, -1.2*L0, 0.5) ``` ```python plt.rc('animation', html='jshtml') anim ``` ## Chariot mobile et longueur de cable variable ### mouvement de la charge - coordonnees $\xi(t)$, $\eta(t)$ dans le repere fixe (voir cours) $$\xi = l \sin\theta + x-c $$ $$\eta = -l \cos\theta $$ - équations d'équilibre $$ m\ddot\xi = -T \sin\theta$$ $$ m\ddot\xi = -T \sin\theta$$ - d'où l'expression de $\theta$, $l$ et $x_c$ en fonction de $\xi$ et $\eta$ $$ \tan \theta = \frac{\ddot{\xi}}{\ddot{\eta}-g}$$ $$ x_c = \xi - \frac{\eta\ddot{\xi}}{\ddot{\eta}-g}$$ $$ (\xi - x_c)^2 + \eta^2 = l^2 $$ - trajectoire idéale on planifie une trajectoire idéale pour $\xi$ et $\eta$ , et on en déduit la valeur de $x_c(t)$ $\theta(t)$ et $l(t)$, et le contrôle associé - trajectoire rectiligne AB ![obstacle](images/obstacle.png) ```python xi, eta = dynamicsymbols('xi eta') display(sp.Eq(sp.tan(theta), xi.diff(t,t)/(eta.diff(t,t)-g))) display(sp.Eq(xc, xi - eta*xi.diff(t,t)/(eta.diff(t,t)-g)) ) ``` #### trajectoire planifiée on utilise la trajectoire planifiée précédente pour $\xi(t)$ - trajectoire $\xi(t)$ tq. $$\xi(0)=0, \frac{d\xi}{dt}(0)=0, \frac{d^2\xi}{dt^2}(0)=0, \frac{d^3\xi}{dt^3}(0)=0, \frac{d^4\xi}{dt^4}(0)=0$$ $$\xi(t_f)=x_f, \frac{d\xi}{dt}(t_f)=0, \frac{d^2\xi}{dt^2}(t_f)=0, \frac{d^3\xi}{dt^3}(t_f)=0, \frac{d^4\xi}{dt^4}(t_f)=0$$ - $\xi(t)$ polynome de degré 9 en t pour la trajectoire $\eta(t)$ (même condition que $\xi$) sauf sur la position - $\eta=-l_0$ longueur à $t=0$ - $\eta=-l_f$ longueur à $t=t_f$ ```python xf, tf = sp.symbols('x_f t_f') x = xf*(t/tf)**5*(126-420*(t/tf)+540*(t/tf)**2-315*(t/tf)**3+70*(t/tf)**4) display(sp.Eq(xi,x)) # verification print("dérivées en tf:",x.diff(t,1).subs({t:tf}),x.diff(t,2).subs({t:tf}),x.diff(t,3).subs({t:tf}),x.diff(t,4).subs({t:tf})) ``` ```python lf = sp.symbols('l_f') y = - l0 - (lf - l0)*(x/xf) display(sp.Eq(eta,y)) ``` ### cas linéaire on suppose que $\theta \approx 0$ trajectoire idéale du chariot et du cable : $$x_{ref}(t) \approx \xi(t) $$ $$l_{ref}(t) \approx - \eta(t)$$ d'ou l'asservissement de $F_c$ et $C_t$ $$ F_c = -K_c ( (x-x_{ref}) + T_c (\dot{x}-\dot{x}_{ref})) $$ $$ C_t = -g -K_t ( (l-l_{ref}) + T_t (\dot{l}-\dot{l}_{ref})) $$ ```python xref = x lref = -y display("trajectoire idéale:",xref, lref) ``` ```python # asservissement Kc, Tc = sp.symbols('K_c T_c') Fca = -Kc*((xc-xref)+(xcp-xref.diff(t,1))*Tc) Fca = Fca.simplify() display(Fca) Kt, Tt = sp.symbols('K_t T_t') Cta = -g -Kt*((l-lref)+(lp-lref.diff(t,1))*Tt) Cta = Cta.simplify() display(Cta) ``` ```python nbgrader={"grade": false, "grade_id": "cell-17be5fd098394fb6", "locked": false, "schema_version": 3, "solution": true, "task": false} # fct python FCa = 0 CTa = 0 Xref = 0 Lref = 0 ### BEGIN SOLUTION ### END SOLUTION ``` ### Equation lagrange avec couple ```python alpha = sp.Symbol('alpha') La = sp.simplify(La.subs({M:alpha*m})) display(La) Fc, Ct = sp.symbols('F_c C_t') LM = LagrangesMethod(La,[theta,xc,l],forcelist=[(C,Fc*alpha*m*RO.x),(Rt,Ct*m*rho*RO.z)], frame = RO) eq = sp.simplify(LM.form_lagranges_equations()) display(eq) ``` ```python # calcul du second membre smb = sp.simplify(sp.simplify(LM.rhs()).doit()) display(smb) smbF = sp.lambdify([theta,xc,l,thetap,xcp,lp,Fc,Ct,g,alpha],smb,'numpy') ``` ### Simulation de la trajectoire ```python hide_input=false nbgrader={"grade": false, "grade_id": "cell-67bf7eccaeb89f89", "locked": true, "schema_version": 3, "solution": false, "task": false} printmd("### parametres trajectoire: Xf={} L0={} Lf={} Lm={} alpha={}". format(_XF_,_L0_,_L1_,_L2_,_ALPHA_)) ``` ```python nbgrader={"grade": false, "grade_id": "cell-37226f680b99bc8b", "locked": false, "schema_version": 3, "solution": true, "task": false} # PARAMETRES NUMERIQUES TF = 10.0 #TF = 8.0 XF = 0 L0 = 0 LF = 0 ALPHA = 0 ### BEGIN SOLUTION ### END SOLUTION GG = 9.81 # KC = 50.0 TC = 20./KC KT = 50.0 TT = 20./KT Nt = 400 tt = np.linspace(0,TF,Nt) ``` ```python # trajectoire idéale linéarisée XREF = Xref(tt,XF,TF) LREF = Lref(tt,L0,LF,TF) plt.figure(figsize=(12,6)) plt.subplot(1,2,1) plt.plot(tt,XREF) plt.title("Position ") plt.subplot(1,2,2) plt.plot(tt,LREF) plt.title("Longueur") ``` ## cas non linéaire - ddl fonction de la trajectoire (idéale) $\xi$ $\eta$ $$ x_{ref} = \xi - \frac{\eta\ddot{\xi}}{\ddot{\eta}-g}$$ $$ l_{ref} = \sqrt{(\frac{\eta\ddot{\xi}}{\ddot{\eta}-g})^2 + \eta^2}$$ ```python xref = x - y*x.diff(t,2)/(y.diff(t,2)-g) xref = xref.doit() lref = sp.sqrt((y*x.diff(t,2)/(y.diff(t,2)-g))**2 + y**2) lref = lref.doit() print(xref, lref) ``` ```python # asservissement pour obtenir la trajectoire Kc, Tc = sp.symbols('K_c T_c') Fca = -Kc*((xc-xref)+(xcp-xref.diff(t,1))*Tc) #Fca = Fca.simplify() #display(Fca) print(Fca) Kt, Tt = sp.symbols('K_t T_t') Cta = -g - Kt*((l-lref)+(lp-lref.diff(t,1))*Tt) #Cta = Cta.simplify() #display(Cta) print(Cta) ``` ```python # fct numpy FCa = sp.lambdify([t,xc,xcp,xf,l0,lf,tf,Kc,Tc,g],Fca,'numpy') CTa = sp.lambdify([t,l,lp,xf,l0,lf,tf,Kt,Tt,g],Cta,'numpy') Xref= sp.lambdify([t,xf,l0,lf,tf,g],xref,'numpy') Lref= sp.lambdify([t,xf,l0,lf,tf,g],lref,'numpy') ``` ```python # trajectoire ideale XREF = Xref(tt,XF,L0,LF,TF,GG) LREF = Lref(tt,XF,L0,LF,TF,GG) plt.figure(figsize=(12,6)) plt.subplot(1,2,1) plt.plot(tt,XREF) plt.title("Position ") plt.subplot(1,2,2) plt.plot(tt,LREF) plt.title("Longueur") ``` ```python nbgrader={"grade": false, "grade_id": "cell-72888170a649c0d0", "locked": false, "schema_version": 3, "solution": true, "task": false} # fonction 2nd membre def F(Y,t): '''2nd mbre de l EDO dY/dt = F(Y,t) ''' global ALPHA,TF,XF ``` ```python # simulation Y0 = [0.0, 0.0, L0, 0., 0., 0. ] print(F(Y0,0.)) sol = odeint(F,Y0,tt) THETA = sol[:,0] XC = sol[:,1] LL = sol[:,2] ``` ```python plt.figure(figsize=(12,6)) plt.subplot(1,3,1) plt.plot(tt,THETA*180/np.pi) plt.ylim(-10,10) plt.title("Angle$\\theta$ en degré") plt.subplot(1,3,2) plt.plot(tt,XC) plt.plot(tt,XREF,'--') plt.title('Position $x_c(t)$') plt.subplot(1,3,3) plt.plot(tt,LL) plt.plot(tt,LREF,'--') plt.title('longueur $l(t)$') ``` ```python # tracer du mouvement plt.figure(figsize=(12,8)) for i in range(0,len(tt),20): X,Y = [XC[i],XC[i]+LL[i]*np.sin(THETA[i])],[0.,-LL[i]*np.cos(THETA[i])] plt.plot(X,Y,'-o',ms=20) plt.axis('equal') ``` ## Evitement d'un obstacle (optionnel) - au milieu de la trajectoire se trouve un obstacle et la charge doit avoir en ce point une hauteur $l_m$. - Refaire la planification de trajectoire dans ce cas: trajectoire ACB ![obstacle](images/obstacle.png) - sans obstacle la trajectoire droite: $$ y = l_0 + (l_f-l_0) x $$ - avec obstacle tq $y=y_a$: on xie par une parabole $$ y = l_0 + (l_f-l_0) x (a+bx+cx^2)$$ avec 3 conditions: - $y = l_f$ en x=1 - $y = l_a$ en x=1/2 - $\dot{y} = 0$ en x=1/2 pour $l_a = 2l_f - l_0$ on trouve: $$ y = l_0 + (l_f-l_0) x (9-12x+4x^2)$$ ```python ``` ## FIN du TP ```python ```