Ticket #2742: CheckContactCalculation.mo

File CheckContactCalculation.mo, 31.5 KB (added by Martin Sjölund, 11 years ago)
Line 
1package ModelicaServices
2 extends Modelica.Icons.Package;
3
4 package Animation
5 extends Modelica.Icons.Package;
6
7 model Shape
8 extends Modelica.Utilities.Internal.PartialModelicaServices.Animation.PartialShape;
9 end Shape;
10
11 model Surface
12 extends Modelica.Utilities.Internal.PartialModelicaServices.Animation.PartialSurface;
13 end Surface;
14 end Animation;
15
16 package Machine
17 extends Modelica.Icons.Package;
18 final constant Real eps = 1.e-15;
19 final constant Real small = 1.e-60;
20 final constant Real inf = 1.e+60;
21 final constant Integer Integer_inf = OpenModelica.Internal.Architecture.integerMax();
22 end Machine;
23end ModelicaServices;
24
25package Modelica
26 extends Modelica.Icons.Package;
27
28 package Mechanics
29 extends Modelica.Icons.Package;
30
31 package MultiBody
32 extends Modelica.Icons.Package;
33
34 model World
35 Interfaces.Frame_b frame_b;
36 parameter Boolean enableAnimation = true;
37 parameter Boolean animateWorld = true;
38 parameter Boolean animateGravity = true;
39 parameter .Modelica.Mechanics.MultiBody.Types.AxisLabel label1 = "x";
40 parameter .Modelica.Mechanics.MultiBody.Types.AxisLabel label2 = "y";
41 parameter .Modelica.Mechanics.MultiBody.Types.GravityTypes gravityType = Types.GravityTypes.UniformGravity annotation(Evaluate = true);
42 parameter .Modelica.SIunits.Acceleration g = 9.81;
43 parameter .Modelica.Mechanics.MultiBody.Types.Axis n = {0, -1, 0} annotation(Evaluate = true);
44 parameter Real mue(unit = "m3/s2", min = 0) = 3.986e14;
45 parameter Boolean driveTrainMechanics3D = true;
46 parameter .Modelica.SIunits.Distance axisLength = nominalLength / 2;
47 parameter .Modelica.SIunits.Distance axisDiameter = axisLength / defaultFrameDiameterFraction;
48 parameter Boolean axisShowLabels = true;
49 input .Modelica.Mechanics.MultiBody.Types.Color axisColor_x = Types.Defaults.FrameColor;
50 input .Modelica.Mechanics.MultiBody.Types.Color axisColor_y = axisColor_x;
51 input .Modelica.Mechanics.MultiBody.Types.Color axisColor_z = axisColor_x;
52 parameter .Modelica.SIunits.Position[3] gravityArrowTail = {0, 0, 0};
53 parameter .Modelica.SIunits.Length gravityArrowLength = axisLength / 2;
54 parameter .Modelica.SIunits.Diameter gravityArrowDiameter = gravityArrowLength / defaultWidthFraction;
55 input .Modelica.Mechanics.MultiBody.Types.Color gravityArrowColor = {0, 230, 0};
56 parameter .Modelica.SIunits.Diameter gravitySphereDiameter = 12742000;
57 input .Modelica.Mechanics.MultiBody.Types.Color gravitySphereColor = {0, 230, 0};
58 parameter .Modelica.SIunits.Length nominalLength = 1;
59 parameter .Modelica.SIunits.Length defaultAxisLength = nominalLength / 5;
60 parameter .Modelica.SIunits.Length defaultJointLength = nominalLength / 10;
61 parameter .Modelica.SIunits.Length defaultJointWidth = nominalLength / 20;
62 parameter .Modelica.SIunits.Length defaultForceLength = nominalLength / 10;
63 parameter .Modelica.SIunits.Length defaultForceWidth = nominalLength / 20;
64 parameter .Modelica.SIunits.Length defaultBodyDiameter = nominalLength / 9;
65 parameter Real defaultWidthFraction = 20;
66 parameter .Modelica.SIunits.Length defaultArrowDiameter = nominalLength / 40;
67 parameter Real defaultFrameDiameterFraction = 40;
68 parameter Real defaultSpecularCoefficient(min = 0) = 0.7;
69 parameter Real defaultN_to_m(unit = "N/m", min = 0) = 1000;
70 parameter Real defaultNm_to_m(unit = "N.m/m", min = 0) = 1000;
71 protected
72 parameter Integer ndim = if enableAnimation and animateWorld then 1 else 0;
73 parameter Integer ndim2 = if enableAnimation and animateWorld and axisShowLabels then 1 else 0;
74 parameter .Modelica.SIunits.Length headLength = min(axisLength, axisDiameter * Types.Defaults.FrameHeadLengthFraction);
75 parameter .Modelica.SIunits.Length headWidth = axisDiameter * Types.Defaults.FrameHeadWidthFraction;
76 parameter .Modelica.SIunits.Length lineLength = max(0, axisLength - headLength);
77 parameter .Modelica.SIunits.Length lineWidth = axisDiameter;
78 parameter .Modelica.SIunits.Length scaledLabel = Types.Defaults.FrameLabelHeightFraction * axisDiameter;
79 parameter .Modelica.SIunits.Length labelStart = 1.05 * axisLength;
80 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape x_arrowLine(shapeType = "cylinder", length = lineLength, width = lineWidth, height = lineWidth, lengthDirection = {1, 0, 0}, widthDirection = {0, 1, 0}, color = axisColor_x, specularCoefficient = 0) if enableAnimation and animateWorld;
81 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape x_arrowHead(shapeType = "cone", length = headLength, width = headWidth, height = headWidth, lengthDirection = {1, 0, 0}, widthDirection = {0, 1, 0}, color = axisColor_x, r = {lineLength, 0, 0}, specularCoefficient = 0) if enableAnimation and animateWorld;
82 Modelica.Mechanics.MultiBody.Visualizers.Internal.Lines x_label(lines = scaledLabel * {[0, 0; 1, 1], [0, 1; 1, 0]}, diameter = axisDiameter, color = axisColor_x, r_lines = {labelStart, 0, 0}, n_x = {1, 0, 0}, n_y = {0, 1, 0}, specularCoefficient = 0) if enableAnimation and animateWorld and axisShowLabels;
83 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape y_arrowLine(shapeType = "cylinder", length = lineLength, width = lineWidth, height = lineWidth, lengthDirection = {0, 1, 0}, widthDirection = {1, 0, 0}, color = axisColor_y, specularCoefficient = 0) if enableAnimation and animateWorld;
84 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape y_arrowHead(shapeType = "cone", length = headLength, width = headWidth, height = headWidth, lengthDirection = {0, 1, 0}, widthDirection = {1, 0, 0}, color = axisColor_y, r = {0, lineLength, 0}, specularCoefficient = 0) if enableAnimation and animateWorld;
85 Modelica.Mechanics.MultiBody.Visualizers.Internal.Lines y_label(lines = scaledLabel * {[0, 0; 1, 1.5], [0, 1.5; 0.5, 0.75]}, diameter = axisDiameter, color = axisColor_y, r_lines = {0, labelStart, 0}, n_x = {0, 1, 0}, n_y = {-1, 0, 0}, specularCoefficient = 0) if enableAnimation and animateWorld and axisShowLabels;
86 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape z_arrowLine(shapeType = "cylinder", length = lineLength, width = lineWidth, height = lineWidth, lengthDirection = {0, 0, 1}, widthDirection = {0, 1, 0}, color = axisColor_z, specularCoefficient = 0) if enableAnimation and animateWorld;
87 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape z_arrowHead(shapeType = "cone", length = headLength, width = headWidth, height = headWidth, lengthDirection = {0, 0, 1}, widthDirection = {0, 1, 0}, color = axisColor_z, r = {0, 0, lineLength}, specularCoefficient = 0) if enableAnimation and animateWorld;
88 Modelica.Mechanics.MultiBody.Visualizers.Internal.Lines z_label(lines = scaledLabel * {[0, 0; 1, 0], [0, 1; 1, 1], [0, 1; 1, 0]}, diameter = axisDiameter, color = axisColor_z, r_lines = {0, 0, labelStart}, n_x = {0, 0, 1}, n_y = {0, 1, 0}, specularCoefficient = 0) if enableAnimation and animateWorld and axisShowLabels;
89 parameter .Modelica.SIunits.Length gravityHeadLength = min(gravityArrowLength, gravityArrowDiameter * Types.Defaults.ArrowHeadLengthFraction);
90 parameter .Modelica.SIunits.Length gravityHeadWidth = gravityArrowDiameter * Types.Defaults.ArrowHeadWidthFraction;
91 parameter .Modelica.SIunits.Length gravityLineLength = max(0, gravityArrowLength - gravityHeadLength);
92 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape gravityArrowLine(shapeType = "cylinder", length = gravityLineLength, width = gravityArrowDiameter, height = gravityArrowDiameter, lengthDirection = n, widthDirection = {0, 1, 0}, color = gravityArrowColor, r_shape = gravityArrowTail, specularCoefficient = 0) if enableAnimation and animateGravity and gravityType == Types.GravityTypes.UniformGravity;
93 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape gravityArrowHead(shapeType = "cone", length = gravityHeadLength, width = gravityHeadWidth, height = gravityHeadWidth, lengthDirection = n, widthDirection = {0, 1, 0}, color = gravityArrowColor, r_shape = gravityArrowTail + Modelica.Math.Vectors.normalize(n) * gravityLineLength, specularCoefficient = 0) if enableAnimation and animateGravity and gravityType == Types.GravityTypes.UniformGravity;
94 parameter Integer ndim_pointGravity = if enableAnimation and animateGravity and gravityType == Types.GravityTypes.UniformGravity then 1 else 0;
95 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape gravitySphere(shapeType = "sphere", r_shape = {-gravitySphereDiameter / 2, 0, 0}, lengthDirection = {1, 0, 0}, length = gravitySphereDiameter, width = gravitySphereDiameter, height = gravitySphereDiameter, color = gravitySphereColor, specularCoefficient = 0) if enableAnimation and animateGravity and gravityType == Types.GravityTypes.PointGravity;
96 equation
97 Connections.root(frame_b.R);
98 assert(Modelica.Math.Vectors.length(n) > 1.e-10, "Parameter n of World object is wrong (length(n) > 0 required)");
99 frame_b.r_0 = zeros(3);
100 frame_b.R = Frames.nullRotation();
101 end World;
102
103 package Frames
104 extends Modelica.Icons.Package;
105
106 record Orientation
107 extends Modelica.Icons.Record;
108 Real[3, 3] T;
109 .Modelica.SIunits.AngularVelocity[3] w;
110
111 encapsulated function equalityConstraint
112 extends .Modelica.Icons.Function;
113 input .Modelica.Mechanics.MultiBody.Frames.Orientation R1;
114 input .Modelica.Mechanics.MultiBody.Frames.Orientation R2;
115 output Real[3] residue;
116 algorithm
117 residue := {.Modelica.Math.atan2(cross(R1.T[1, :], R1.T[2, :]) * R2.T[2, :], R1.T[1, :] * R2.T[1, :]), .Modelica.Math.atan2(-cross(R1.T[1, :], R1.T[2, :]) * R2.T[1, :], R1.T[2, :] * R2.T[2, :]), .Modelica.Math.atan2(R1.T[2, :] * R2.T[1, :], R1.T[3, :] * R2.T[3, :])};
118 end equalityConstraint;
119 end Orientation;
120
121 function nullRotation
122 extends Modelica.Icons.Function;
123 output Orientation R;
124 algorithm
125 R := Orientation(T = identity(3), w = zeros(3));
126 end nullRotation;
127
128 package TransformationMatrices
129 extends Modelica.Icons.Package;
130
131 type Orientation
132 extends Internal.TransformationMatrix;
133
134 encapsulated function equalityConstraint
135 extends .Modelica.Icons.Function;
136 input .Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.Orientation T1;
137 input .Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.Orientation T2;
138 output Real[3] residue;
139 algorithm
140 residue := {cross(T1[1, :], T1[2, :]) * T2[2, :], -cross(T1[1, :], T1[2, :]) * T2[1, :], T1[2, :] * T2[1, :]};
141 end equalityConstraint;
142 end Orientation;
143
144 function resolve1
145 extends Modelica.Icons.Function;
146 input TransformationMatrices.Orientation T;
147 input Real[3] v2;
148 output Real[3] v1;
149 algorithm
150 v1 := transpose(T) * v2;
151 end resolve1;
152
153 function absoluteRotation
154 extends Modelica.Icons.Function;
155 input TransformationMatrices.Orientation T1;
156 input TransformationMatrices.Orientation T_rel;
157 output TransformationMatrices.Orientation T2;
158 algorithm
159 T2 := T_rel * T1;
160 end absoluteRotation;
161
162 function from_nxy
163 extends Modelica.Icons.Function;
164 input Real[3] n_x(each final unit = "1");
165 input Real[3] n_y(each final unit = "1");
166 output TransformationMatrices.Orientation T;
167 protected
168 Real abs_n_x = sqrt(n_x * n_x);
169 Real[3] e_x(each final unit = "1") = if abs_n_x < 1.e-10 then {1, 0, 0} else n_x / abs_n_x;
170 Real[3] n_z_aux(each final unit = "1") = cross(e_x, n_y);
171 Real[3] n_y_aux(each final unit = "1") = if n_z_aux * n_z_aux > 1.0e-6 then n_y else if abs(e_x[1]) > 1.0e-6 then {0, 1, 0} else {1, 0, 0};
172 Real[3] e_z_aux(each final unit = "1") = cross(e_x, n_y_aux);
173 Real[3] e_z(each final unit = "1") = e_z_aux / sqrt(e_z_aux * e_z_aux);
174 algorithm
175 T := {e_x, cross(e_z, e_x), e_z};
176 end from_nxy;
177 end TransformationMatrices;
178
179 package Internal
180 extends Modelica.Icons.InternalPackage;
181 type TransformationMatrix = Real[3, 3];
182 end Internal;
183 end Frames;
184
185 package Interfaces
186 extends Modelica.Icons.InterfacesPackage;
187
188 connector Frame
189 .Modelica.SIunits.Position[3] r_0;
190 Frames.Orientation R;
191 flow .Modelica.SIunits.Force[3] f annotation(unassignedMessage = "All Forces cannot be uniquely calculated.
192 The reason could be that the mechanism contains
193 a planar loop or that joints constrain the
194 same motion. For planar loops, use for one
195 revolute joint per loop the joint
196 Joints.RevolutePlanarLoopConstraint instead of
197 Joints.Revolute.");
198 flow .Modelica.SIunits.Torque[3] t;
199 end Frame;
200
201 connector Frame_b
202 extends Frame;
203 end Frame_b;
204
205 partial function partialSurfaceCharacteristic
206 extends Modelica.Icons.Function;
207 input Integer nu;
208 input Integer nv;
209 input Boolean multiColoredSurface = false;
210 output Modelica.SIunits.Position[nu, nv] X;
211 output Modelica.SIunits.Position[nu, nv] Y;
212 output Modelica.SIunits.Position[nu, nv] Z;
213 output Real[if multiColoredSurface then nu else 0, if multiColoredSurface then nv else 0, 3] C;
214 end partialSurfaceCharacteristic;
215 end Interfaces;
216
217 package Visualizers
218 extends Modelica.Icons.Package;
219
220 package Advanced
221 extends Modelica.Icons.Package;
222
223 model Shape
224 extends ModelicaServices.Animation.Shape;
225 extends Modelica.Utilities.Internal.PartialModelicaServices.Animation.PartialShape;
226 end Shape;
227
228 model Surface
229 extends Modelica.Mechanics.MultiBody.Icons.Surface;
230 extends Modelica.Utilities.Internal.PartialModelicaServices.Animation.PartialSurface;
231 extends ModelicaServices.Animation.Surface;
232 end Surface;
233 end Advanced;
234
235 package Internal
236 extends Modelica.Icons.InternalPackage;
237
238 model Lines
239 input Modelica.Mechanics.MultiBody.Frames.Orientation R = .Modelica.Mechanics.MultiBody.Frames.nullRotation();
240 input .Modelica.SIunits.Position[3] r = {0, 0, 0};
241 input .Modelica.SIunits.Position[3] r_lines = {0, 0, 0};
242 input Real[3] n_x(each final unit = "1") = {1, 0, 0};
243 input Real[3] n_y(each final unit = "1") = {0, 1, 0};
244 input .Modelica.SIunits.Position[:, 2, 2] lines = zeros(0, 2, 2);
245 input .Modelica.SIunits.Length diameter(min = 0) = 0.05;
246 input Modelica.Mechanics.MultiBody.Types.Color color = {0, 128, 255};
247 input .Modelica.Mechanics.MultiBody.Types.SpecularCoefficient specularCoefficient = 0.7;
248 protected
249 parameter Integer n = size(lines, 1);
250 .Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.Orientation R_rel = .Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.from_nxy(n_x, n_y);
251 .Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.Orientation R_lines = .Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.absoluteRotation(R.T, R_rel);
252 Modelica.SIunits.Position[3] r_abs = r + .Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.resolve1(R.T, r_lines);
253 Modelica.Mechanics.MultiBody.Visualizers.Advanced.Shape[n] cylinders(each shapeType = "cylinder", lengthDirection = array(.Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.resolve1(R_rel, vector([lines[i, 2, :] - lines[i, 1, :]; 0])) for i in 1:n), length = array(Modelica.Math.Vectors.length(lines[i, 2, :] - lines[i, 1, :]) for i in 1:n), r = array(r_abs + .Modelica.Mechanics.MultiBody.Frames.TransformationMatrices.resolve1(R_lines, vector([lines[i, 1, :]; 0])) for i in 1:n), each width = diameter, each height = diameter, each widthDirection = {0, 1, 0}, each color = color, each R = R, each specularCoefficient = specularCoefficient);
254 end Lines;
255 end Internal;
256 end Visualizers;
257
258 package Types
259 extends Modelica.Icons.TypesPackage;
260 type Axis = Modelica.Icons.TypeReal[3](each final unit = "1");
261 type AxisLabel = Modelica.Icons.TypeString;
262 type Color = Modelica.Icons.TypeInteger[3](each min = 0, each max = 255);
263 type SpecularCoefficient = Modelica.Icons.TypeReal(min = 0);
264 type ShapeType = Modelica.Icons.TypeString;
265 type ShapeExtra = Modelica.Icons.TypeReal;
266 type GravityTypes = enumeration(NoGravity, UniformGravity, PointGravity);
267
268 package Defaults
269 extends Modelica.Icons.Package;
270 constant Types.Color FrameColor = {0, 0, 0};
271 constant Real FrameHeadLengthFraction = 5.0;
272 constant Real FrameHeadWidthFraction = 3.0;
273 constant Real FrameLabelHeightFraction = 3.0;
274 constant Real ArrowHeadLengthFraction = 4.0;
275 constant Real ArrowHeadWidthFraction = 3.0;
276 end Defaults;
277 end Types;
278
279 package Icons
280 extends Modelica.Icons.IconsPackage;
281
282 model Surface end Surface;
283 end Icons;
284 end MultiBody;
285 end Mechanics;
286
287 package Math
288 extends Modelica.Icons.Package;
289
290 package Icons
291 extends Modelica.Icons.IconsPackage;
292
293 partial function AxisCenter end AxisCenter;
294 end Icons;
295
296 package Vectors
297 extends Modelica.Icons.Package;
298
299 function length
300 extends Modelica.Icons.Function;
301 input Real[:] v;
302 output Real result;
303 algorithm
304 result := sqrt(v * v);
305 end length;
306
307 function normalize
308 extends Modelica.Icons.Function;
309 input Real[:] v;
310 input Real eps(min = 0.0) = 100 * Modelica.Constants.eps;
311 output Real[size(v, 1)] result;
312 algorithm
313 result := smooth(0, noEvent(if length(v) >= eps then v / length(v) else v / eps));
314 end normalize;
315 end Vectors;
316
317 function asin
318 extends Modelica.Math.Icons.AxisCenter;
319 input Real u;
320 output .Modelica.SIunits.Angle y;
321 external "builtin" y = asin(u);
322 end asin;
323
324 function atan2
325 extends Modelica.Math.Icons.AxisCenter;
326 input Real u1;
327 input Real u2;
328 output .Modelica.SIunits.Angle y;
329 external "builtin" y = atan2(u1, u2);
330 end atan2;
331
332 function exp
333 extends Modelica.Math.Icons.AxisCenter;
334 input Real u;
335 output Real y;
336 external "builtin" y = exp(u);
337 end exp;
338 end Math;
339
340 package Utilities
341 extends Modelica.Icons.Package;
342
343 package Internal
344 extends Modelica.Icons.InternalPackage;
345
346 partial package PartialModelicaServices
347 extends Modelica.Icons.InternalPackage;
348
349 package Animation
350 extends Modelica.Icons.Package;
351
352 partial model PartialShape
353 parameter .Modelica.Mechanics.MultiBody.Types.ShapeType shapeType = "box";
354 input .Modelica.Mechanics.MultiBody.Frames.Orientation R = .Modelica.Mechanics.MultiBody.Frames.nullRotation();
355 input .Modelica.SIunits.Position[3] r = {0, 0, 0};
356 input .Modelica.SIunits.Position[3] r_shape = {0, 0, 0};
357 input Real[3] lengthDirection(each final unit = "1") = {1, 0, 0};
358 input Real[3] widthDirection(each final unit = "1") = {0, 1, 0};
359 input .Modelica.SIunits.Length length = 0;
360 input .Modelica.SIunits.Length width = 0;
361 input .Modelica.SIunits.Length height = 0;
362 input .Modelica.Mechanics.MultiBody.Types.ShapeExtra extra = 0.0;
363 input Real[3] color = {255, 0, 0};
364 input .Modelica.Mechanics.MultiBody.Types.SpecularCoefficient specularCoefficient = 0.7;
365 end PartialShape;
366
367 model PartialSurface
368 input .Modelica.Mechanics.MultiBody.Frames.Orientation R = .Modelica.Mechanics.MultiBody.Frames.nullRotation();
369 input Modelica.SIunits.Position[3] r_0 = {0, 0, 0};
370 parameter Integer nu = 2;
371 parameter Integer nv = 2;
372 replaceable function surfaceCharacteristic = Modelica.Mechanics.MultiBody.Interfaces.partialSurfaceCharacteristic;
373 parameter Boolean wireframe = false;
374 parameter Boolean multiColoredSurface = false;
375 input Real[3] color = {255, 0, 0};
376 input .Modelica.Mechanics.MultiBody.Types.SpecularCoefficient specularCoefficient = 0.7;
377 input Real transparency = 0;
378 end PartialSurface;
379 end Animation;
380 end PartialModelicaServices;
381 end Internal;
382 end Utilities;
383
384 package Constants
385 extends Modelica.Icons.Package;
386 final constant Real pi = 2 * Math.asin(1.0);
387 final constant Real eps = ModelicaServices.Machine.eps;
388 final constant .Modelica.SIunits.Velocity c = 299792458;
389 final constant Real mue_0(final unit = "N/A2") = 4 * pi * 1.e-7;
390 end Constants;
391
392 package Icons
393 extends Icons.Package;
394
395 partial model Example end Example;
396
397 partial package Package end Package;
398
399 partial package VariantsPackage
400 extends Modelica.Icons.Package;
401 end VariantsPackage;
402
403 partial package InterfacesPackage
404 extends Modelica.Icons.Package;
405 end InterfacesPackage;
406
407 partial package TypesPackage
408 extends Modelica.Icons.Package;
409 end TypesPackage;
410
411 partial package IconsPackage
412 extends Modelica.Icons.Package;
413 end IconsPackage;
414
415 partial package InternalPackage end InternalPackage;
416
417 partial function Function end Function;
418
419 partial record Record end Record;
420
421 type TypeReal
422 extends Real;
423 end TypeReal;
424
425 type TypeInteger
426 extends Integer;
427 end TypeInteger;
428
429 type TypeString
430 extends String;
431 end TypeString;
432 end Icons;
433
434 package SIunits
435 extends Modelica.Icons.Package;
436
437 package Conversions
438 extends Modelica.Icons.Package;
439
440 package NonSIunits
441 extends Modelica.Icons.Package;
442 type Temperature_degC = Real(final quantity = "ThermodynamicTemperature", final unit = "degC");
443 end NonSIunits;
444 end Conversions;
445
446 type Angle = Real(final quantity = "Angle", final unit = "rad", displayUnit = "deg");
447 type Length = Real(final quantity = "Length", final unit = "m");
448 type Position = Length;
449 type Distance = Length(min = 0);
450 type Radius = Length(min = 0);
451 type Diameter = Length(min = 0);
452 type AngularVelocity = Real(final quantity = "AngularVelocity", final unit = "rad/s");
453 type Velocity = Real(final quantity = "Velocity", final unit = "m/s");
454 type Acceleration = Real(final quantity = "Acceleration", final unit = "m/s2");
455 type Force = Real(final quantity = "Force", final unit = "N");
456 type Torque = Real(final quantity = "Torque", final unit = "N.m");
457 type FaradayConstant = Real(final quantity = "FaradayConstant", final unit = "C/mol");
458 end SIunits;
459end Modelica;
460
461package VehicleInterfaces
462 extends Modelica.Icons.Package;
463
464 package Icons
465 extends Modelica.Icons.Package;
466
467 partial class VariantLibrary
468 extends Modelica.Icons.VariantsPackage;
469 end VariantLibrary;
470 end Icons;
471
472 package Roads
473 extends VehicleInterfaces.Icons.VariantLibrary;
474
475 package Interfaces
476 extends Modelica.Icons.InterfacesPackage;
477
478 partial function positionBase
479 extends Modelica.Icons.Function;
480 input Real s = 0;
481 input Real w = 0;
482 output Modelica.SIunits.Position[3] r_0 = zeros(3);
483 end positionBase;
484
485 partial function trackOffsetBase
486 extends Modelica.Icons.Function;
487 input Real s = 0;
488 input Real w = 0;
489 output Modelica.SIunits.Position[3] trackOffset = zeros(3);
490 end trackOffsetBase;
491
492 partial function normalBase
493 extends Modelica.Icons.Function;
494 input Real s = 0;
495 input Real w = 0;
496 output Real[3] e_n_0 = {0, 0, 1};
497 end normalBase;
498
499 partial function headingDirectionBase
500 extends Modelica.Icons.Function;
501 input Real s = 0;
502 input Real w = 0;
503 output Real[3] e_s_0 = {1, 0, 0};
504 end headingDirectionBase;
505
506 partial function frictionCoefficientBase
507 extends Modelica.Icons.Function;
508 input Real s = 0;
509 input Real w = 0;
510 output Real mue = 1;
511 end frictionCoefficientBase;
512
513 partial model Base
514 replaceable function position = VehicleInterfaces.Roads.Interfaces.positionBase;
515 replaceable function trackOffset = VehicleInterfaces.Roads.Interfaces.trackOffsetBase;
516 replaceable function normal = VehicleInterfaces.Roads.Interfaces.normalBase;
517 replaceable function headingDirection = VehicleInterfaces.Roads.Interfaces.headingDirectionBase;
518 replaceable function frictionCoefficient = VehicleInterfaces.Roads.Interfaces.frictionCoefficientBase;
519 end Base;
520 end Interfaces;
521
522 model CircleRoad
523 extends VehicleInterfaces.Roads.Interfaces.Base(redeclare final function position = circlePosition(radius = radius, width = width), redeclare final function trackOffset = constantOffset, redeclare final function normal = circleNormal, redeclare final function headingDirection = circleHeadingDirection(radius = radius), redeclare final function frictionCoefficient = circleFrictionCoefficient(mue_fixed = mue));
524 parameter Boolean animation = true;
525 parameter Modelica.SIunits.Radius radius;
526 parameter Modelica.SIunits.Length width;
527 parameter Real mue;
528 parameter Modelica.Mechanics.MultiBody.Types.Color roadColor = {255, 0, 0};
529 constant Real pi = Modelica.Constants.pi;
530 protected
531 outer Modelica.Mechanics.MultiBody.World world;
532 VehicleInterfaces.Roads.Internal.VisualizeSimpleRoads roadShape(ns = 100, nw = 10, s_min = 0, s_max = 2 * pi * radius, w_min = -width / 2, w_max = width / 2, color = roadColor) if animation and world.enableAnimation;
533
534 function r_middle
535 extends Modelica.Icons.Function;
536 input Real s = 0;
537 input Real radius = 1;
538 output Real[3] r_0 = {radius * sin(s / radius), -radius * cos(s / radius), 0};
539 algorithm
540 r_0 := {radius * sin(s / radius), -radius * cos(s / radius), 0};
541 end r_middle;
542
543 function circlePosition
544 extends VehicleInterfaces.Roads.Interfaces.positionBase;
545 input Modelica.SIunits.Radius radius = 1;
546 input Modelica.SIunits.Length width = 1;
547 algorithm
548 r_0 := r_middle(s, radius) + w * .Modelica.Math.Vectors.normalize(r_middle(s, radius));
549 end circlePosition;
550
551 function constantOffset
552 extends VehicleInterfaces.Roads.Interfaces.trackOffsetBase;
553 algorithm
554 trackOffset := {0, 0, 0};
555 end constantOffset;
556
557 function circleNormal
558 extends VehicleInterfaces.Roads.Interfaces.normalBase;
559 algorithm
560 e_n_0 := {0, 0, 1};
561 end circleNormal;
562
563 function circleHeadingDirection
564 extends VehicleInterfaces.Roads.Interfaces.headingDirectionBase;
565 input Modelica.SIunits.Radius radius = 1;
566 algorithm
567 e_s_0 := {cos(s / radius), sin(s / radius), 0};
568 end circleHeadingDirection;
569
570 function circleFrictionCoefficient
571 extends VehicleInterfaces.Roads.Interfaces.frictionCoefficientBase;
572 input Real mue_fixed = 1;
573 algorithm
574 mue := mue_fixed;
575 end circleFrictionCoefficient;
576 end CircleRoad;
577
578 package Internal
579 extends Modelica.Icons.InternalPackage;
580
581 model VisualizeSimpleRoads
582 parameter Integer ns(min = 2) = 2;
583 parameter Integer nw(min = 2) = 2;
584 extends Modelica.Mechanics.MultiBody.Visualizers.Advanced.Surface(final nu = ns, final nv = nw, redeclare final function surfaceCharacteristic = roadSurfaceCharacteristic(final position = road.position, final s_min = s_min, final s_max = s_max, final w_min = w_min, final w_max = w_max));
585 parameter Real s_min = 0;
586 parameter Real s_max = 1;
587 parameter Real w_min = -1;
588 parameter Real w_max = 1;
589 protected
590 outer VehicleInterfaces.Roads.Interfaces.Base road;
591
592 encapsulated function roadSurfaceCharacteristic
593 extends .Modelica.Mechanics.MultiBody.Interfaces.partialSurfaceCharacteristic(final multiColoredSurface = false);
594 input .VehicleInterfaces.Roads.Interfaces.positionBase position;
595 input Real s_min = 0;
596 input Real s_max = 1;
597 input Real w_min = -1;
598 input Real w_max = 1;
599 protected
600 Real s;
601 Real w;
602 Real[3] r;
603 parameter Real ds = s_max - s_min;
604 parameter Real dw = w_max - w_min;
605 algorithm
606 for j in 1:nv loop
607 w := w_min + (j - 1) * dw / (nv - 1);
608 for i in 1:nu loop
609 s := s_min + (i - 1) * ds / (nu - 1);
610 r := position(s, w);
611 X[i, j] := r[1];
612 Y[i, j] := r[2];
613 Z[i, j] := r[3];
614 end for;
615 end for;
616 end roadSurfaceCharacteristic;
617 end VisualizeSimpleRoads;
618
619 model DummyTyre
620 parameter .Modelica.SIunits.Radius wheelRadius = 1;
621 input Real[3] e_axis;
622 input .Modelica.SIunits.Position[3] r_wheel;
623 output .Modelica.SIunits.Position[3] r_CP(start = {0, -wheelRadius, 0});
624 output .Modelica.SIunits.Length penetrationDepth;
625 output Real mue;
626 Real s(start = 0);
627 Real w(start = 0);
628 outer VehicleInterfaces.Roads.Interfaces.Base road;
629 protected
630 Real[3] e_n;
631 Real[3] e_s;
632 Real[3] e_w;
633 Real[3] e_CP;
634 Real[3] d_CP;
635 Modelica.SIunits.Radius reducedRadius;
636 equation
637 r_CP = road.position(s, w);
638 e_n = road.normal(s, w);
639 e_s = road.headingDirection(s, w);
640 e_w = cross(e_n, e_s);
641 e_CP = .Modelica.Math.Vectors.normalize(e_n - e_n * e_axis * e_axis);
642 d_CP = r_wheel - r_CP;
643 reducedRadius = e_n * d_CP / (e_n * e_CP);
644 penetrationDepth = wheelRadius - reducedRadius;
645 0 = e_s * d_CP - reducedRadius * (e_s * e_CP);
646 0 = e_w * d_CP - reducedRadius * (e_w * e_CP);
647 mue = road.frictionCoefficient(s, w);
648 end DummyTyre;
649
650 model CheckContactCalculation
651 extends Modelica.Icons.Example;
652 parameter Real wheelRadius = 1;
653 Real phi;
654 Real radius_wheel;
655 Real[3] axis_wheel;
656 Real[3] r_circle = {road.radius * sin(phi), -road.radius * cos(phi), 0};
657 VehicleInterfaces.Roads.Internal.DummyTyre tyre(wheelRadius = wheelRadius, e_axis = Modelica.Math.Vectors.normalize(axis_wheel), r_wheel = {radius_wheel * sin(phi), -radius_wheel * cos(phi), 1 + sin(phi) / 20});
658 inner VehicleInterfaces.Roads.CircleRoad road(radius = 50, width = 8, mue = 0.5, roadColor = {100, 100, 100});
659 inner Modelica.Mechanics.MultiBody.World world(enableAnimation = true, axisLength = 20, n = {0, 0, -1});
660 equation
661 phi = time;
662 radius_wheel = road.radius + road.radius / 20 * sin(20 * phi);
663 axis_wheel = {sin(phi), -cos(phi), sin(10 * phi) / 10};
664 end CheckContactCalculation;
665 end Internal;
666 end Roads;
667end VehicleInterfaces;
668
669model CheckContactCalculation
670 extends VehicleInterfaces.Roads.Internal.CheckContactCalculation;
671 annotation(experiment(StopTime = 6.2));
672end CheckContactCalculation;