carRollingDiscTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  car with wheels modeled by ObjectConnectorRollingDiscPenalty
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2020-06-19
  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
 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
 30SC = exu.SystemContainer()
 31mbs = SC.AddSystem()
 32
 33g = [0,0,-9.81]     #gravity in m/s^2
 34
 35doBreaking = False
 36
 37#++++++++++++++++++++++++++++++
 38#wheel parameters:
 39rhoWheel = 500      #density kg/m^3
 40rWheel = 0.4            #radius of disc in m
 41wWheel = 0.1             #width of disc in m, just for drawing
 42p0Wheel = [0,0,rWheel]        #origin of disc center point at reference, such that initial contact point is at [0,0,0]
 43initialRotationCar = RotationMatrixZ(0)
 44
 45v0 = -5*0 #initial car velocity in y-direction
 46omega0Wheel = [v0/rWheel,0,0]                   #initial angular velocity around z-axis
 47
 48#v0 = [0,0,0]                                   #initial translational velocity
 49#print("v0Car=",v0)
 50
 51#%%++++++++++++++++++++++++++++++
 52#car parameters and inertia:
 53p0Car = [0,0,rWheel]        #origin of disc center point at reference, such that initial contact point is at [0,0,0]
 54lCar = 3
 55wCar = 2
 56hCar = rWheel
 57mCar = 500
 58omega0Car = [0,0,0]                   #initial angular velocity around z-axis
 59v0Car = [0,-v0,0]                  #initial velocity of car center point
 60
 61#inertia for infinitely small ring:
 62inertiaWheel = InertiaCylinder(density=rhoWheel, length=wWheel, outerRadius=rWheel, axis=0)
 63#exu.Print(inertiaWheel)
 64
 65inertiaCar = InertiaCuboid(density=mCar/(lCar*wCar*hCar),sideLengths=[wCar, lCar, hCar])
 66#exu.Print(inertiaCar)
 67
 68#%%++++++++++++++++++++++++++++++
 69#create car node and body:
 70graphicsCar = graphics.Brick(centerPoint=[0,0,0],size=[wCar-1.1*wWheel, lCar, hCar], color=graphics.color.lightred)
 71bCar=mbs.CreateRigidBody(inertia = inertiaCar,
 72                         referencePosition = p0Car,
 73                         referenceRotationMatrix = initialRotationCar,
 74                         initialAngularVelocity = omega0Car,
 75                         initialVelocity = v0Car,
 76                         gravity = g,
 77                         graphicsDataList = [graphicsCar])
 78
 79nCar = mbs.GetObject(bCar)['nodeNumber']
 80nWheels = 4
 81markerWheels=[]
 82markerCarAxles=[]
 83oRollingDiscs=[]
 84
 85# car setup:
 86# ^Y, lCar
 87# | W2 +---+ W3
 88# |    |   |
 89# |    | + | car center point
 90# |    |   |
 91# | W0 +---+ W1
 92# +---->X, wCar
 93
 94#ground body and marker
 95gGround = graphics.Brick(centerPoint=[0,0,-0.001],size=[30,30,0.002], color=graphics.color.lightgrey)
 96oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
 97markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
 98
 99sCarVel = mbs.AddSensor(SensorBody(bodyNumber=bCar, #fileName='solution/rollingDiscCarVel.txt',
100                                   storeInternal=True,
101                                   outputVariableType = exu.OutputVariableType.Velocity))
102
103sAngVels=[]
104sWheelPos=[]
105sRollPos=[]
106sRollForce=[]
107
108#%%++++++++++++++++++++++++++++++
109#create wheels bodies and nodes:
110for iWheel in range(nWheels):
111    #additional graphics for visualization of rotation:
112    graphicsWheel = graphics.Brick(centerPoint=[0,0,0],size=[wWheel*1.1,0.7*rWheel,0.7*rWheel], color=graphics.color.lightred)
113
114    dx = -0.5*wCar
115    dy = -0.5*lCar
116    if iWheel > 1: dy *= -1
117    if iWheel == 1 or iWheel == 3: dx *= -1
118
119    kRolling = 1e5
120    dRolling = kRolling*0.01
121
122    rSteering = 5
123    phiZwheelLeft = 0
124    phiZwheelRight = 0
125    if rSteering != 0:
126        phiZwheelLeft = np.arctan(lCar/rSteering) #5/180*np.pi   #steering angle
127        phiZwheelRight = np.arctan(lCar/(wCar+rSteering)) #5/180*np.pi   #steering angle
128
129    initialRotationWheelLeft = RotationMatrixZ(phiZwheelLeft)
130    initialRotationWheelRight = RotationMatrixZ(phiZwheelRight)
131
132    initialRotation = RotationMatrixZ(0)
133    if iWheel == 2:
134        initialRotation = initialRotationWheelLeft
135    if iWheel == 3:
136        initialRotation = initialRotationWheelRight
137
138    #v0Wheel = Skew(omega0Wheel) @ initialRotationWheel @ [0,0,rWheel]   #initial angular velocity of center point
139    v0Wheel = v0Car #approx.
140
141    pOff = [dx,dy,0]
142
143
144    #add wheel body
145    b0 = mbs.CreateRigidBody(inertia = inertiaWheel,
146                             referencePosition = VAdd(p0Wheel,pOff),
147                             referenceRotationMatrix = initialRotation, #np.diag([1,1,1]),
148                             initialAngularVelocity = omega0Wheel,
149                             initialVelocity = v0Wheel,
150                             gravity = g,
151                             graphicsDataList = [graphicsWheel])
152
153    n0 = mbs.GetObject(b0)['nodeNumber']
154
155    #markers for rigid body:
156    mWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0,0,0]))
157    markerWheels += [mWheel]
158
159    mCarAxle = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pOff))
160    markerCarAxles += [mCarAxle]
161
162    lockedAxis0 = 0
163    if doBreaking: lockedAxis0 = 1
164    #if iWheel==0 or iWheel==1: freeAxis = 1 #lock rotation
165    mbs.AddObject(GenericJoint(markerNumbers=[mWheel,mCarAxle],rotationMarker1=initialRotation,
166                               constrainedAxes=[1,1,1,lockedAxis0,1,1])) #revolute joint for wheel
167
168    nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
169    oRolling = mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[markerGround, mWheel], nodeNumber = nGeneric,
170                                                  discRadius=rWheel, dryFriction=[0.4,0.4],
171                                                  dryFrictionProportionalZone=1e-1,
172                                                  rollingFrictionViscous=0.2*0,
173                                                  contactStiffness=kRolling, contactDamping=dRolling,
174                                                  visualization=VObjectConnectorRollingDiscPenalty(discWidth=wWheel, color=graphics.color.blue)))
175    oRollingDiscs += [oRolling]
176
177    strNum = str(iWheel)
178    if useGraphics:
179        sAngVels+=[mbs.AddSensor(SensorBody(bodyNumber=b0, #fileName='solution/rollingDiscAngVelLocal'+strNum+'.txt',
180                                 storeInternal=True,
181                                 outputVariableType = exu.OutputVariableType.AngularVelocityLocal))]
182
183        sWheelPos+=[mbs.AddSensor(SensorBody(bodyNumber=b0, #fileName='solution/rollingDiscPos'+strNum+'.txt',
184                                     storeInternal=True,
185                                     outputVariableType = exu.OutputVariableType.Position))]
186
187        sRollPos+=[mbs.AddSensor(SensorObject(objectNumber=oRolling, #fileName='solution/rollingDiscTrail'+strNum+'.txt',
188                                            storeInternal=True,
189                                            outputVariableType = exu.OutputVariableType.Position))]
190
191        sRollForce+=[mbs.AddSensor(SensorObject(name='wheelForce'+strNum,objectNumber=oRolling, #fileName='solution/rollingDiscForce'+strNum+'.txt',
192                                               storeInternal=True,
193                                               outputVariableType = exu.OutputVariableType.ForceLocal))]
194
195
196#user function for time-dependent torque on two wheels 0,1
197def UFtorque(mbs, t, torque):
198    if t < 4:
199        return torque
200    else:
201        return [0,0,0]
202
203mbs.AddLoad(Torque(markerNumber=markerWheels[0],loadVector=[-200,0,0], bodyFixed = True, loadVectorUserFunction=UFtorque))
204mbs.AddLoad(Torque(markerNumber=markerWheels[1],loadVector=[-200,0,0], bodyFixed = True, loadVectorUserFunction=UFtorque))
205
206
207mbs.Assemble()
208
209simulationSettings = exu.SimulationSettings() #takes currently set values or default values
210
211tEnd = 0.5 #40#1.2
212h=0.002 #no visual differences for step sizes smaller than 0.0005
213
214if useGraphics:
215    tEnd = 4
216    exu.StartRenderer()
217    mbs.WaitForUserToContinue()
218
219simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
220simulationSettings.timeIntegration.endTime = tEnd
221simulationSettings.timeIntegration.verboseMode = 1
222
223
224#simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
225#simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
226
227SC.visualizationSettings.nodes.show = True
228SC.visualizationSettings.nodes.drawNodesAsPoint  = False
229SC.visualizationSettings.nodes.showBasis = True
230SC.visualizationSettings.nodes.basisSize = 0.015
231
232mbs.SolveDynamic(simulationSettings, solverType=exu.DynamicSolverType.TrapezoidalIndex2)
233
234if useGraphics:
235    SC.WaitForRenderEngineStopFlag()
236    exu.StopRenderer() #safely close rendering window!
237
238c=mbs.GetNodeOutput(n0, variableType=exu.OutputVariableType.Coordinates)
239u=sum(c)
240exu.Print("carRollingDiscTest u=",u)
241
242exudynTestGlobals.testError = u - (-0.23940048717113419) #2020-12-18: -0.23940048717113419
243exudynTestGlobals.testResult = u
244
245##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
246#plot results
247if useGraphics:
248
249
250    mbs.PlotSensor(sensorNumbers=sCarVel, components=[0,1,2], title='car velocitiy', closeAll=True)
251    for i in range(4):
252        mbs.PlotSensor(sensorNumbers=sRollPos[i], componentsX=0, components=1,
253                   labels='wheel trail '+str(i), newFigure=(i==0), colorCodeOffset=i)
254        #trail and wheel pos are almost same, just if car slightly tilts, there is a deviation
255        mbs.PlotSensor(sensorNumbers=sWheelPos[i], componentsX=0, components=1,
256                   labels='wheel pos '+str(i), newFigure=False, colorCodeOffset=i+7,
257                   lineStyles='', markerStyles='x')
258
259    mbs.PlotSensor(sensorNumbers=sRollForce, components=[2]*4, title='wheel contact forces')
260
261    mbs.PlotSensor(sensorNumbers=sRollForce*2, components=[0]*4+[1]*4, title='wheel lateral (X) and drive/acceleration (Y) forces')
262
263    mbs.PlotSensor(sensorNumbers=sAngVels, components=[0]*4, title='wheel local angular velocity')