scissorPrismaticRevolute2D.py

You can view and download this file on Github: scissorPrismaticRevolute2D.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Create scissor-like chain of bodies and prismatic joints to test functionality
  5#
  6# Author:   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.utilities import * #includes itemInterface and rigidBodyUtilities
 15import exudyn.graphics as graphics
 16
 17testIsActive = exu.sys.get('testIsActive', False)
 18
 19SC = exu.SystemContainer()
 20mbs = SC.AddSystem()
 21
 22L = 0.8 #distance
 23b=L*0.1
 24mass = 1
 25g = 9.81*0.1
 26
 27#number of scissors:
 28n=3 #run test with n=3
 29
 30r = 0.05 #just for graphics
 31nL = (n+0.5)*L
 32graphicsBackground = graphics.Lines([[-L*1.5,-L*1.5,0], [1.5*nL,-L*1.5,0], [1.5*nL,nL,0], [-L*1.5,nL,0], [-L*1.5,-L*1.5,0]], color=graphics.color.lightgrey) #for appropriate zoom
 33graphicscube = graphics.Lines([[-L,-0.5*b,0], [L,-0.5*b,0], [L,0.5*b,0], [-L,0.5*b,0], [-L,-0.5*b,0]], color=graphics.color.steelblue) #graphics.Sphere(point=[0,0,0], radius=r, color=[1.,0.2,0.2,1], nTiles = 8)
 34graphicscube2 = graphics.Lines([[-L,-0.5*b,0], [n*L*2**0.5,-0.5*b,0], [n*L*2**0.5,0.5*b,0], [-L,0.5*b,0], [-L,-0.5*b,0]], color=graphics.color.steelblue) #graphics.Sphere(point=[0,0,0], radius=r, color=[1.,0.2,0.2,1], nTiles = 8)
 35#add ground object and mass point:
 36
 37pi = 3.1415926535897932384626
 38
 39#prescribed driving function:
 40def springForceUF(mbs, t, itemIndex, u, v, k, d, offset): #changed 2023-01-21:, mu, muPropZone):
 41    f=k*(u+offset)+v*d
 42    return f
 43
 44addPrismaticJoint = True
 45useCartesianSD = True
 46
 47simulationSettings = exu.SimulationSettings()
 48
 49f = 500
 50simulationSettings.timeIntegration.numberOfSteps = int(1*f)
 51simulationSettings.timeIntegration.endTime = 0.02*f #make small steps to see something during simulation
 52simulationSettings.solution.file.writePeriod = simulationSettings.timeIntegration.endTime/5000
 53
 54simulationSettings.solution.file.write = (not testIsActive) #only the SolutionViewer reads it (#2492)
 55simulationSettings.show.computationTime = True
 56simulationSettings.timeIntegration.verboseMode = 1
 57#simulationSettings.timeIntegration.verboseModeFile = 0
 58
 59simulationSettings.timeIntegration.newton.useModifiedNewton = False
 60simulationSettings.timeIntegration.newton.modifiedNewtonJacUpdatePerStep = True
 61
 62#added JacobianODE2, but example computed with numDiff forODE2Connectors, 2022-01-18: 27.202556489044145 :
 63simulationSettings.timeIntegration.newton.numericalDifferentiation.forODE2Connectors=True 
 64
 65simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
 66simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = simulationSettings.timeIntegration.generalizedAlpha.useNewmark
 67simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.61
 68simulationSettings.timeIntegration.adaptiveStep = False
 69simulationSettings.linearSolver.solverType = exu.LinearSolverType.EigenSparse
 70
 71simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = True
 72simulationSettings.solution.file.name= "solution/coordinatesSolution.txt"
 73
 74
 75simulationSettings.show.computationTime = False
 76simulationSettings.show.statistics = True
 77
 78
 79if not testIsActive: #only start graphics once, but after background is set
 80#    SC.visualizationSettings.view0.window.alwaysOnTop = True #must be done before SC.renderer.Start() called
 81#    SC.visualizationSettings.view0.window.maximize = True
 82#    SC.visualizationSettings.view0.window.showWindow = False
 83    SC.renderer.Start()
 84
 85
 86
 87resUy = 0 #add up displacements of selected node
 88resIt = 0 #total iterations
 89nMeasure = 0 #selected node
 90#treat two cases: 0=revolute, 1=ObjectConnectorCartesianSpringDamper
 91for case in range(2):
 92    mbs.Reset()
 93    oGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0], visualization = VObjectGround(graphicsData = [graphicsBackground])))
 94    mGround = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition = [0,0,0]))
 95    #start 3D visualization
 96
 97    lastMarkerV = mGround
 98    lastMarkerH = mGround
 99
