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