Planification en Mécanique

Contenu

3. Planification en Mécanique#

Marc BUFFAT dpt mécanique, Université Lyon 1 grue

%matplotlib inline
import numpy as np
import sympy as sp
import matplotlib.pyplot as plt
plt.rc('font', family='serif', size='14')
from IPython.display import display, Markdown, clear_output
def printmd(string):
    display(Markdown(string))
from metakernel import register_ipython_magics
register_ipython_magics()
from sympy.physics.vector import init_vprinting
init_vprinting()

3.1. Modélisation du bras d’une grue à vide (sans charge)#

grue

3.2. Calcul symbolique avec sympy (sp)#

variable symbolique: Symbol, symbols
fonction symbolique: Function
expression symbolique (variable)
affichage: display
transformation en fonction: lambda
dérivation: diff
évaluation numerique: evalf, lambdify
substitution: subs

attention avec Python on utilise des variables symboliques et numériques. Donc attention à ne pas mélanger

sp.cos() fonction cosinus symbolique
np.cos() fonction cosinus numérique

3.2.1. Documentation sympy et mechanics#

Sympy: https://docs.sympy.org/latest/index.html
Mechanics: https://docs.sympy.org/latest/modules/physics/mechanics/index.html

3.3. Problème: modèle de solide en rotation avec 1DDL#

bras en rotation dans le plan

masse sur un cercle

3.4. Modélisation du mouvement d’un solide#

paramètres et ddl

définition des parametres avec sp.symbols
définition des ddl avec dynamicsymbols

repère, point

repère avec ReferenceFrame
point Point
definition position des points et repères (rotation)

définition des solides

masse
matrice d’inertie inertia

cinématique / différent repère

solide indéformable: RigidBody
vitesse, qte de mouvement linear_momentum angular_momentum
composition des vitesses v1pt_theory v2pt_theory

formalisme lagrangien

Lagrangian
Equation de Lagrange

résolution numérique

génération automatique des équations
changement de variables
EDO du 1er ordre

3.4.1. paramétrage#

paramètres L, a, (longueur et diamètre du bras), M (masse) , g
1 ddl $θ$ et $\dot{θ}$

# parametres et ddl
from sympy.physics.mechanics import dynamicsymbols, Point, ReferenceFrame
from sympy.physics.mechanics import RigidBody, Lagrangian, LagrangesMethod
theta   = dynamicsymbols('theta')
thetap  = dynamicsymbols('theta',level=1)
L,a,M,g = sp.symbols('L a M g')
t       = sp.Symbol('t')

3.4.2. repères et position#

O = Point('O')
G = Point('G')
P = Point('P')
RO = ReferenceFrame('R_O')
RP = ReferenceFrame('R_P')
# definition position relative P,G et des repères RP,RO
### BEGIN SOLUTION
RP.orient(RO,'Axis',[theta, RO.z])
P.set_pos(O,L*RP.x)
G.set_pos(O,L/2*RP.x)
### END SOLUTION

3.4.3. définition du solide: masse et inertie#

from sympy.physics.mechanics import inertia
Ix = M*a**2/2
Iy = M*a**2/4 + M*L**2/12
# definition matrice inertie IG et du solide TigeOP
IG = 0
TigeOP = 0
### BEGIN SOLUTION
IG = inertia(RP,Ix,Iy,Iy)
TigeOP = RigidBody('Tige_OP',G,RP,M,(IG,G))
### END SOLUTION
display("IG:",IG)
display("repère lié au solide:",TigeOP.frame)

'IG:'

../../_images/7216bcec89eda75442352cd3b2138b7e97706ea27a5445beb59b5da9fbfc67e5.png

'repère lié au solide:'

R_P

3.4.4. cinématique (relation de transport)#

O.set_vel(RO,0.)
P.set_vel(RP,0.)
G.set_vel(RP,0.)
# calcul vitesse de P et G avec la relation de transport
### BEGIN SOLUTION
P.v2pt_theory(O,RO,RP)
G.v2pt_theory(O,RO,RP)
### END SOLUTION
display("VP,VG / RO:",P.vel(RO),G.vel(RO))
display("VP,VG / RP:",P.vel(RP),G.vel(RP))