100    useCartesianSD = True
101    if case == 0: useCartesianSD = False
102    oBodyD = 0
103    mBodyDCOM = 0
104    
105    #create several scissor elements if wanted
106    for i in range(n):
107        #stiffness and damping for CartesianSpringDamper
108        k=1e4
109        d=1e-2*k
110    
111        #horizontal body:
112        nBodyH = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=[L*i,L*i,0]))
113        oBodyH = mbs.AddObject(RigidBody2D(mass = mass, inertia=mass, nodeNumber = nBodyH, visualization = VObjectRigidBody2D(graphicsData = [graphicscube])))
114    
115        mBodyH0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oBodyH, localPosition=[-L,0,0]))
116        mBodyH1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oBodyH, localPosition=[ L,0,0]))
117        mBodyHCOM = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oBodyH, localPosition=[ 0,0,0]))
118        
119        #vertical body:
120        nBodyV = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=[L*i,L*i,0.5*pi]))
121        oBodyV = mbs.AddObject(RigidBody2D(mass = mass, inertia=mass, nodeNumber = nBodyV, visualization = VObjectRigidBody2D(graphicsData = [graphicscube])))
122        nMeasure = nBodyV
123        
124        mBodyV0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oBodyV, localPosition=[-L,0,0]))
125        mBodyV1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oBodyV, localPosition=[ L,0,0]))
126        mBodyVCOM = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oBodyV, localPosition=[ 0,0,0]))
127    
128        #diagonal body:
129        if i==0 and addPrismaticJoint:
130            nBodyD = mbs.AddNode(NodeRigidBody2D(referenceCoordinates=[0,0,0.25*pi]))
131            oBodyD = mbs.AddObject(RigidBody2D(mass = mass, inertia=mass, nodeNumber = nBodyD, visualization = VObjectRigidBody2D(graphicsData = [graphicscube2])))
132    
133            #mBodyD0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oBodyD, localPosition=[-L,0,0]))
134            #mBodyD1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oBodyD, localPosition=[ L,0,0]))
135            mBodyDCOM = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oBodyD, localPosition=[ 0,0,0]))
136            mbs.AddLoad(Force(markerNumber = mBodyDCOM, loadVector = [0, -mass*g, 0])) 
137            #keep this as Cartesian spring damper, as revolute joint may overconstrain system?
138            mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mBodyDCOM, mGround], stiffness = [k, k, k], damping=[d,d,d]))
139    
140        if addPrismaticJoint and i>0:
141            mBodyDact = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oBodyD, localPosition=[ i*L*2**0.5,0,0]))
142            mbs.AddObject(PrismaticJoint2D(markerNumbers=[mBodyVCOM, mBodyDact], axisMarker0=[1,0,0], normalMarker1=[0,1,0], constrainRotation=False))
143    
144        
145        if i==0:
146            if useCartesianSD:
147                mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mBodyHCOM, mGround], stiffness = [k, k, k], damping=[d,d,d]))
148                mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mBodyVCOM, mGround], stiffness = [k, k, k], damping=[d,d,d]))
149            else:
150                mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBodyHCOM, mGround]))
151                mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBodyVCOM, mGround]))
152    
153            #fix rotation of H-body
154            nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[L,0,0]))
155            mCoordGround = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nGround, coordinate=0)) #ref node
156            mCoordPhiH = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nBodyH, coordinate=2)) #rotation
157            mbs.AddObject(CoordinateConstraint(markerNumbers=[mCoordGround, mCoordPhiH]))
158    
159            #activate rotation of V-body
160            mCoordPhiV = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nBodyV, coordinate=2)) #rotation
161            mbs.AddObject(ObjectConnectorCoordinateSpringDamper(markerNumbers=[mCoordGround, mCoordPhiV], stiffness=1e4, damping=10e3, 
162            offset=0.25*pi,springForceUserFunction=springForceUF))
163    
164        else:
165            if useCartesianSD:
166                mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mBodyHCOM, mBodyVCOM], stiffness = [k, k, k], damping=[d,d,d]))
167                mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mBodyH0, lastMarkerV], stiffness = [k, k, k], damping=[d,d,d]))
168                mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers=[mBodyV0, lastMarkerH], stiffness = [k, k, k], damping=[d,d,d]))
169            else:
170                mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBodyHCOM, mBodyVCOM]))
171                mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBodyH0, lastMarkerV]))
172                mbs.AddObject(RevoluteJoint2D(markerNumbers=[mBodyV0, lastMarkerH]))
173    
174        lastMarkerH = mBodyH1
175        lastMarkerV = mBodyV1
176        
177        mbs.AddLoad(Force(markerNumber = mBodyHCOM, loadVector = [0, -mass*g, 0])) 
178        mbs.AddLoad(Force(markerNumber = mBodyVCOM, loadVector = [0, -mass*g, 0])) 
179
180    #exu.Print(mbs)
181    mbs.Assemble()
182    
183    if not testIsActive:
184        SC.renderer.ZoomAll()
185        SC.renderer.DoIdleTasks()
186    #solve
187    solver = exu.MainSolverImplicitSecondOrder()
188    solver.SolveSystem(mbs, simulationSettings)
189    #exu.Print("jac=",solver.GetSystemJacobian())
190    #exu.Print(solver.conv)
191    #exu.Print(solver.it)
192    uy=mbs.GetNodeOutput(nMeasure,exu.OutputVariableType.Position)[1] #y-coordinate of node point
193    exu.Print("uy=",uy)
194    nit = solver.it.newtonStepsCount
195    exu.Print("solver.it.newtonStepsCount=",nit)
196    resUy += uy #add up displacements of selected node
197    resIt += nit #total iterations
198#    SC.renderer.DoIdleTasks()
199    
200    #alternative solver command
201    #mbs.SolveDynamic(simulationSettings)
202
203
204
205#stop 3D visualization
206if not testIsActive:
207    SC.renderer.DoIdleTasks()
208    SC.renderer.Stop() #safely close rendering window!
209
210#factor 1e-2: 32bit version shows larger differences ...
211exu.sys['testResult'] = 1e-2*(resUy + resIt)
212#+++++++++++++++++++++++++++++++++++
213#plot data:
214
215#if simulationSettings.solution.file.write:
216#    import matplotlib.pyplot as plt
217#    import matplotlib.ticker as ticker
218
219#    data = np.loadtxt('coordinatesSolution.txt', comments='#', delimiter=',')
220#    plt.plot(data[:,0], data[:,1+2*nODE2+1], 'b-')
221#    #plt.plot(data[:,0], data[:,1+1], 'r-') #y-coordinate
222
223#    ax=plt.gca() # get current axes
224#    ax.grid(True, 'major', 'both')
225#    ax.xaxis.set_major_locator(ticker.MaxNLocator(10)) 
226#    ax.yaxis.set_major_locator(ticker.MaxNLocator(10)) 
227#    plt.tight_layout()
228#    plt.show()