1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
| import numpy as np
#import matplotlib.pyplot as plt
#import matplotlib.animation as animation
from assimulo.problem import Implicit_Problem
from assimulo.solvers import IDA
# Parametres
l=1.0 # longueur du pendule
g=9.81 # constante de pesanteur
m=1.0 # petite masse
n=6089. # 4 dernies chiffres etudiant
M=(n/5000.)*m # masse suspendue
omega=3.3 # pulsation
K=0.5 # coefficient de frottement
# Penalisation
beta1=1.
beta2=100.
# Problème index 2
def res (t,Y,dY):
"""
q=(x,y,theta,xpoint,ypoint,thetapoint,lambda)
0 1 2 3 4 5 6
res=q/dt - F(q,t)
"""
res=np.zeros(7)
# Simplification
costh = np.cos(Y[2]) # cos(theta)
sinth = np.sin(Y[2]) # sin(theta)
cosw = np.cos(omega*Y[0]) # cos(omega*x)
sinw = np.sin(omega*Y[0]) # sin(omega*x)
# Calculs des résidus
res[0] = dY[0]-Y[3]
res[1] = dY[1]-Y[4]
res[2] = dY[2]-Y[5]
du = -(M*l*(costh*dY[5] - sinth*Y[5]**2) - Y[6]*(omega/3.0*sinw - 2*Y[0]) + K*Y[3]) / (m+M)
res[3]=(m+M)*(dY[3] - du) # Lagrange selon x
dv = -(M*l*(sinth*dY[5] + costh*Y[5]**2) - Y[6] + (m+M)*g + K*Y[4]) / (m+M)
res[4]=(m+M)*(dY[4] - dv) # Lagrange selon y
res[5]= M*l*(costh*dY[3] + sinth*dY[4]) + M*l*l*dY[5] + M*g*l*sinth # Lagrange selon theta
# Definition de la contrainte
G = Y[1] - Y[0]**2-1./3.*cosw
dG = Y[4] + (-2*Y[0] + omega/3.*sinw)*Y[3]
d2G = dv + (-2*Y[0] + omega/3.*sinw)*du + (-2. + omega**2/3.*cosw)*Y[3]**2
res[6] = d2G + beta1*dG + beta2*G
return np.array([res])
# Conditions initiales
t0=0.0
x0=1./3.
y0=x0**2 + np.cos(omega*x0)/3.
theta0=0.0
lambdaval=0.0
Y0=np.array([x0,y0,theta0,0.0,0.0,0.0,lambdaval])
dY0=np.array([0.,0.,0.,0.,-g,0.,0.])
print "CI Y0=",Y0,"\n dY0=",dY0,"\n res=",res(t0,Y0,dY0)
# Creation probleme implicite Assimulo
imp_mod=Implicit_Problem(res,Y0,dY0,t0)
# Creation solver implicite Assimulo
imp_sim= IDA(imp_mod)
#sim+Radau5DAE(mod)
imp_sim.atol=1.e-5
imp_sim.rtol=1.e-5
imp_sim.algvar=[True,True,True,True,True,True,False]
# rend les conditions intiales compatibles
imp_sim.make_consistent('IDA_YA_YDP_INIT')
imp_sim.report_continuously=True
#integration
tfinal=30*np.pi/np.sqrt(g/l)
N=500
t,Y,dY=imp_sim.simulate(tfinal,N) |
