mecanumWheelRollingDiscTest.py
You can view and download this file on Github: mecanumWheelRollingDiscTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: mecanum wheels modeled by ObjectConnectorRollingDiscPenalty
5# specific friction angle of rolling disc is used to model rolls of mecanum wheels
6# formulation is still under development and needs more testing
7#
8# Author: Johannes Gerstmayr
9# Date: 2020-06-19
10#
11# 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.
12#
13#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
15import exudyn as exu
16from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
17import exudyn.graphics as graphics #only import if it does not conflict
18
19import numpy as np
20
21useGraphics = True #without test
22#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
24try: #only if called from test suite
25 from modelUnitTests import exudynTestGlobals #for globally storing test results
26 useGraphics = exudynTestGlobals.useGraphics
27except:
28 class ExudynTestGlobals:
29 pass
30 exudynTestGlobals = ExudynTestGlobals()
31#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32
33SC = exu.SystemContainer()
34mbs = SC.AddSystem()
35
36g = [0,0,-9.81] #gravity in m/s^2
37
38doBreaking = False
39
40#++++++++++++++++++++++++++++++
41#wheel parameters:
42rhoWheel = 500 #density kg/m^3
43rWheel = 0.4 #radius of disc in m
44wWheel = 0.2 #width of disc in m, just for drawing
45p0Wheel = [0,0,rWheel] #origin of disc center point at reference, such that initial contact point is at [0,0,0]
46initialRotationCar = RotationMatrixZ(0)
47
48v0 = -5*0 #initial car velocity in y-direction
49omega0Wheel = [v0/rWheel,0,0] #initial angular velocity around z-axis
50
51#v0 = [0,0,0] #initial translational velocity
52#exu.Print("v0Car=",v0)
53
54#++++++++++++++++++++++++++++++
55#car parameters:
56p0Car = [0,0,rWheel] #origin of disc center point at reference, such that initial contact point is at [0,0,0]
57lCar = 3 #y-direction
58wCar = 3 #x-direction
59hCar = rWheel #z-direction
60mCar = 500
61omega0Car = [0,0,0] #initial angular velocity around z-axis
62v0Car = [0,-v0,0] #initial velocity of car center point
63
64#inertia for infinitely small ring:
65inertiaWheel = InertiaCylinder(density=rhoWheel, length=wWheel, outerRadius=rWheel, axis=0)
66#exu.Print(inertiaWheel)
67
68inertiaCar = InertiaCuboid(density=mCar/(lCar*wCar*hCar),sideLengths=[wCar, lCar, hCar])
69#exu.Print(inertiaCar)
70
71
72graphicsCar = graphics.Brick(centerPoint=[0,0,0],size=[wCar-1.1*wWheel, lCar, hCar],
73 color=graphics.color.steelblue)
74#create car body:
75dictCar = mbs.CreateRigidBody(referencePosition=p0Car,
76 referenceRotationMatrix=initialRotationCar,
77 initialVelocity=v0Car,
78 initialAngularVelocity=omega0Car,
79 inertia=inertiaCar,
80 gravity=g,
81 graphicsDataList=[graphicsCar],
82 returnDict=True)
83[nCar, bCar] = [dictCar['nodeNumber'], dictCar['bodyNumber']]
84
85nWheels = 4
86markerWheels=[]
87markerCarAxles=[]
88oRollingDiscs=[]
89sAngularVelWheels=[]
90
91# car setup:
92# ^Y, lCar
93# | W2 +---+ W3
94# | | |
95# | | + | car center point
96# | | |
97# | W0 +---+ W1
98# +---->X, wCar
99
100#ground body and marker
101gGround = graphics.Brick(centerPoint=[4,4,-0.001],size=[12,12,0.002], color=graphics.color.lightgrey[0:3]+[0.2])
102oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
103markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
104
105if useGraphics:
106 sCarVel = mbs.AddSensor(SensorBody(bodyNumber=bCar, storeInternal=True, #fileName='solution/rollingDiscCarVel.txt',
107 outputVariableType = exu.OutputVariableType.Velocity))
108
109sPos=[]
110sTrail=[]
111sForce=[]
112
113
114for iWheel in range(nWheels):
115 frictionAngle = 0.25*np.pi #45°
116 if iWheel == 0 or iWheel == 3: #difference in diagonal
117 frictionAngle *= -1
118
119 #additional graphics for visualization of rotation (JUST FOR DRAWING!):
120 graphicsWheel = [graphics.Brick(centerPoint=[0,0,0],size=[wWheel*1.1,0.7*rWheel,0.7*rWheel], color=graphics.color.lightred)]
121 nCyl = 12
122 rCyl = 0.1*rWheel
123 for i in range(nCyl): #draw cylinders on wheels
124 iPhi = i/nCyl*2*np.pi
125 pAxis = np.array([0,rWheel*np.sin(iPhi),-rWheel*np.cos(iPhi)])
126 vAxis = [0.5*wWheel*np.cos(frictionAngle),0.5*wWheel*np.sin(frictionAngle),0]
127 vAxis2 = RotationMatrixX(iPhi)@vAxis
128 rColor = graphics.color.grey
129 if i >= nCyl/2: rColor = graphics.color.darkgrey
130 graphicsWheel += [graphics.Cylinder(pAxis=pAxis-vAxis2, vAxis=2*vAxis2, radius=rCyl,
131 color=rColor)]
132
133
134 dx = -0.5*wCar
135 dy = -0.5*lCar
136 if iWheel > 1: dy *= -1
137 if iWheel == 1 or iWheel == 3: dx *= -1
138
139 kRolling = 1e5
140 dRolling = kRolling*0.01
141
142 initialRotation = RotationMatrixZ(0)
143
144 #v0Wheel = Skew(omega0Wheel) @ initialRotationWheel @ [0,0,rWheel] #initial angular velocity of center point
145 v0Wheel = v0Car #approx.
146
147 pOff = [dx,dy,0]
148
149
150 #add wheel body
151 dict0 = mbs.CreateRigidBody(referencePosition=VAdd(p0Wheel,pOff),
152 referenceRotationMatrix=initialRotation,
153 initialVelocity=v0Wheel,
154 initialAngularVelocity=omega0Wheel,
155 inertia=inertiaWheel,
156 gravity=g,
157 graphicsDataList=graphicsWheel,
158 returnDict=True)
159 [n0, b0] = [dict0['nodeNumber'], dict0['bodyNumber']]
160
161 #markers for rigid body:
162 mWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0,0,0]))
163 markerWheels += [mWheel]
164
165 mCarAxle = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pOff))
166 markerCarAxles += [mCarAxle]
167
168 lockedAxis0 = 0
169 if doBreaking: lockedAxis0 = 1
170 #if iWheel==0 or iWheel==1: freeAxis = 1 #lock rotation
171 mbs.AddObject(GenericJoint(markerNumbers=[mWheel,mCarAxle],rotationMarker1=initialRotation,
172 constrainedAxes=[1,1,1,lockedAxis0,1,1])) #revolute joint for wheel
173
174 #does not work, because revolute joint does not accept off-axis
175 #kSuspension = 1e4
176 #dSuspension = kSuspension*0.01
177 #mbs.AddObject(CartesianSpringDamper(markerNumbers=[mWheel,mCarAxle],stiffness=[0,0,kSuspension],damping=[0,0,dSuspension]))
178
179 nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
180 oRolling = mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[markerGround, mWheel], nodeNumber = nGeneric,
181 discRadius=rWheel, dryFriction=[1.,0.], dryFrictionAngle=frictionAngle,
182 dryFrictionProportionalZone=1e-1,
183 rollingFrictionViscous=0.2*0,
184 contactStiffness=kRolling, contactDamping=dRolling,
185 visualization=VObjectConnectorRollingDiscPenalty(discWidth=wWheel, color=graphics.color.blue)))
186 oRollingDiscs += [oRolling]
187
188 strNum = str(iWheel)
189 sAngularVelWheels += [mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,#fileName='solution/rollingDiscAngVelLocal'+strNum+'.txt',
190 outputVariableType = exu.OutputVariableType.AngularVelocityLocal))]
191
192 if useGraphics:
193 sPos+=[mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,#fileName='solution/rollingDiscPos'+strNum+'.txt',
194 outputVariableType = exu.OutputVariableType.Position))]
195
196 sTrail+=[mbs.AddSensor(SensorObject(name='Trail'+strNum,objectNumber=oRolling, storeInternal=True,#fileName='solution/rollingDiscTrail'+strNum+'.txt',
197 outputVariableType = exu.OutputVariableType.Position))]
198
199 sForce+=[mbs.AddSensor(SensorObject(objectNumber=oRolling, storeInternal=True,#fileName='solution/rollingDiscForce'+strNum+'.txt',
200 outputVariableType = exu.OutputVariableType.ForceLocal))]
201
202
203torqueFactor = 100
204def UFBasicTorque(mbs, t, torque):
205 if t < 0.2:
206 return torque
207 else:
208 return [0,0,0]
209
210#takes as input the translational and angular velocity and outputs the velocities for all 4 wheels
211#wheel axis is mounted at x-axis; positive angVel rotates CCW in x/y plane viewed from top
212# car setup:
213# ^Y, lCar
214# | W2 +---+ W3
215# | | |
216# | | + | car center point
217# | | |
218# | W0 +---+ W1
219# +---->X, wCar
220#values given for wheel0/3: frictionAngle=-pi/4, wheel 1/2: frictionAngle=pi/4; dryFriction=[1,0] (looks in lateral (x) direction)
221#==>direction of axis of roll on ground of wheel0: [1,-1] and of wheel1: [1,1]
222def MecanumXYphi2WheelVelocities(xVel, yVel, angVel, R, Lx, Ly):
223 LxLy2 = (Lx+Ly)/2
224 mat = (1/R)*np.array([[ 1,-1, LxLy2],
225 [-1,-1,-LxLy2],
226 [-1,-1, LxLy2],
227 [ 1,-1,-LxLy2]])
228 return mat @ [xVel, yVel, angVel]
229
230#compute velocity trajectory
231def ComputeVelocity(t):
232 vel = [0,0,0] #vx, vy, angVel; these are the local velocities!!!
233 f=1
234 if t < 4:
235 vel = [f,0,0]
236 elif t < 8:
237 vel = [0,f,0]
238 elif t < 16:
239 vel = [0,0,0.125*np.pi]
240 elif t < 20:
241 vel = [f,0,0]
242 return vel
243
244pControl = 500
245#compute controlled torque; torque[0] contains wheel number
246def UFtorque(mbs, t, torque):
247 iWheel = int(torque[0]) #wheel number
248
249 v = ComputeVelocity(t) #desired velocity
250 vDesired = MecanumXYphi2WheelVelocities(v[0],v[1],v[2],rWheel,wCar,lCar)[iWheel]
251 vCurrent = mbs.GetSensorValues(sAngularVelWheels[iWheel])[0] #local x-axis = wheel axis
252
253 cTorque = pControl*(vDesired-vCurrent)
254 #print("W",iWheel, ": vDes=", vDesired, ", vCur=", vCurrent, ", torque=", cTorque)
255
256 return [cTorque,0,0]
257
258if False:
259 mbs.AddLoad(Torque(markerNumber=markerWheels[0],loadVector=[ torqueFactor,0,0], bodyFixed = True, loadVectorUserFunction=UFBasicTorque))
260 mbs.AddLoad(Torque(markerNumber=markerWheels[1],loadVector=[-torqueFactor,0,0], bodyFixed = True, loadVectorUserFunction=UFBasicTorque))
261 mbs.AddLoad(Torque(markerNumber=markerWheels[2],loadVector=[-torqueFactor,0,0], bodyFixed = True, loadVectorUserFunction=UFBasicTorque))
262 mbs.AddLoad(Torque(markerNumber=markerWheels[3],loadVector=[ torqueFactor,0,0], bodyFixed = True, loadVectorUserFunction=UFBasicTorque))
263
264if True:
265 for i in range(4):
266 mbs.AddLoad(Torque(markerNumber=markerWheels[i],loadVector=[ i,0,0], bodyFixed = True, loadVectorUserFunction=UFtorque))
267
268#mbs.AddSensor(SensorObject(objectNumber=oRolling, fileName='solution/rollingDiscTrailVel.txt',
269# outputVariableType = exu.OutputVariableType.VelocityLocal))
270
271
272mbs.Assemble()
273
274simulationSettings = exu.SimulationSettings() #takes currently set values or default values
275
276tEnd = 0.5
277if useGraphics:
278 tEnd = 0.5 #24
279
280h=0.002
281
282simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
283simulationSettings.timeIntegration.endTime = tEnd
284#simulationSettings.solutionSettings.solutionWritePeriod = 0.01
285simulationSettings.solutionSettings.sensorsWritePeriod = 0.002
286simulationSettings.timeIntegration.verboseMode = 0
287simulationSettings.displayComputationTime = False
288simulationSettings.displayStatistics = False
289
290simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
291simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
292simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5#0.5
293simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
294
295simulationSettings.timeIntegration.newton.useModifiedNewton = True
296simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
297simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
298
299SC.visualizationSettings.nodes.show = True
300SC.visualizationSettings.nodes.drawNodesAsPoint = False
301SC.visualizationSettings.nodes.showBasis = True
302SC.visualizationSettings.nodes.basisSize = 0.015
303
304#create animation:
305if useGraphics:
306 SC.visualizationSettings.window.renderWindowSize=[1920,1080]
307 SC.visualizationSettings.openGL.multiSampling = 4
308 if False:
309 simulationSettings.solutionSettings.recordImagesInterval = 0.05
310 SC.visualizationSettings.exportImages.saveImageFileName = "animation/frame"
311
312if useGraphics:
313 exu.StartRenderer()
314 mbs.WaitForUserToContinue()
315
316mbs.SolveDynamic(simulationSettings)
317
318p0=mbs.GetObjectOutputBody(bCar, exu.OutputVariableType.Position, localPosition=[0,0,0])
319exu.Print('solution of mecanumWheelRollingDiscTest=',p0[0]) #use x-coordinate
320
321exudynTestGlobals.testError = p0[0] - (0.2714267238324345) #2020-06-20: 0.2714267238324345
322exudynTestGlobals.testResult = p0[0]
323
324
325if useGraphics:
326 SC.WaitForRenderEngineStopFlag()
327 exu.StopRenderer() #safely close rendering window!
328
329##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
330#plot results
331if useGraphics:
332
333
334 mbs.PlotSensor(sTrail, componentsX=[0]*4, components=[1]*4, title='wheel trails', closeAll=True,
335 markerStyles=['x ','o ','^ ','D '], markerSizes=12)
336 mbs.PlotSensor(sForce, components=[1]*4, title='wheel forces')