Ticket #3329: FlexBeamPackage.mo

File FlexBeamPackage.mo, 16.4 KB (added by Bruno Scaglioni, 11 years ago)

Modelica code

Line 
1within ;
2package FlexBeamPackage "FEM exported package"
3
4 model FlexBeam
5 import SI = Modelica.SIunits;
6 import Cv = Modelica.SIunits.Conversions;
7 import Modelica.Math.*;
8 parameter Integer N = 2 "Number of elements";
9 parameter Integer M = 3 * N;
10 parameter Real q_start[M] = zeros(M) "Initialization||Initial deformation";
11 parameter Real dq_start[M] = zeros(M)
12 "Initialization||Initial velocity of deformation";
13 parameter SI.Length L "Beam Length";
14 parameter SI.Density rho "Material Volume Density";
15 parameter SI.Area A "Cross sectional area";
16 parameter Real d "Damping coefficient";
17 parameter SI.ModulusOfElasticity E "Material Youngs modulus";
18 parameter SI.SecondMomentOfArea J "Cross sectional inertia";
19 parameter Boolean ClampedFree = true
20 "if true Clamped-Free else simply supported model";
21 parameter Boolean CircularSection = true
22 "if true the beam has circular section";
23 parameter Modelica.Mechanics.MultiBody.Types.Color ColorBeam = {128, 0, 0}
24 "|3D Graphics|| Beam color ";
25 Modelica.Mechanics.MultiBody.Interfaces.Frame_a FrameA annotation(Placement(transformation(extent = {{-116, -12}, {-84, 20}}, rotation = 0)));
26 Modelica.Mechanics.MultiBody.Interfaces.Frame_b FrameB annotation(Placement(transformation(extent = {{84, -12}, {116, 20}}, rotation = 0)));
27 Modelica.Mechanics.MultiBody.Frames.Orientation R_rel;
28 Real q[M](start = q_start,fixed=true) "Elastic coordinates";
29 Real dq[M](start = dq_start,fixed=true) "Elastic velocities";
30 Real ddq[M] "Elastic accelerations";
31 Real h_w_r[3, 1];
32 Real h_w_theta[3, 1];
33 Real h_w_f[M, 1];
34 Real h_e_r[3, 1];
35 Real h_e_theta[3, 1];
36 Real h_e_f[M, 1];
37 Real ra_w[3];
38 Real r_cm_w[3];
39 Real va_a[3];
40 Real v_0a[3];
41 Real aa_a[3];
42 Real wa_a[3];
43 Real za_a[3];
44 Real fa[3];
45 Real taua[3];
46 Real rb_w[3];
47 Real fb[3];
48 Real taub[3];
49 Real fb_a[3];
50 Real taub_a[3];
51 //terms for energy calculations
52 Real MassMat[M + 6, M + 6];
53 Real coords[M + 6];
54 Real KinEnergy;
55 Real PotEnergy;
56 Real TotalEnergy;
57 parameter Real m = rho * A * L;
58 parameter Real r0b[3] = {L, 0, 0};
59 parameter Real Cm[3] = {L / 2, 0, 0};
60 final parameter Real h = L / N;
61 final parameter Real KffEl[6, 6] = E * [A / h, 0, 0, -A / h, 0, 0; 0, 12 * J / h ^ 3, 6 * J / h ^ 2, 0, -12 * J / h ^ 3, 6 * J / h ^ 2; 0, 6 * J / h ^ 2, 4 * J / h, 0, -6 * J / h ^ 2, 2 * J / h; -A / h, 0, 0, A / h, 0, 0; 0, -12 * J / h ^ 3, -6 * J / h ^ 2, 0, 12 * J / h ^ 3, -6 * J / h ^ 2; 0, 6 * J / h ^ 2, 2 * J / h, 0, -6 * J / h ^ 2, 4 * J / h];
62 final parameter Real SbarEl[3, 6] = m / N / 12 * [6, 0, 0, 6, 0, 0; 0, 6, h, 0, 6, -h; 0, 0, 0, 0, 0, 0];
63 final parameter Real Sbar11[6, 6] = m / N * [1 / 3, 0, 0, 1 / 6, 0, 0; 0, 0, 0, 0, 0, 0; 0, 0, 0, 0, 0, 0; 1 / 6, 0, 0, 1 / 3, 0, 0; 0, 0, 0, 0, 0, 0; 0, 0, 0, 0, 0, 0];
64 final parameter Real Sbar12[6, 6] = transpose(Sbar21);
65 final parameter Real Sbar13[6, 6] = zeros(6, 6);
66 final parameter Real Sbar21[6, 6] = m / N * [0, 0, 0, 0, 0, 0; 7 / 20, 0, 0, 3 / 20, 0, 0; 1 / 20 * h, 0, 0, 1 / 30 * h, 0, 0; 0, 0, 0, 0, 0, 0; 3 / 20, 0, 0, 7 / 20, 0, 0; -1 / 30 * h, 0, 0, -1 / 20 * h, 0, 0];
67 final parameter Real Sbar22[6, 6] = m / N * [0, 0, 0, 0, 0, 0; 0, 13 / 35, 11 / 210 * h, 0, 9 / 70, -13 / 420 * h; 0, 11 / 210 * h, 1 / 105 * h ^ 2, 0, 13 / 420 * h, -1 / 140 * h ^ 2; 0, 0, 0, 0, 0, 0; 0, 9 / 70, 13 / 420 * h, 0, 13 / 35, -11 / 210 * h; 0, -13 / 420 * h, -1 / 140 * h ^ 2, 0, -11 / 210 * h, 1 / 105 * h ^ 2];
68 final parameter Real Sbar23[6, 6] = zeros(6, 6);
69 final parameter Real Sbar31[6, 6] = zeros(6, 6);
70 final parameter Real Sbar32[6, 6] = zeros(6, 6);
71 final parameter Real Sbar33[6, 6] = zeros(6, 6);
72 final parameter Real Ibar11[1, 6] = h * m / N * [1 / 6, 0, 0, 1 / 3, 0, 0]
73 annotation(Evaluate=true);
74 final parameter Real Ibar11adj[1, 6] = h * m / N * [1 / 2, 0, 0, 1 / 2, 0, 0] annotation(Evaluate=true);
75 final parameter Real Ibar12[1, 6] = h * m / N * [0, 3 / 20, 1 / 30 * h, 0, 7 / 20, -1 / 20 * h] annotation(Evaluate=true);
76 final parameter Real Ibar12adj[1, 6] = h * m / N * [0, 1 / 2, 1 / 12 * h, 0, 1 / 2, -1 / 12 * h] annotation(Evaluate=true);
77 final parameter Real Ibar13[1, 6] = zeros(1, 6);
78 final parameter Real Ibar21[1, 6] = zeros(1, 6);
79 final parameter Real Ibar22[1, 6] = zeros(1, 6);
80 final parameter Real Ibar23[1, 6] = zeros(1, 6);
81 final parameter Real Ibar31[1, 6] = zeros(1, 6);
82 final parameter Real Ibar32[1, 6] = zeros(1, 6);
83 final parameter Real Ibar33[1, 6] = zeros(1, 6);
84 final parameter Real S0[3, 6] = [1, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0; 0, 0, 0, 0, 0, 0];
85 final parameter Real dS0[3, 6] = [0, 0, 0, 0, 0, 0; 0, 0, 0, 0, 0, 0; 0, 0, 1, 0, 0, 0];
86 final parameter Real S1[3, 6] = [0, 0, 0, 1, 0, 0; 0, 0, 0, 0, 1, 0; 0, 0, 0, 0, 0, 0];
87 final parameter Real S2[3, 6] = [0, 0, 0, 0, 0, 0; 0, 0, 0, 0, 1, 0; 0, 0, 0, 0, 0, 0];
88 final parameter Real dS1[3, 6] = [0, 0, 0, 0, 0, 0; 0, 0, 0, 0, 0, 0; 0, 0, 0, 0, 0, 1];
89 SI.Acceleration g_0[3] "Gravity acc. vector resolved in world frame";
90 //matrici di connettivita
91 Real B[N, 6, M];
92 //matrice di forma del connettore B, � variabile perch� dipende dalle condizioni al contorno dell'asta
93 Real Sb[3, M];
94 Real SbCap[3, M];
95 Real Ke[M, M];
96 Real Me[M, M];
97 Real Ct[M, 3];
98 Real Sbar[3, M];
99 Real Stbar[3];
100 Real mDcbar[3];
101 Real mDcbartilde[3, 3];
102 Real Ithf_bar[3, M];
103 Real Cr[M, 3];
104 Real Ithth_bar[3, 3];
105 Real Ithth_bar11;
106 Real Ithth_bar22;
107 Real Ithth_bar33;
108 Real Ithth_bar12;
109 Real Jbar[3, 3];
110 //per il calcolo di h_w_f
111 Real F[6, 6];
112 Real H[6, 6];
113 Real wCross[3, 3];
114 Real wCross2[3, 3];
115 protected
116 outer Modelica.Mechanics.MultiBody.World world;
117 /* 3D graphics variables */
118 type vec3D = Real[3];
119 vec3D r0shape[N];
120 vec3D rrelshape[N];
121 Real Lshape[N];
122 parameter Real pi = Modelica.Constants.pi;
123 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape Segment[N](r_shape = r0shape, length = Lshape, each width = if CircularSection then 2 * sqrt(A / pi) else A / (0.4 * sqrt(A)), each height = if CircularSection then 2 * sqrt(A / pi) else 0.4 * sqrt(A), lengthDirection = rrelshape, each widthDirection = {0, 0, 1}, each shapeType = if CircularSection then "cylinder" else "box", each color = ColorBeam, each extra = 0.0, each r = FrameA.r_0, each R = FrameA.R);
124 equation
125 Connections.branch(FrameA.R, FrameB.R);
126 assert(cardinality(FrameA) > 0 or cardinality(FrameB) > 0, "Frames both disconnected, object floating!");
127 //connectivity matrices calculus, in beam model connectivity matrixes determinate boundary conditions
128 if ClampedFree then
129 B[1, :, :] = [zeros(3, 3 * N); identity(3), zeros(3, 3 * (N - 1))];
130 B[N, :, :] = [zeros(6, 3 * (N - 2)), identity(6)];
131 else
132 B[1, :, :] = [zeros(2, 3 * N); [zeros(1, 3 * (N - 1)), [0, 1, 0]]; identity(3), zeros(3, 3 * (N - 1))];
133 B[N, :, :] = [zeros(3, 3 * (N - 2)), identity(3), zeros(3, 3); zeros(3, 3 * (N - 2)), zeros(3, 3), [1, 0, 0; 0, 0, 0; 0, 0, 1]];
134 end if;
135 for i in 2:N - 1 loop
136 B[i, :, :] = [zeros(6, 3 * (i - 2)), identity(6), zeros(6, 3 * (N - i))];
137 end for;
138 /* calcolo dei componenti della matrice di inerzia */
139 /*-----------------------*/
140 //Sbar in modello beam equivale a Ct trasposto in FEM
141 Sbar = sum(SbarEl * B[i, :, :] for i in 1:N);
142 Ct = transpose(Sbar);
143 //Stbar equivale a mDcbar
144 Stbar = m * Cm + Sbar * q;
145 mDcbar = Stbar;
146 mDcbartilde = skew(mDcbar);
147 //Ithf_bar equivale a Cr trasposto
148 Ithf_bar = [zeros(2, 3 * N); sum((Ibar12 + (i - 1) * Ibar12adj + transpose(matrix(q)) * transpose(B[i, :, :]) * (Sbar12 - Sbar21)) * B[i, :, :] for i in 1:N)];
149 Cr = transpose(Ithf_bar);
150 //Ithth equivale a Jbar
151 //ATTENZIONE per calcolare Ithth_bar � necessario calcolare molte quantit�
152 Ithth_bar11 = scalar(transpose(matrix(q)) * sum(transpose(B[i, :, :]) * Sbar22 * B[i, :, :] for i in 1:N) * q);
153 Ithth_bar22 = m * L ^ 2 / 3 + scalar(2 * sum((Ibar11 + (i - 1) * Ibar11adj) * B[i, :, :] for i in 1:N) * q + transpose(matrix(q)) * sum(transpose(B[i, :, :]) * Sbar11 * B[i, :, :] for i in 1:N) * q);
154 Ithth_bar33 = Ithth_bar11 + Ithth_bar22;
155 Ithth_bar12 = scalar((-sum((Ibar12 + (i - 1) * Ibar12adj) * B[i, :, :] for i in 1:N) * q) - transpose(matrix(q)) * sum(transpose(B[i, :, :]) * Sbar21 * B[i, :, :] for i in 1:N) * q);
156 Ithth_bar = [Ithth_bar11, Ithth_bar12, 0; Ithth_bar12, Ithth_bar22, 0; 0, 0, Ithth_bar33];
157 Jbar = Ithth_bar;
158 //credo di poterlo calcolare come der(Jbar)
159 //mff equivale a Me
160 Me = sum(transpose(B[i, :, :]) * (Sbar11 + Sbar22) * B[i, :, :] for i in 1:N);
161 //kff equivale a Ke
162 Ke = sum(transpose(B[i, :, :]) * KffEl * B[i, :, :] for i in 1:N);
163 r_cm_w = FrameA.r_0 + Modelica.Mechanics.MultiBody.Frames.resolve1(FrameA.R, Cm);
164 g_0 = Modelica.Mechanics.MultiBody.Frames.resolve2(FrameA.R, world.gravityAcceleration(r_cm_w));
165 ra_w = FrameA.r_0;
166 va_a = Modelica.Mechanics.MultiBody.Frames.resolve2(FrameA.R, der(FrameA.r_0));
167 v_0a = der(ra_w);
168 aa_a = Modelica.Mechanics.MultiBody.Frames.resolve2(FrameA.R, der(v_0a));
169 wa_a = Modelica.Mechanics.MultiBody.Frames.angularVelocity2(FrameA.R);
170 za_a = der(wa_a);
171 fa = FrameA.f;
172 taua = FrameA.t;
173 rb_w = FrameB.r_0;
174 fb = FrameB.f;
175 taub = FrameB.t;
176 //Relation between A and B DYNAMIC positions, must write equations for translation AND rotation
177 //translation
178 rb_w = ra_w + Modelica.Mechanics.MultiBody.Frames.resolve1(FrameA.R, r0b + Sb * q);
179 //Devo definire l'orientamento di B rispetto al world frame
180 R_rel = Modelica.Mechanics.MultiBody.Frames.planarRotation({0, 0, 1}, q[M], dq[M]);
181 FrameB.R = Modelica.Mechanics.MultiBody.Frames.absoluteRotation(FrameA.R, R_rel);
182 //forces and torques on B expressed in A
183 fb_a = Modelica.Mechanics.MultiBody.Frames.resolve1(R_rel, fb);
184 taub_a = Modelica.Mechanics.MultiBody.Frames.resolve1(R_rel, taub);
185 dq = der(q);
186 ddq = der(dq);
187 //coriolis terms calculus
188 h_w_r = matrix((-cross(wa_a, cross(wa_a, mDcbar))) - 2 * cross(wa_a, transpose(Ct) * dq));
189 h_w_theta = matrix((-cross(wa_a, Jbar * wa_a)) - der(Jbar) * wa_a - cross(wa_a, transpose(Cr) * dq));
190 wCross = skew(wa_a);
191 wCross2 = wCross * wCross;
192 F = Sbar11 * wCross2[1, 1] + Sbar12 * wCross2[2, 1] + Sbar21 * wCross2[1, 2] + Sbar22 * wCross2[2, 2];
193 H = Sbar11 * wCross[1, 1] + Sbar12 * wCross[2, 1] + Sbar21 * wCross[1, 2] + Sbar22 * wCross[2, 2];
194 h_w_f = -sum(transpose(B[i, :, :]) * transpose([(Ibar11 + (i - 1) * Ibar11adj) * wCross2[1, 1] + (Ibar12 + (i - 1) * Ibar12adj) * wCross2[2, 1]]) + matrix(transpose(B[i, :, :]) * F * B[i, :, :] * q) + matrix(2 * (transpose(B[i, :, :]) * H * B[i, :, :]) * dq) for i in 1:N);
195 h_e_r = matrix(fa + fb_a);
196 h_e_theta = matrix(taua + taub_a + cross(r0b + Sb * q, fb_a));
197 transpose(h_e_f) = transpose(matrix(fb_a)) * S1 * B[N, :, :] + transpose(matrix(taub_a)) * dS1 * B[N, :, :];
198 Sb = S1 * B[N, :, :];
199 SbCap = dS1 * B[N, :, :];
200 /*---Dynamic equations---*/
201 //rigid traslation
202 [m * identity(3), transpose(mDcbartilde), transpose(Ct)] * [aa_a - g_0; za_a; ddq] = h_w_r + h_e_r;
203 //rigid rotation
204 [mDcbartilde, Jbar, transpose(Cr)] * [aa_a - g_0; za_a; ddq] = h_w_theta + h_e_theta;
205 //flexible modal dynamic equations
206 [Ct, Cr, Me] * [aa_a - g_0; za_a; ddq] = h_w_f + h_e_f - matrix(Ke * q) - matrix(d / 100 * (Me + 0.5 * Ke) * dq);
207 //Conservation of Energy
208 MassMat = [m * identity(3), transpose(mDcbartilde), transpose(Ct); mDcbartilde, Jbar, transpose(Cr); Ct, Cr, Me];
209 coords = vector([va_a; wa_a; dq]);
210 KinEnergy = scalar(0.5 * transpose(matrix(coords)) * MassMat * matrix(coords));
211 PotEnergy = world.g * m * r_cm_w[2] + 0.5 * scalar(transpose(matrix(q)) * Ke * matrix(q));
212 TotalEnergy = KinEnergy + PotEnergy;
213 /* 3D Visual Representation */
214 r0shape[1, :] = {0, 0, 0};
215 rrelshape[1, :] = r0shape[2, :];
216 Lshape[1] = sqrt(rrelshape[1, :] * rrelshape[1, :]);
217 r0shape[N, :] = {L * (N - 1) / N, 0, 0} + S1 * B[N - 1, :, :] * q;
218 rrelshape[N, :] = {L, 0, 0} + S1 * B[N, :, :] * q - r0shape[N, :];
219 Lshape[N] = sqrt(rrelshape[N, :] * rrelshape[N, :]);
220 for i in 2:N - 1 loop
221 r0shape[i, :] = {L * (i - 1) / N, 0, 0} + S1 * B[i - 1, :, :] * q;
222 rrelshape[i, :] = r0shape[i + 1, :] - r0shape[i, :];
223 Lshape[i] = sqrt(rrelshape[i, :] * rrelshape[i, :]);
224 end for;
225 annotation(Diagram(graphics), Icon(graphics={ Text(extent = {{-118, 102}, {114, 38}}, lineColor = {0, 0, 255}, fillColor = {0, 0, 255},
226 fillPattern = FillPattern.Solid, textString = "%name"), Rectangle(extent = {{-98, 46}, {96, -40}}, lineColor = {0, 0, 255}, fillColor = {0, 0, 255},
227 fillPattern = FillPattern.Solid)}), DymolaStoredErrors);
228 end FlexBeam;
229
230
231
232
233 package ExamplesBeam
234 model FreeVibrationBeam
235
236 FlexBeamPackage.FlexBeam fEMBeam(
237 CircularSection=true,
238 L=2,
239 N=2,
240 rho=2767,
241 A=0.01*0.01*3.1415,
242 E=7.2e10,
243 d=1,
244 J=Modelica.Constants.pi*0.01^4/4) annotation (Placement(transformation(
245 extent={{-8,-18},{12,2}}, rotation=0)));
246 inner Modelica.Mechanics.MultiBody.World world annotation(Placement(transformation(extent = {{-74, -18}, {-54, 2}}, rotation = 0)));
247 Modelica.Mechanics.MultiBody.Parts.Body body(sphereDiameter = 0.1, sphereColor = {255, 0, 0}, m = 0,
248 r_CM={0,0,0}) annotation(Placement(transformation(extent = {{50, -18}, {70, 2}}, rotation = 0)));
249 equation
250 connect(world.frame_b, fEMBeam.FrameA) annotation(Line(points = {{-54, -8}, {-28, -8}, {-28, -7.6}, {-8, -7.6}}, color = {95, 95, 95}, thickness = 0.5));
251 connect(fEMBeam.FrameB, body.frame_a) annotation(Line(points = {{12, -7.6}, {32, -7.6}, {32, -8}, {50, -8}}, color = {95, 95, 95}, thickness = 0.5));
252 annotation(Diagram(graphics));
253 end FreeVibrationBeam;
254
255 model PendulumBeam
256 parameter Real b_length=0.4;
257 parameter Integer N=2;
258 parameter Integer M=3*10;
259 inner Modelica.Mechanics.MultiBody.World world annotation(Placement(transformation(extent = {{-92, 8}, {-72, 28}}, rotation = 0)));
260 FlexBeamPackage.FlexBeam fEMBeam(
261 CircularSection=true,
262 N=10,
263 L=b_length,
264 A=0.0239*0.0239*Modelica.Constants.pi,
265 d=0,
266 E=4.7124e7,
267 J=Modelica.Constants.pi*0.0239^4/4,
268 rho=5.54e3,
269 q_start=zeros(M),
270 dq_start=zeros(M)) annotation (Placement(transformation(extent={{6,4},{
271 32,30}}, rotation=0)));
272 Modelica.Mechanics.MultiBody.Joints.Revolute revolute(phi(fixed=true, start=0),
273 w(fixed=true, start=0)) annotation(Placement(transformation(extent = {{-42, 10}, {-22, 30}})));
274 Modelica.Mechanics.MultiBody.Parts.Body body(m = 0, I_11 = 0, I_22 = 0, I_33 = 0,
275 r_CM={b_length,0,0}) annotation(Placement(transformation(extent = {{66, 8}, {86, 28}})));
276 equation
277 connect(world.frame_b, revolute.frame_a) annotation(Line(points = {{-72, 18}, {-58, 18}, {-58, 20}, {-42, 20}}, color = {95, 95, 95}, thickness = 0.5, smooth = Smooth.None));
278 connect(revolute.frame_b, fEMBeam.FrameA) annotation(Line(points = {{-22, 20}, {-8, 20}, {-8, 17.52}, {6, 17.52}}, color = {95, 95, 95}, thickness = 0.5, smooth = Smooth.None));
279 connect(fEMBeam.FrameB, body.frame_a) annotation(Line(points = {{32, 17.52}, {50, 17.52}, {50, 18}, {66, 18}}, color = {95, 95, 95}, thickness = 0.5, smooth = Smooth.None));
280 annotation(Diagram(coordinateSystem(preserveAspectRatio=false, extent={{-100,-100},
281 {100,100}}),
282 graphics));
283 end PendulumBeam;
284
285
286 end ExamplesBeam;
287
288 annotation(uses(Modelica(version = "3.2.1")));
289end FlexBeamPackage;