'VP,VG / RO:'

../../_images/0dafdbee781479633ecf5f3343e28b54ddbbfb347a1813edba0556e7d18c9afc.png

../../_images/967bb75e228cdbc457b4b4a76163f9e8abea4133a5f700eccab369e7416235ba.png

'VP,VG / RP:'

../../_images/702ba12f4bf2cef7092842f0bdd4dc2b8bfdc07d13bc79cc481075cd1cc7c88c.png

3.4.5. formalisme lagragien (solide indéformable)#

display("mt cinetique, qte mvt, :",TigeOP.angular_momentum(O,RO),TigeOP.linear_momentum(RO))
display("energie cinetique:",TigeOP.kinetic_energy(RO))
# definition energie potentiel dans TigeOP.potential_energy
### BEGIN SOLUTION
TigeOP.potential_energy = M*g*G.pos_from(O).dot(RO.y)
### END SOLUTION
display("energie potentiel:",TigeOP.potential_energy)

'mt cinetique, qte mvt, :'

../../_images/dd27f4fa1b80ce2c1529adf754f68d50b6a4c2859cd1f5dd151a9ae5116ad85d.png

../../_images/a0bede80d90a4ae5a03569144b914630249f16f2d8ecf3ae2b77c48287599349.png

'energie cinetique:'

../../_images/2746e725caf898cd2dab9b1ef7f6e296978bbf39b4b2e0557ff82b9cf4391488.png

'energie potentiel:'

../../_images/dfdeaa4d2b20079d3d5a6dd0026ba89f7a9c8e01bee9c661616f5bde28932145.png

from sympy.physics.mechanics import Lagrangian,LagrangesMethod
# calcul du lagrangien dans La
La = 0
### BEGIN SOLUTION
La=Lagrangian(RO,TigeOP)
### END SOLUTION
display("Lagrangien:",La)

'Lagrangien:'

../../_images/595994034378610d3da8036e7272a7e0375e5dfb68e886361717e00b408e77e8.png

3.5. Equation de Lagrange#

Bilan des forces non conservatives (qui travaillent):

couple moteur : $C M$ en O (attention adimensionnalisé par M)
équation de Lagrange avec LagrangesMethod

C = sp.symbols('C')
# calcul de l'equation de lagrange dans Eq
Eq = 0
### BEGIN SOLUTION
LM = LagrangesMethod(La,[theta],forcelist=[(RP,C*M*RO.z)],frame=RO)
Eq = LM.form_lagranges_equations()[0]
Eq = Eq.simplify()
### END SOLUTION
display("equation de Lagrange:",sp.Eq(Eq,0))

'equation de Lagrange:'

../../_images/1adf9f5723c9bca277f872e5731792690bed26c7fd5d2de86e83041739191fce.png

3.5.1. Mise sous forme matricielle $A . \dot{Y} = B$ #

pour la résolution numérique: transformation en 2 EDO du 1er ordre: $ $\dot{Y} = F (Y) avec F = A^{- 1} B$ $

# calcul de A et B puis F (en simplifiant)
A = 0
B = 0
F = 0
### BEGIN SOLUTION
A=LM.mass_matrix_full
B=LM.forcing_full
display("A=",A,"B=",B)
F = A.inv()*B
F[1]=F[1].simplify()
### END SOLUTION
display('F(Y)=',F)

'A='

../../_images/f8974f2b588b0a4b04105bb8f21b97ac04e4d8860439b648f19ef29eec8fafd8.png

'B='

../../_images/0ccf0043959e6596a0a4194f1f6caa7c3695333179fdb8ed10c2b26a7c3e2ba4.png

'F(Y)='

../../_images/917336894ec7545ae4c68365c6a7266c21b445c470d0bde4c63889f4bdb1d241.png

3.6. Mouvement oscillant autour position équilibre#

\dot{Y} = F (Y)

