PARTS_ATEs_moving.py
You can view and download this file on Github: PARTS_ATEs_moving.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: example to simulate moving ATEs of PARTS
5#
6# Author: Michael Pieber and Johannes Gerstmayr
7# Date: 2020-01-14
8#
9# Copyright:This file is part of Exudyn. Exudyn is free software. You can redistribute it and/or modify it under the terms of the Exudyn license. See 'LICENSE.txt' for more details.
10#
11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
13import exudyn as exu
14from exudyn.itemInterface import *
15
16import numpy as np
17
18testIsActive = exu.sys.get('testIsActive', False)
19
20#plots
21if not testIsActive:
22 import matplotlib.pyplot as plt
23 import matplotlib.ticker as ticker
24
25SC = exu.SystemContainer()
26mbs = SC.AddSystem()
27
28computeDynamic=True
29displaySimulation=True
30
31
32#Options, if False RevoluteJoint2D are used
33CartesianSpringDamperActive = False
34
35#Options for visualization
36frameList = True
37
38
39#Dimensions
40L1 = 38e-3 #mm
41L2 = 29e-3 #mm
42L3 = 8e-3 #mm
43L4 = 10e-3 #mm
44psi = np.arctan(L3/L1) #11.9°
45CE = L3/np.sin(psi) #38.8e-3 mm
46
47
48
49
50
51
52
53
54
55## Visualization
56if frameList:
57 fL = 0.01 #frame length
58else:
59 fL = 0
60graphicsX = {'type':'Line', 'color':[0.8,0.1,0.1,1], 'data':[0,0,0, fL,0,0]}
61graphicsY = {'type':'Line', 'color':[0.1,0.8,0.1,1], 'data':[0,0,0, 0,fL,0]}
62graphicsZ = {'type':'Line', 'color':[0.1,0.1,0.8,1], 'data':[0,0,0, 0,0,fL]}
63frameList = [] #no frames
64#frameList = [graphicsX,graphicsY,graphicsZ]
65
66
67
68
69class CSixBarLinkage:
70 def __init__(self, name = ''):
71 self.name = name #needed?
72
73## USER defined functions
74
75def To3D(p): #convert numpy 2D vector to 3D list
76 return [p[0],p[1],0]
77
78
79def RotZVec(angleRad,vec):
80 Rz=np.array([ [np.cos(angleRad),-np.sin(angleRad)],
81 [np.sin(angleRad), np.cos(angleRad)] ])
82 return np.dot(Rz,np.array(vec));
83
84
85def centreM(vec1, vec2): #Centre M of a line AB
86 return (vec1+vec2)/2
87
88
89def userLoadDriveAle(t, load): #Add drive via ALE:
90 if t < 1:
91 return [-t*0.1,t*0.1,0 ]
92 return [-1,1,0]
93
94
95
96def SixBarLinkage(P00,theta,alphaMean): #compute one six-bar linkage (P00: idealized rotation point, theta: rotaton of the six-bar linkage, angleMean: angle of the six-bar linkage (alpha, beta, gamma))
97
98 #Parameters for RigidBody2D
99 massRigid = 1e0
100 inertiaRigid = 3.3e1
101
102 #Parameters for CartesianSpringDamper
103 if CartesianSpringDamperActive:
104 k=[1e6,1e6,0]
105 d=[1e2,1e2,0]
106
107 #define the points/coordinates one six-bar linkage
108 P01=[0,0]
109 P1=[L1,0]
110 P2=[L1+L2,0]
111 P3=[L1,L3]
112 P4=[L1+L2,L3]
113 P5=[L1+CE*np.cos(alphaMean-psi),L3+CE*np.sin(alphaMean-psi)]
114 P6=[L1+L2+CE*np.cos(alphaMean-psi),L3+CE*np.sin(alphaMean-psi)]
115 P7=[L1+CE*np.cos(alphaMean-psi)+L2*np.cos(alphaMean),L3+CE*np.sin(alphaMean-psi)+L2*np.sin(alphaMean)]
116 P8=[CE*np.cos(alphaMean-psi),CE*np.sin(alphaMean-psi)]
117 P9=[CE*np.cos(alphaMean-psi)+L2*np.cos(alphaMean),CE*np.sin(alphaMean-psi)+L2*np.sin(alphaMean)]
118 P10=[L1*np.cos(alphaMean),L1*np.sin(alphaMean)]
119 P11=[(L1+L2)*np.cos(alphaMean),(L1+L2)*np.sin(alphaMean)]
120
121 PList=[P01,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11]
122
123 #calculate vectors of the two long arms
124 P53=list(np.array(PList[3])-np.array(PList[5]))
125 P56=list(np.array(PList[6])-np.array(PList[5]))
126 P57=list(np.array(PList[7])-np.array(PList[5]))
127 P58=list(np.array(PList[8])-np.array(PList[5]))
128
129 #displacement and rotation of all points P01-P11
130 pT=[]
131 for x in range(len(PList)):
132 pT+=[list(RotZVec(theta,np.array(PList[x]))+P00)]
133
134 #calculate centrers of mass of the rigid bodies
135 M0=list(centreM(np.array(pT[4]), np.array(pT[3])))
136 M2=list(centreM(np.array(pT[6]), np.array(pT[4])))
137 M5=list(centreM(np.array(pT[7]), np.array(pT[9])))
138 M7=list(centreM(np.array(pT[9]), np.array(pT[8])))
139
140
141 #Draw++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
142 bLength = 0.5e-3 #y-dim of pendulum
143 graphicsCE = {'type':'Line', 'color':[0.1,0.1,0.8,1], 'data':[-CE/2,-bLength,0, CE/2,-bLength,0, CE/2,bLength,0, -CE/2,bLength,0, -CE/2,-bLength,0]}
144 graphicsL2 = {'type':'Line', 'color':[0.1,0.1,0.8,1], 'data':[-L2/2,-bLength,0, L2/2,-bLength,0, L2/2,bLength,0, -L2/2,bLength,0, -L2/2,-bLength,0]}
145
146 ad=list(RotZVec((alphaMean-psi),[-CE,-bLength]))
147 bd=list(RotZVec((alphaMean-psi),[0,-bLength]))
148 cd=list(RotZVec((alphaMean-psi),[0,bLength]))
149 dd=list(RotZVec((alphaMean-psi),[-CE,bLength]))
150 ed=list(RotZVec((alphaMean-psi),[-CE,-bLength]))
151 graphicsCE1 = {'type':'Line', 'color':[0.1,0.1,0.8,1], 'data':ad+[0]+ bd+[0]+ cd+[0]+ dd+[0]+ ed+[0]} #background
152
153 ad=list(RotZVec((alphaMean-psi),[0,-bLength]))
154 bd=list(RotZVec((alphaMean-psi),[L2*np.cos(psi),L2*np.sin(psi)-bLength]))
155 cd=list(RotZVec((alphaMean-psi),[L2*np.cos(psi),L2*np.sin(psi)+bLength]))
156 dd=list(RotZVec((alphaMean-psi),[0,bLength]))
157 ed=list(RotZVec((alphaMean-psi),[0,-bLength]))
158 graphicsL21 = {'type':'Line', 'color':[0.1,0.1,0.8,1], 'data':ad+[0]+ bd+[0]+ cd+[0]+ dd+[0]+ ed+[0]} #background
159
160 ad=list(RotZVec(0,[-CE*np.cos(psi),-CE*np.sin(psi)-bLength]))
161 bd=list(RotZVec(0,[0,-bLength]))
162 cd=list(RotZVec(0,[0,bLength]))
163 dd=list(RotZVec(0,[-CE*np.cos(psi),-CE*np.sin(psi)+bLength]))
164 ed=list(RotZVec(0,[-CE*np.cos(psi),-CE*np.sin(psi)-bLength]))
165 graphicsCE2 = {'type':'Line', 'color':[0.1,0.1,0.8,1],'data':ad+[0]+ bd+[0]+ cd+[0]+ dd+[0]+ ed+[0]}
166
167 ad=list(RotZVec(0,[0,-bLength]))
168 bd=list(RotZVec(0,[L2,-bLength]))
169 cd=list(RotZVec(0,[L2,bLength]))
170 dd=list(RotZVec(0,[0,bLength]))
171 ed=list(RotZVec(0,[0,-bLength]))
172 graphicsL22 = {'type':'Line', 'color':[0.1,0.1,0.8,1], 'data':ad+[0]+ bd+[0]+ cd+[0]+ dd+[0]+ ed+[0]} #background
173
174
175 #Define rigid bodies and markers+++++++++++++++++++++++++++++++++++++++++++
176 #rigid body M0 with markers mR01,mR02
177 nRigid0 = mbs.AddNode(Rigid2D(referenceCoordinates=M0+[theta], initialVelocities=[0.,0.,0.]));
178 oRigid0 = mbs.AddObject(RigidBody2D(mass=massRigid, inertia=inertiaRigid,nodeNumber=nRigid0,visualization=VObjectRigidBody2D(graphicsData= [graphicsL2]+frameList)))
179 mR01 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid0, localPosition=[-L2/2,0.,0.]))
180 mR02 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid0, localPosition=[L2/2,0.,0.]))
181
182 #rigid body M1 with markers mR1,mR2,mR15
183 nRigid1 = mbs.AddNode(Rigid2D(referenceCoordinates=pT[5]+[0.+theta], initialVelocities=[0.,0.,0.]));
184 oRigid1 = mbs.AddObject(RigidBody2D(mass=massRigid, inertia=inertiaRigid,nodeNumber=nRigid1,visualization=VObjectRigidBody2D(graphicsData= [graphicsCE1,graphicsL21]+frameList)))
185 mR1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid1, localPosition=P53+[0.]))
186 mR2 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid1, localPosition=P57+[0.]))
187 mR15 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid1, localPosition=[0.,0.,0.]))
188
189 #rigid body M2 with markers mR3,mR4
190 nRigid2 = mbs.AddNode(Rigid2D(referenceCoordinates=M2+[alphaMean-psi+theta], initialVelocities=[0.,0.,0.]));
191 oRigid2 = mbs.AddObject(RigidBody2D(mass=massRigid, inertia=inertiaRigid,nodeNumber=nRigid2,visualization=VObjectRigidBody2D(graphicsData= [graphicsCE]+frameList)))
192 mR3 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=[-CE/2,0.,0]))
193 mR4 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=[CE/2,0.,0]))
194
195 #rigid body M5 with markers mR9,mR10
196 nRigid5 = mbs.AddNode(Rigid2D(referenceCoordinates=M5+[psi+theta], initialVelocities=[0.,0.,0.]));
197 oRigid5 = mbs.AddObject(RigidBody2D(mass=massRigid, inertia=inertiaRigid,nodeNumber=nRigid5,visualization=VObjectRigidBody2D(graphicsData= [graphicsCE]+frameList)))
198 mR9 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid5, localPosition=[-CE/2,0.,0]))
199 mR10 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid5, localPosition=[CE/2,0.,0]))
200
201 #rigid body M6 with markers mR11,mR12,mR65
202 nRigid6 = mbs.AddNode(Rigid2D(referenceCoordinates=pT[5]+[0.+theta], initialVelocities=[0.,0.,0.]));
203 oRigid6 = mbs.AddObject(RigidBody2D(mass=massRigid, inertia=inertiaRigid,nodeNumber=nRigid6,visualization=VObjectRigidBody2D(graphicsData= [graphicsCE2,graphicsL22]+frameList)))
204 mR11 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid6, localPosition=P58+[0.]))
205 mR12 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid6, localPosition=P56+[0.]))
206 mR65 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid6, localPosition=[0.,0.,0.]))
207
208 #rigid body M7 with markers mR13,mR14
209 nRigid7 = mbs.AddNode(Rigid2D(referenceCoordinates=M7+[alphaMean+theta], initialVelocities=[0.,0.,0.]));
210 oRigid7 = mbs.AddObject(RigidBody2D(mass=massRigid, inertia=inertiaRigid,nodeNumber=nRigid7,visualization=VObjectRigidBody2D(graphicsData= [graphicsL2]+frameList)))
211 mR13 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid7, localPosition=[-L2/2,0.,0.]))
212 mR14 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid7, localPosition=[L2/2,0.,0.]))
213
214 #plug and socket for connection
215 mC1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid0, localPosition=[-L2/2-L4,-L3,0.]))
216 mC2 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid7, localPosition=[-L2/2-L4,L3,0.]))
217
218 #markers for prismatic joints
219 mP02 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRigid0, localPosition=[L2/2,0.,0.]))
220 mP14 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRigid7, localPosition=[L2/2,0.,0.]))
221
222
223 #Define joints+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
224 if CartesianSpringDamperActive:
225 mbs.AddObject(CartesianSpringDamper(markerNumbers=[mR01,mR1],stiffness=k,damping=d))
226 mbs.AddObject(CartesianSpringDamper(markerNumbers=[mR02,mR3],stiffness=k,damping=d))
227
228 mbs.AddObject(CartesianSpringDamper(markerNumbers=[mR10,mR2],stiffness=k,damping=d))
229 mbs.AddObject(CartesianSpringDamper(markerNumbers=[mR14,mR9],stiffness=k,damping=d))
230 mbs.AddObject(CartesianSpringDamper(markerNumbers=[mR13,mR11],stiffness=k,damping=d))
231
232 mbs.AddObject(CartesianSpringDamper(markerNumbers=[mR15,mR65],stiffness=k,damping=d))
233 mbs.AddObject(CartesianSpringDamper(markerNumbers=[mR4,mR12],stiffness=k,damping=d))
234 else:
235 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR01,mR1]))
236 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR02,mR3]))
237
238 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR10,mR2]))
239 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR14,mR9]))
240 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR13,mR11]))
241
242 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR15,mR65]))
243 mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR4,mR12]))
244
245
246 dictSixBarLinkages = {"markersForPrismaticJoints":[mP02,mP14],"markersForConnectors":[mC1,mC2],"markers":[mR1,mR3,mR9,mR01,mR02],"coordinatePoints":pT,"objects":[oRigid0,oRigid7],"nodes":[nRigid0,nRigid7]}
247
248 return dictSixBarLinkages
249
250
251
252def ATC(A,B,C):
253
254 AB=(np.array(B)-np.array(A))
255 BC=(np.array(C)-np.array(B))
256 AC=(np.array(C)-np.array(A))
257
258# offsetAngle=np.arcsin( np.dot(np.array(e1),AB)/(np.linalg.norm(np.array(e1)*np.linalg.norm(AB))))
259 offsetAngle=np.arctan2(AB[1],AB[0])
260
261 a=np.linalg.norm(np.array(C)-np.array(B))
262 b=np.linalg.norm(np.array(C)-np.array(A))
263 c=np.linalg.norm(np.array(B)-np.array(A))
264
265 alpha=np.arccos((np.square(b)+np.square(c)-np.square(a))/(2*b*c))
266 beta=np.arccos((np.square(a)+np.square(c)-np.square(b))/(2*a*c))
267 gamma=np.arccos((np.square(a)+np.square(b)-np.square(c))/(2*a*b))
268
269 s1=SixBarLinkage(A,offsetAngle,alpha)
270 s2=SixBarLinkage(B,np.pi-beta+offsetAngle,beta)
271 s3=SixBarLinkage(C,-(beta+gamma)+offsetAngle,gamma)
272
273 #markersForConnectors
274 mFC=[s1["markersForConnectors"][0],s2["markersForConnectors"][1],s2["markersForConnectors"][0],s3["markersForConnectors"][1],s3["markersForConnectors"][0],s1["markersForConnectors"][1]]
275
276 #markersForPismaticJoints
277 mFPJ=[s1["markersForPrismaticJoints"][0],s2["markersForPrismaticJoints"][1],s2["markersForPrismaticJoints"][0],s3["markersForPrismaticJoints"][1],s3["markersForPrismaticJoints"][0],s1["markersForPrismaticJoints"][1]]
278
279 mbs.AddObject(PrismaticJoint2D(markerNumbers=[mFPJ[0],mFPJ[1]],axisMarker0=[1.,0.,0.],normalMarker1=[0.,-1.,0.], constrainRotation=True))
280 mbs.AddObject(PrismaticJoint2D(markerNumbers=[mFPJ[2],mFPJ[3]],axisMarker0=[1.,0.,0.],normalMarker1=[0.,-1.,0.], constrainRotation=True))
281 mbs.AddObject(PrismaticJoint2D(markerNumbers=[mFPJ[5],mFPJ[4]],axisMarker0=[1.,0.,0.],normalMarker1=[0.,-1.,0.], constrainRotation=True))
282
283 La=a-2*L1-2*L2
284 Lb=b-2*L1-2*L2
285 Lc=c-2*L1-2*L2
286 mbs.AddObject(SpringDamper(markerNumbers = [mFPJ[0],mFPJ[1]], stiffness = 1e5, damping=10e2, referenceLength=Lc))
287 mbs.AddObject(SpringDamper(markerNumbers = [mFPJ[2],mFPJ[3]], stiffness = 1e5, damping=10e2, referenceLength=La))
288 mbs.AddObject(SpringDamper(markerNumbers = [mFPJ[4],mFPJ[5]], stiffness = 1e5, damping=10e2, referenceLength=Lb))
289
290 points=[s1["coordinatePoints"][0],s1["coordinatePoints"][3],s1["coordinatePoints"][4]]
291 nodes=[s1["nodes"][0],s1["nodes"][1],s2["nodes"][1],s3["nodes"][1]]
292 objects=[s1["objects"][0],s1["objects"][1]]
293 markers=[s1["markers"][0],s1["markers"][1],s1["markers"][2],s1["markers"][3],s1["markers"][4]]
294
295 dictATE = {"markersForConnectors":mFC,"markersForPrismaticJoints":mFPJ,"coordinatePoints":points,"nodes":nodes,"objects":objects,"markers":markers}
296 return dictATE
297
298
299
300
301
302## START main program
303
304#define start mesh
305
306nx = 4
307ny = 2
308endTime = 0.05
309
310###connection of ATCs
311kk=[1e8,1e8,0]
312dd=[1e2,1e2,0]
313
314
315topCon=[]
316bottomCon=[]
317for y in range(ny):
318 for x in range(nx):
319 A=[0.229*(x),0.229*(y)]
320 B=[0.229*(x+1),0.229*(y)]
321 C=[0.229*(x+1),0.229*(y+1)]
322 D=[0.229*(x),0.229*(y+1)]
323
324
325 #create ATCs
326 n1=ATC(A,B,C)
327 n2=ATC(A,C,D)
328 #n3=ATC(C,D,E)
329
330 mbs.AddObject(CartesianSpringDamper(markerNumbers=[n1["markersForConnectors"][5],n2["markersForConnectors"][0]],stiffness=kk,damping=dd))
331 mbs.AddObject(CartesianSpringDamper(markerNumbers=[n1["markersForConnectors"][4],n2["markersForConnectors"][1]],stiffness=kk,damping=dd))
332
333 if x == 0 and y == 0:
334 nFirstATE=n1 #used only now first ATC for visualization purpose/boundary/force
335 if x == 0:
336 topCon+=[n2["markersForConnectors"][3]]
337 topCon+=[n2["markersForConnectors"][2]]
338 bottomCon+=[n1["markersForConnectors"][0]]
339 bottomCon+=[n1["markersForConnectors"][1]]
340 if x > 0 and x < nx:
341 c21=n2["markersForConnectors"][5]
342 c22=n2["markersForConnectors"][4]
343
344 mbs.AddObject(CartesianSpringDamper(markerNumbers=[c11alt,c21],stiffness=kk,damping=dd))
345 mbs.AddObject(CartesianSpringDamper(markerNumbers=[c12alt,c22],stiffness=kk,damping=dd))
346
347 topCon+=[n2["markersForConnectors"][3]]
348 topCon+=[n2["markersForConnectors"][2]]
349 bottomCon+=[n1["markersForConnectors"][0]]
350 bottomCon+=[n1["markersForConnectors"][1]]
351
352 c11alt=n1["markersForConnectors"][2]
353 c12alt=n1["markersForConnectors"][3]
354
355
356 if y > 0 and y < ny:
357 for i in range(len(topCon)-(nx*2)):
358 mbs.AddObject(CartesianSpringDamper(markerNumbers=[topCon[i],bottomCon[i+(nx*2)]],stiffness=kk,damping=dd))
359
360
361
362n1=nFirstATE
363P0=n1["coordinatePoints"][0]
364#
365#
366###connection of ATCs
367#kk=[1e4,1e4,0]
368#dd=[1e2,1e2,0]
369#
370#mbs.AddObject(CartesianSpringDamper(markerNumbers=[n2["markersForConnectors"][0],n1["markersForConnectors"][5]],stiffness=kk,damping=dd))
371#mbs.AddObject(CartesianSpringDamper(markerNumbers=[n2["markersForConnectors"][1],n1["markersForConnectors"][4]],stiffness=kk,damping=dd))
372##
373
374
375##loads
376#mbs.AddLoad(Force(markerNumber = n1["markersForConnectors"][4], loadVector=[1e2,1e2,0]))
377
378
379#Michael
380#mbs.AddLoad(Force(markerNumber = topCon[15], loadVector=[1e3,-1e3,0]))
381#mbs.AddLoad(Force(markerNumber = bottomCon[7], loadVector=[-1e3,-1e3,0]))
382
383
384#Johannes:
385def userLoad(mbs, t, loadVector):
386 f=0.01+0.99*(1-np.cos((t/0.05)*np.pi)) #use small initial value for solver
387 if t>8:
388 f = 1
389 return [f*loadVector[0], f*loadVector[1], f*loadVector[2]]
390
391fL = 1e3*10
392mbs.AddLoad(Force(markerNumber = topCon[15], loadVector=[fL,-fL,0], loadVectorUserFunction = userLoad))
393mbs.AddLoad(Force(markerNumber = bottomCon[7], loadVector=[-fL,-fL,0], loadVectorUserFunction = userLoad))
394
395#for i in range (8):
396# mbs.AddLoad(Force(markerNumber = topCon[15-i], loadVector=[1e2,-1e2*0,0]))
397#for i in range (8):
398# mbs.AddLoad(Force(markerNumber = bottomCon[i], loadVector=[-1e2,-1e2*0,0]))
399#
400#
401#
402#mbs.AddLoad(Force(markerNumber = bottomCon[7], loadVector=[1e2,1e2,0]))
403#mbs.AddLoad(Force(markerNumber = topCon[15], loadVector=[1e2,1e2,0]))
404
405
406
407#boundaries
408nGround=mbs.AddNode(PointGround(referenceCoordinates=P0+[0], visualization=VNodePointGround(show=False)))
409
410nRidgid0=n1["nodes"][0]
411##prescribe rotation of link
412mCoordinateGround = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nGround, coordinate=0)) #gives always 0 displacement
413
414mCoordinateRigid7x = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRidgid0, coordinate=0)) #angle of node of Rigid7
415mCoordinateRigid7y = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRidgid0, coordinate=1)) #angle of node of Rigid7
416mCoordinateRigid7phi = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRidgid0, coordinate=2)) #angle of node of Rigid7
417#
418#
419#JOH: oCoordAlphax = mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround,mCoordinateRigid7x], offset = 0))
420oCoordAlphay = mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround,mCoordinateRigid7y], offset = 0))
421#JOH: oCoordAlpha = mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround,mCoordinateRigid7phi], offset = 0))
422
423
424
425nRidgid7=n1["nodes"][1]
426mCoordinateRigid7x = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRidgid7, coordinate=0)) #angle of node of Rigid7
427mCoordinateRigid7y = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRidgid7, coordinate=1)) #angle of node of Rigid7
428mCoordinateRigid7phi = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRidgid7, coordinate=2)) #angle of node of Rigid7
429
430
431#oCoordAlphax = mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround,mCoordinateRigid7x], offset = 0))
432#JOH: oCoordAlphay = mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround,mCoordinateRigid7y], offset = 0))
433#oCoordAlpha = mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround,mCoordinateRigid7phi], offset = 0))
434
435#hack to add additional coordinate constraints:
436constrainLeftSide = True
437if constrainLeftSide:
438 nn = mbs.systemData.NumberOfNodes()
439 for i in range(nn):
440 n0 = mbs.GetNode(i)
441 if n0['nodeType'] == 'RigidBody2D':
442 if np.round(n0['referenceCoordinates'][0],3) == 0.008: #constrain all rigidbody nodes with xref=0.008
443 mCC = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=i, coordinate=0)) #x-displacement
444 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround,mCC]))
445 mCC = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=i, coordinate=1)) #y-displacement
446 mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordinateGround,mCC]))
447
448
449
450#mbs.AddObject(DistanceConstraint(markerNumbers=[n1["markersForPrismaticJoints"][0],n1["markersForPrismaticJoints"][1]], distance = 0.))
451#mbs.AddObject(DistanceConstraint(markerNumbers=[n1["markersForPrismaticJoints"][2],n1["markersForPrismaticJoints"][3]], distance = 0.))
452#mbs.AddObject(DistanceConstraint(markerNumbers=[n1["markersForPrismaticJoints"][4],n1["markersForPrismaticJoints"][5]], distance = 0.))
453
454
455mbs.Assemble() #creates initial configuration
456#exu.Print(mbs)
457
458simulationSettings = exu.SimulationSettings() #takes currently set values or default values
459
460
461uList=[]
462phiList=[]
463
464
465T=0.002
466SC.visualizationSettings.connectors.defaultSize = T
467SC.visualizationSettings.bodies.defaultSize = [T, T, T]
468SC.visualizationSettings.nodes.defaultSize = 0.0025
469SC.visualizationSettings.markers.defaultSize = 0.005
470SC.visualizationSettings.loads.defaultSize = 0.005
471
472SC.visualizationSettings.nodes.show= False
473SC.visualizationSettings.markers.show= False
474
475SC.visualizationSettings.openGL.lineWidth=2 #maximum
476
477SC.visualizationSettings.view0.scene.drawCoordinateSystem = False
478SC.visualizationSettings.view0.window.renderWindowSize=[1600,1024]
479
480##++++++++++++++++++++++++++++++
481##ANIMATIONS
482##make images for animations (requires FFMPEG):
483##requires a subfolder 'images'
484#simulationSettings.solution.recordImagesInterval=endTime/200
485
486if not testIsActive: #only start graphics once, but after background is set
487 SC.renderer.Start()
488
489 if displaySimulation:
490 SC.renderer.DoIdleTasks()
491
492#SC.visualizationSettings.nodes.show = False
493
494simulationSettings.solution.file.information = "PARTS_1Joint"
495
496
497if computeDynamic:
498 simulationSettings.timeIntegration.numberOfSteps = 50
499 simulationSettings.timeIntegration.endTime = endTime
500 simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8 #10000
501 simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-8
502 simulationSettings.timeIntegration.verboseMode = 1
503 #simulationSettings.timeIntegration.verboseModeFile = 1
504
505 simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
506 simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = simulationSettings.timeIntegration.generalizedAlpha.useNewmark
507 simulationSettings.timeIntegration.newton.useModifiedNewton = False
508 simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
509 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8
510 simulationSettings.timeIntegration.adaptiveStep = False #disable adaptive step reduction
511 ##############################################################
512 # IMPORTANT!!!!!!!!!
513 simulationSettings.linearSolver.solverType = exu.LinearSolverType.EigenSparse #sparse solver !!!!!!!!!!!!!!!
514 ##############################################################
515 simulationSettings.show.statistics = True
516
517
518 mbs.SolveDynamic(simulationSettings)
519
520
521if not testIsActive: #only start graphics once, but after background is set
522 if displaySimulation:
523 SC.renderer.DoIdleTasks()
524
525 SC.renderer.Stop() #safely close rendering window!
526
527nLast = mbs.systemData.NumberOfNodes()-1#just take last node-1 (last node is ground)
528
529uy=mbs.GetNodeOutput(nLast-1,exu.OutputVariableType.Position)[1] #y-coordinate of last node
530exu.Print("uy=", uy)
531exu.sys['testResult'] = uy