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