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