3.6.1. position d’équilibre en $θ_{0}$ #

calcul du couple $C_{0}$ assurant l’équilibre
à partir de l’équation de Lagrange avec $\ddot{θ} = 0$
utilisation de sp.solve()

# calcul du couple C0 pour un position theta
C0 = 0
### BEGIN SOLUTION
Eq1 = Eq.subs({theta.diff(t,t):0})
display(Eq1)
C0 = sp.solve(Eq1,C)[0]
### END SOLUTION
display("C0=",C0)

../../_images/fcffafc9e8a951ee97c5f20d9a793c1c20d28ff70e3eace19195b7ed75b6b0d5.png

'C0='

../../_images/978cd0da6f19d8eeba7029c6ac23382da4c7081c2a02514bc99852418d649a1c.png

3.6.2. conversion calcul symbolique -> fonction python#

choix des valeurs des paramètres

# fonction FY  avec sp.lambdify
smFY = 0
# parametres
GG = 9.81
LL = 1.0
aa = 0.1
### BEGIN SOLUTION
valnum = [(g,GG), (L,LL), (a,aa)]
smFY = sp.lambdify((theta,thetap,C),F.subs(valnum),'numpy')
### END SOLUTION

# fonction F(Y)
def F(Y,t):
    '''second memebre EDO dY/dt = F(Y)'''
    ### BEGIN SOLUTION
    global CC0
    FF = smFY(Y[0],Y[1],CC0)
    return FF[:,0]
    ### END SOLUTION

# choix du couple C0 pour obtenir une position theta0
# position stable 
Theta0 = -np.pi/4
# position instable
Theta0 =  np.pi/4
# calcul du couple CC0
CC0 = 0
### BEGIN SOLUTION
CC0 = float(C0.subs(theta,Theta0).subs(valnum))
### END SOLUTION
print("Pour theta0={:.3f} C0={:.3f}".format(Theta0,CC0))

Pour theta0=0.785 C0=3.468

3.6.3. simulation numérique#

utilisation du solveur odeint

validation dans le cas theta(0)=theta0
cas avec perturbation

from scipy.integrate import odeint
# calcul solution THETA et THETAP
### BEGIN SOLUTION
# position d'équilibre
Y0 = np.array([Theta0,0.])
# cas perturbé
Y0 = np.array([Theta0*1.1,0.])
tt  = 1
N   = 200
T = np.linspace(0,tt,N)
sol = odeint(F,Y0,T)
THETA = np.mod(sol[:,0],2*np.pi)
THETAP= sol[:,1]
### END SOLUTION

3.6.4. tracé du résultat#

plt.figure(figsize=(10,8))
plt.subplot(2,1,1)
plt.title('trajectoire non controllée')
plt.plot(T,THETA,label="$\\theta(t)$")
plt.plot(T,Theta0*np.ones(THETA.size),'--',label="$\\theta_0$")
plt.legend()
plt.xlabel('t')
plt.subplot(2,1,2)
plt.plot(LL*np.cos(THETA),LL*np.sin(THETA),'-')
plt.plot([LL*np.cos(Theta0)],[LL*np.sin(Theta0)],'*',ms=12)
plt.plot([LL*np.cos(THETA[0])],[LL*np.sin(THETA[0])],'o',ms=12)
plt.plot([0,LL*np.cos(Theta0)],[0,LL*np.sin(Theta0)],'--')
plt.xlabel('$X_P$')
plt.ylabel('$Y_P$')
plt.axis('equal');

../../_images/472e553d869140854a2532952e0f3a02d4664431354cf7026dfd70a8ee1b5da1.png

3.6.5. analyse#

pour $θ_{0} > 0$ : position instable !!!
pour $θ_{0} < 0$ : position stable !!!

paramétrisation de l’équation en fonction de $ω$ $β$

$ω = \sqrt{\frac{g}{L}}$
$β^{2}$ coefficient de $\ddot{θ}$

β^{2} = \frac{a^{2}}{2 L^{2}} + \frac{2}{3}

