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