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