display("Equation:",sp.Eq(2*Eq/(L**2*M),0))
omega, beta, theta0 = sp.symbols('omega beta theta_0')

'Equation:'

../../_images/931af32eed4f656e5d10a402bfd3948c14be1448b4d3b44843da480590bf1fff.png

rels = 0
eq1 = 0
### BEGIN SOLUTION
rels = [(g,omega**2*L),(a**2,(beta**2-2/3)*2*L**2)]
Eq1 = 2*Eq/(L**2*M)
Eq1 = Eq1.subs(rels).simplify()
### END SOLUTION
display(Eq1)

../../_images/63aaddd67eadc11c542422003c80fe9d6bc79544d4e409952039c2becb9547a1.png

# equation à l'équilibre -> theta0
Eq0 = 0
### BEGIN SOLUTION
Eq0=Eq1.subs({theta.diff(t,t,):0,theta:theta0})
display(Eq0)
### END SOLUTION
# equation fct de theta0
Eq0=Eq1-Eq0
display(sp.Eq(Eq0,0))

../../_images/5de44fadb1bb8ad7daeff53ce54238f8f62c6ae4c9890cc96074f895898a8ad3.png

../../_images/f2daf34bb60dd00ee139b0eb12b9b173e7c0680d8b261148ac0928153b37e664.png

### linéarisation autour de la position d’équilibre $θ_{0}$

θ = θ_{0} + θ^{'} avec θ^{'} ≪ 1

# linearisation autour de theta0, en remplacant theta par theta+theta0
Eq1 = 0
### BEGIN SOLUTION
Eq1 = Eq0.subs({theta:theta+theta0})
display(Eq1)
Eq1 = Eq1.subs({sp.cos(theta0+theta):sp.cos(theta0)-sp.sin(theta0)*theta})
Eq1 = Eq1.simplify()
### END SOLUTION
display("linéarisation ",Eq1)

../../_images/30fb6426d1c4e20fc4208efab515f312029256660d0c57636c2ebb2cd397953f.png

'linéarisation '

../../_images/3c90ae0811c27e716f0305efbf0221218004f2e11e38216d775ac18d915fa654.png

3.6.6. Analyse de solution linéarisée#

