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