stabilité si $\sin (θ_{0}) < 0$
si on impose un couple $C_{0}$ correspondant à l’équilibre quasi-statique $ $C_{0} = \frac{L g \cos θ}{2} = \frac{ω^{2} L^{2} \cos θ}{2}$ $p o s s i b i l i t é d^{'} i n s t a b i l i t é :$ $\ddot{θ} \approx 0$ $
dans le cas instable nécessité d’un feedback (contrôle) p.e. $ $C = C_{0} - α (θ - θ_{0})$ $

3.7. Mouvement imposé du bras#

On étudie le positionnement du bras de $θ_{0} = 0$ à $θ_{1}$ pendant un temps $T_{0}$ en imposant la cinématique

3.7.1. cinématique imposée $Θ (t)$ #

conditions $ $Θ (0) = 0, \dot{Θ} (0) = 0, Θ (T) = θ_{1}, \dot{Θ} (T) = 0$ $
cinématique

Θ (t) = \frac{θ_{1} t^{2} (3 T - 2 t)}{T^{3}}

calcul du couple C

# cinematique imposée
t = sp.symbols('t')
theta1,T = sp.symbols('theta_1 T')
Theta = sp.lambdify(t,theta1*t**2*(3*T-2*t)/T**3,'numpy')
display("loi Theta(t):",Theta(t))
display("dérivee:",sp.diff(Theta(t),t).simplify())

'loi Theta(t):'

../../_images/c8c8f06940990c0f9d59db8a75195510669c5379e18190309598886593d5d966.png

'dérivee:'

../../_images/246c7b6c85e385273d746b63c38da3e91ab013b563970d6d3a28582ac405f44f.png

3.7.2. simulation : calcul du couple CC#

solution analytique en imposant $θ = Θ (t)$ imposé

from sympy.solvers import solve
# calcul du couple analytiquement en remplacant dans l'equation linearisée
display(Eq)
### BEGIN SOLUTION
Eq1=Eq.subs({theta:Theta(t),theta.diff(t,t):Theta(t).diff(t,t)})
display(Eq1)
CC=solve(Eq1,C)[0].simplify()
### END SOLUTION
display("C=",CC)

../../_images/f37b11a02b5b159587435bd4031a38947cc775123f5bfe93f0616716192f9c0b.png

../../_images/83e265f9c65666bb905e80f0e8cfa3e4bd34db18e43e95c4cef2e1859aed29d0.png

'C='

../../_images/a532630b05d943842ca690148144221144b0288313b5094039965f639f2148b9.png

# conversion en fonction numpy CClin
CClin = sp.lambdify((t,T,theta1),CC.subs(valnum),'numpy')

TT = 2.
Theta1 = np.pi/4
tt = np.linspace(0,TT,100)
# solution linearisée
cc = CClin(tt,TT,Theta1)
plt.plot(tt,cc)
plt.xlabel('t')
plt.title('Couple C en cinématique imposée');

../../_images/719a7e408f368c244205d3e3e51533eced826764f46fbeb5b8fafc84707b949b.png

3.7.3. simulation du pble#

résolution EDO avec C donnée par CClin

# fonction F(Y)
def F(Y,t):
    '''second membre de dY/dt = F(Y)  avec Y=[theta, thetap]'''
    global TT,Theta1
    ### BEGIN SOLUTION
    CC = CClin(t,TT,Theta1)
    FF =smFY(Y[0],Y[1],CC)
    return FF[:,0]
    ### END SOLUTION

# simulation 
Y0 = np.array([0.,0.])
# cas avec une perturbation initiale: pble !
#Y0 = np.array([0.1,0.])
#
N   = 200
tt = np.linspace(0,TT,N)
sol = odeint(F,Y0,tt)
THETA = np.mod(sol[:,0],2*np.pi)
THETAP= sol[:,1]
# solution imposée
THETA1=sp.lambdify([t],Theta(t).subs({T:TT,theta1:Theta1}),'numpy')

# tracer de la solution
plt.figure(figsize=(12,8))
plt.subplot(2,1,1)
plt.title('Solution cinématique imposée')
plt.plot(tt,THETA,label="$\\theta$")
plt.plot(tt,THETA1(tt),label="$\\Theta(t)$")
plt.plot(tt,THETAP,label="$\dot{\\theta}$")
plt.legend()
plt.xlabel('t')
plt.subplot(2,1,2)
plt.plot(LL*np.cos(THETA),LL*np.sin(THETA),'-',lw=2)
plt.xlabel('x')
plt.ylabel('y')
plt.axis('equal');

../../_images/98aba105390873bf52eedfa9594596a1c088111ae37010a9fad7749add7b9fe7.png

3.8. Modèle dynamique#

On souhaite introduire un contrôle dynamique de la commande

On veut définir la commande $C$ (i.e. le couple) dans l’équation du mouvement: $ $I \ddot{θ} = M C + F (θ, \dot{θ})$ $t e l q u e l e m o u v e m e n t d e$ \theta_0 $à$ \theta_1$ soit le plus régulier possible.

Pour cela on va calculer $C$ en fonction d’un contrôle $u (t)$ . On choisit le couple $C$ par une opération de découplage et feedback en fonction d’un contrôle $u$ t.q. $ $M C = I u - F (θ_{u}, \dot{θ_{u}})$ $c e q u i c o n d u i t à u n s y s t è m e d^{'} E D O l i n é a i r e s i n d é p e n d a n t e s ($ u $é t a n t f i x é)$ $I \ddot{θ_{u}} = I u soit \ddot{θ_{u}} = u$ $

démarche

on choisit la forme du contrôle $u (t)$ en fonction de $θ_{u}$ et $\dot{θ_{u}}$ ,
à partir de la solution $θ_{u}$ de EDO linéaire $\ddot{θ_{u}} = u$ on définit les parametres du contrôle tq $θ_{u} \approx θ_{o p t}$
on en déduit la commande $C$ t.q.
$ $M C = I u - F (θ, \dot{θ})$ $
connaissant $C$ on peut alors résoudre le problème sous contrôle:

I \ddot{θ} = M C + F (θ, \dot{θ})

3.8.1. définition de commande $u$ #

On veut positionner le bras avec un angle $θ_{1}$ (et arrivant avec une vitesse nulle).

On part d’une position $θ_{0} = 0$ avec une vitesse nulle.

On applique donc une commande $u$ proportionnelle à l’écart $θ - θ_{1}$ et à sa dérivée $\dot{θ}$ (feedback): $ $u = - k_{p} (θ - θ_{1}) - k_{v} \dot{θ}$ $L^{'} é q u a t i o n d u m o u v e m e n t s e r é d u i t à :$ $\ddot{θ} + k_{v} \dot{θ} + k_{p} (θ - θ_{1}) = 0$ $

On choisit les constantes $k_{p}$ et $k_{v}$ de façon à être dans le régime critique, i.e. avoir un retour à la position le plus rapidement possible sans oscillation. Le discrimant de l’équation caractéristique est alors nul $ $Δ = k_{v}^{2} - 4 k_{p} = 0 soit k_{p} = k_{v}^{2} / 4$ $

les conditions initiales sont la position $θ_{0} = 0$ avec une vitesse initiale nulle $ $θ (0) = 0 et \dot{θ} (0) = 0$ $L a v a l e u r d e$ k_v $f i x e l e t e m p s d e p a r c o u r s d e l a t r a j e c t o i r e$ T $θ (T) \approx θ_{1}$ $s o i t p o u r u n e p r é c i s i o n r e l a t i v e$ \epsilon $k_{v} \approx - \frac{2}{T} \log ϵ$ $

# calcul de la solution theta_u avec dsolve
theta_u = sp.Function('theta_u')
kv = sp.symbols('k_v',positive=True)
# ecrire l'équation sur theta_u et en déduire sa solution theta_s avec dsolve
eq = 0
theta_s = 0
### BEGIN SOLUTION
eq = sp.Eq(sp.Derivative(theta_u(t),t,t) + kv*sp.Derivative(theta_u(t),t) + kv*kv/4*(theta_u(t)-theta1) , 0 )
display("equation sur theta_u",eq)
solq=sp.dsolve(eq, theta_u(t), ics={theta_u(0):0, theta_u(t).diff(t).subs(t,0):0})
display("solution",solq)
theta_s=sp.Lambda(t,solq.rhs)
display("theta_s(t)",theta_s(t))
### END SOLUTION

'equation sur theta_u'

../../_images/b79984b6ea4bb6e2aa5c1023239210f1b7f6f473f766bca5ef552fbb102c383d.png

'solution'

../../_images/8281d6e0aa9acc786dafd84c91d0b4f7076adc31ed603944c6d631802514b694.png

'theta_s(t)'

../../_images/b8255027388f9a821569bdf0f2b63360adbc169afc8bbe8f403f3b748fe13b1c.png

from sympy.plotting import plot
# parametres
Eps = 1.e-3
Kv  = float((-2/TT)*np.log(Eps))
# vérification et tracer de la solution 
cdtsu = [(theta1,Theta1),(kv,Kv),(T,TT)]
### BEGIN SOLUTION
display("theta_s(T):",theta_s(T))
print("theta_s(T)={:.3f} Theta1={:.3f}".format(theta_s(T).subs(cdtsu),Theta1))
plot(theta_s(t).subs(cdtsu),(t,0,TT), ylabel='$\\theta_s(t)$',title='solution critique');
### END SOLUTION

'theta_s(T):'

../../_images/a405356ee4c473f30bf998be3d3295973cd21043cb0612bfd407eed01351f947.png

theta_s(T)=0.779 Theta1=0.785

../../_images/d4f0c5db220287514c55ca2a26180265e8d93505579bbb85e886888848557fa6.png

3.8.2. calcul du contrôle pour la trajectoire choisie#

$θ$ doit varier de $θ_{0}$ à $θ_{1}$
on impose la solution calculée $θ_{s}$ et sa dérivée et on en déduit le contrôle

# solution imposée theta_s
display("theta_s(t):",theta_s(t))
display("dtheta_s  :",theta_s(t).diff(t))
# conversion en fonction python
Theta_s  = 0
dTheta_s = 0
### BEGIN SOLUTION
Theta_s  = sp.lambdify(t,theta_s(t).subs(cdtsu))
dTheta_s = sp.lambdify(t,theta_s(t).diff(t).subs(cdtsu))
### END SOLUTION

'theta_s(t):'

'dtheta_s  :'

../../_images/5639838110a8bb74c38cc2bef1c0da85964f45f2b348a67e42ec0cc2e08ee2cc.png

tt = np.linspace(0,TT,100)
fig,(ax1,ax2) = plt.subplots(1,2,figsize=(12,6))
ax1.plot(tt,Theta_s(tt),label="$\\theta_s$")
ax1.set_title("loi horaire")
ax1.set_xlabel('t')
ax1.legend()
ax2.plot(tt,dTheta_s(tt),label="$\\dot{\\theta_s}$")
ax2.set_title("vitesse angulaire")
ax2.set_xlabel('t')
ax2.legend();

../../_images/554b9cbcb05e912109590189f87856ad8e4f54b0c8f028ccad696d6482ee264d.png

3.8.2.1. contrôle $u_{s} (t)$ #

feedback $ $u_{s} = - k_{p} (θ (t) - θ_{1} (T)) - k_{v} \dot{θ} (t)$ $

avec $k_{p} = k v^{2} / 4$

# calcul de u_s
u_s = 0
### BEGIN SOLUTION
u_s = -kv*sp.diff(theta,t) - kv*kv/4*(theta-theta1)
### END SOLUTION
display("u_s(t):",u_s)

'u_s(t):'

../../_images/f40a6c523e7b34435abd9e969063019ad689a1e2a438c3bce4bcd7973232f302.png

3.8.2.2. couple $C$ #

connaissant $θ_{u}$ et $u$ , on calcul $C$ t.q.: $ $M C = I u - F (θ, \dot{θ})$ $

CC = (L**2/3+a**2/4)*u_s + L*g*sp.cos(theta)/2
sp.Eq(C,CC)

../../_images/5efb1aaf4fbc79a0a8e10471fd8515769734379177aa92bd31a10cfdd01be210.png

# conversion en fonction Python Cs
Cs = sp.lambdify([theta,thetap],CC.subs(cdtsu).subs(valnum),'numpy')

3.8.3. simulation du système controlé#

# fonction F(Y)
def F(Y,t):
    '''second membre de dY/dt = F(Y)  avec Y=[theta, thetap]'''
    ### BEGIN SOLUTION
    CC = Cs(Y[0],Y[1])
    FF =smFY(Y[0],Y[1],CC)
    return FF[:,0]
    ### END SOLUTION

# simulation système controle
Y0 = np.array([0.,0.])
# cas avec une perturbation
Y0 = np.array([-0.2,0.])
#
N  = 200
tt = np.linspace(0,TT*1.1,N)
sol = odeint(F,Y0,tt)
THETA = sol[:,0]
THETAP= sol[:,1]

plt.figure(figsize=(10,8))
plt.subplot(2,1,1)
plt.title("Système controllé")
plt.plot(tt,THETA,label="$\\theta$")
plt.plot(tt,THETAP,label="$\dot{\\theta}$")
plt.legend()
plt.xlabel('t')
plt.subplot(2,1,2)
plt.plot(LL*np.cos(THETA),LL*np.sin(THETA),'-',lw=2)
plt.xlabel('x')
plt.ylabel('y')
plt.axis('equal');

../../_images/cf32cd541f59b7e2e429471837e8f75fff7f6e3c27785a4158a45942a26cb0b4.png