laserScannerTest.py

You can view and download this file on Github: laserScannerTest.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Simple vehicle model with 'rotating' laser scanner
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2023-04-11
  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
 14import exudyn as exu
 15from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 16import exudyn.graphics as graphics #only import if it does not conflict
 17from exudyn.robotics.utilities import AddLidar
 18
 19import numpy as np
 20from math import sin, cos, tan
 21
 22useGraphics = True #without test
 23#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 24#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 25try: #only if called from test suite
 26    from modelUnitTests import exudynTestGlobals #for globally storing test results
 27    useGraphics = exudynTestGlobals.useGraphics
 28except:
 29    class ExudynTestGlobals:
 30        pass
 31    exudynTestGlobals = ExudynTestGlobals()
 32#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 33#useGraphics=False
 34
 35SC = exu.SystemContainer()
 36mbs = SC.AddSystem()
 37
 38g = [0,0,-9.81]     #gravity in m/s^2
 39
 40doBreaking = False
 41
 42#++++++++++++++++++++++++++++++
 43#wheel parameters:
 44rhoWheel = 500      #density kg/m^3
 45rWheel = 0.4            #radius of disc in m
 46wWheel = 0.2             #width of disc in m, just for drawing
 47p0Wheel = [0,0,rWheel]        #origin of disc center point at reference, such that initial contact point is at [0,0,0]
 48initialRotationCar = RotationMatrixZ(0)
 49
 50v0 = -5*0 #initial car velocity in y-direction
 51omega0Wheel = [v0/rWheel,0,0]                   #initial angular velocity around z-axis
 52
 53#v0 = [0,0,0]                                   #initial translational velocity
 54#exu.Print("v0Car=",v0)
 55
 56#++++++++++++++++++++++++++++++
 57#car parameters:
 58p0Car = [0,0,rWheel]    #origin of disc center point at reference, such that initial contact point is at [0,0,0]
 59lCar = 3                #y-direction
 60wCar = 3                #x-direction
 61hCar = rWheel           #z-direction
 62mCar = 500
 63omega0Car = [0,0,0]                   #initial angular velocity around z-axis
 64v0Car = [0,-v0,0]                  #initial velocity of car center point
 65
 66#inertia for infinitely small ring:
 67inertiaWheel = InertiaCylinder(density=rhoWheel, length=wWheel, outerRadius=rWheel, axis=0)
 68#exu.Print(inertiaWheel)
 69
 70inertiaCar = InertiaCuboid(density=mCar/(lCar*wCar*hCar),sideLengths=[wCar, lCar, hCar])
 71#exu.Print(inertiaCar)
 72#
 73rLidar = 0.5*rWheel
 74pLidar1 = [-wCar*0.5-rLidar, lCar*0.5+rWheel+rLidar,hCar*0.5]
 75pLidar2 = [ wCar*0.5+rLidar,-lCar*0.5-rWheel-rLidar,hCar*0.5]
 76graphicsCar = [graphics.Brick(centerPoint=[0,0,0],size=[wCar-1.1*wWheel, lCar+2*rWheel, hCar],
 77                                         color=graphics.color.steelblue)]
 78graphicsCar += [graphics.Cylinder(pAxis=pLidar1, vAxis=[0,0,0.5*rLidar], radius=rLidar, clor=graphics.color.darkgrey)]
 79graphicsCar += [graphics.Cylinder(pAxis=pLidar2, vAxis=[0,0,0.5*rLidar], radius=rLidar, clor=graphics.color.darkgrey)]
 80
 81#create car body:
 82dictCar = mbs.CreateRigidBody(referencePosition=p0Car,
 83                              referenceRotationMatrix=initialRotationCar,
 84                              initialVelocity=v0Car,
 85                              initialAngularVelocity=omega0Car,
 86                              inertia=inertiaCar,
 87                              gravity=g,
 88                              graphicsDataList=graphicsCar,
 89                              returnDict=True)
 90[nCar, bCar] = [dictCar['nodeNumber'], dictCar['bodyNumber']]
 91
 92markerCar = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=[0,0,hCar*0.5]))
 93
 94
 95markerCar1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pLidar1))
 96markerCar2 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pLidar2))
 97
 98
 99nWheels = 4
100markerWheels=[]
101markerCarAxles=[]
102oRollingDiscs=[]
103sAngularVelWheels=[]
104
105# car setup:
106# ^Y, lCar
107# | W2 +---+ W3
108# |    |   |
109# |    | + | car center point
110# |    |   |
111# | W0 +---+ W1
112# +---->X, wCar
113
114#ground body and marker
115LL = 8
116gGround = graphics.CheckerBoard(point=[0.25*LL,0.25*LL,0],size=2*LL)
117
118#obstacles:
119zz=1
120gGround = graphics.MergeTriangleLists(graphics.Brick(centerPoint=[0,8,0.5*zz],size=[2*zz,zz,1*zz], color=graphics.color.dodgerblue), gGround)
121gGround = graphics.MergeTriangleLists(graphics.Brick(centerPoint=[8,6,1.5*zz],size=[zz,2*zz,3*zz], color=graphics.color.dodgerblue), gGround)
122gGround = graphics.MergeTriangleLists(graphics.Brick(centerPoint=[4,-4,0.5*zz],size=[2*zz,zz,1*zz], color=graphics.color.dodgerblue), gGround)
123gGround = graphics.MergeTriangleLists(graphics.Cylinder(pAxis=[8,0,0],vAxis=[0,0,zz], radius=1.5, color=graphics.color.dodgerblue, nTiles=64), gGround)
124
125#walls:
126tt=0.2
127gGround = graphics.MergeTriangleLists(graphics.Brick(centerPoint=[0.25*LL,0.25*LL-LL,0.5*zz],size=[2*LL,tt,zz], color=graphics.color.dodgerblue), gGround)
128gGround = graphics.MergeTriangleLists(graphics.Brick(centerPoint=[0.25*LL,0.25*LL+LL,0.5*zz],size=[2*LL,tt,zz], color=graphics.color.dodgerblue), gGround)
129gGround = graphics.MergeTriangleLists(graphics.Brick(centerPoint=[0.25*LL-LL,0.25*LL,0.5*zz],size=[tt,2*LL,zz], color=graphics.color.dodgerblue), gGround)
130gGround = graphics.MergeTriangleLists(graphics.Brick(centerPoint=[0.25*LL+LL,0.25*LL,0.5*zz],size=[tt,2*LL,zz], color=graphics.color.dodgerblue), gGround)
131
132
133oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
134mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
135
136
137#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
138#set up general contact geometry where sensors measure
139[meshPoints, meshTrigs] = graphics.ToPointsAndTrigs(gGround)
140
141ngc = mbs.CreateDistanceSensorGeometry(meshPoints, meshTrigs, rigidBodyMarkerIndex=mGround, searchTreeCellSize=[8,8,1])
142
143#single sensor:
144# sDistanceSphere = mbs.CreateDistanceSensor(ngc, positionOrMarker=markerCar2, dirSensor=dirSensor2,
145#                                     minDistance=0, maxDistance=maxDistance, measureVelocity=True,
146#                                     cylinderRadius=0, storeInternal=True, addGraphicsObject=True,
147#                                     selectedTypeIndex=exu.ContactTypeIndex.IndexTrigsRigidBodyBased,
148#                                     color=graphics.color.red)
149
150maxDistance = 100 #max. distance of sensors; just large enough to reach everything; take care, in zoom all it will show this large area
151
152#note that lidar sensors seem to be drawn wrong in the initialization; however, this is because the initial distance
153#  is zero which means that the sensor is drawn into the negative direction during initialization!!!
154sLidar = AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar2, minDistance=0, maxDistance=maxDistance,
155          numberOfSensors=100,angleStart=1.*pi, angleEnd=2.5*pi, inclination=0,
156          lineLength=1, storeInternal=True, color=graphics.color.lawngreen )
157
158AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar2, minDistance=0, maxDistance=maxDistance,
159          numberOfSensors=100,angleStart=1.*pi, angleEnd=2.5*pi, inclination=-4/180*pi,
160          lineLength=1, storeInternal=True, color=graphics.color.grey )
161
162sLidarInc = AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar2, minDistance=0, maxDistance=maxDistance,
163          numberOfSensors=100,angleStart=1.*pi, angleEnd=2.5*pi, inclination= 4/180*pi,
164          lineLength=1, storeInternal=True, color=graphics.color.grey )
165
166AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar2, minDistance=0, maxDistance=maxDistance,
167          numberOfSensors=100,angleStart=1.*pi, angleEnd=2.5*pi, inclination= 8/180*pi,
168          lineLength=1, storeInternal=True, color=graphics.color.grey )
169
170AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar2, minDistance=0, maxDistance=maxDistance,
171          numberOfSensors=100,angleStart=1.*pi, angleEnd=2.5*pi, inclination=12/180*pi,
172          lineLength=1, storeInternal=True, color=graphics.color.grey )
173
174AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar1, minDistance=0, maxDistance=maxDistance,
175          numberOfSensors=100,angleStart=0*pi, angleEnd=1.5*pi,
176          lineLength=1, storeInternal=True, color=graphics.color.red) #, rotation=RotationMatrixX(2/180*pi))
177
178#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
179
180if useGraphics:
181    sCarVel = mbs.AddSensor(SensorBody(bodyNumber=bCar, storeInternal=True, #fileName='solution/rollingDiscCarVel.txt',
182                                outputVariableType = exu.OutputVariableType.Velocity))
183
184sPos=[]
185sTrail=[]
186sForce=[]
187
188
189for iWheel in range(nWheels):
190    frictionAngle = 0.25*np.pi #45°
191    if iWheel == 0 or iWheel == 3: #difference in diagonal
192        frictionAngle *= -1
193
194    #additional graphics for visualization of rotation (JUST FOR DRAWING!):
195    graphicsWheel = [graphics.Brick(centerPoint=[0,0,0],size=[wWheel*1.1,0.7*rWheel,0.7*rWheel], color=graphics.color.lightred)]
196    nCyl = 12
197    rCyl = 0.1*rWheel
198    for i in range(nCyl): #draw cylinders on wheels
199        iPhi = i/nCyl*2*np.pi
200        pAxis = np.array([0,rWheel*np.sin(iPhi),-rWheel*np.cos(iPhi)])
201        vAxis = [0.5*wWheel*np.cos(frictionAngle),0.5*wWheel*np.sin(frictionAngle),0]
202        vAxis2 = RotationMatrixX(iPhi)@vAxis
203        rColor = graphics.color.grey
204        if i >= nCyl/2: rColor = graphics.color.darkgrey
205        graphicsWheel += [graphics.Cylinder(pAxis=pAxis-vAxis2, vAxis=2*vAxis2, radius=rCyl,
206                                               color=rColor)]
207
208
209    dx = -0.5*wCar
210    dy = -0.5*lCar
211    if iWheel > 1: dy *= -1
212    if iWheel == 1 or iWheel == 3: dx *= -1
213
214    kRolling = 1e5
215    dRolling = kRolling*0.01
216
217    initialRotation = RotationMatrixZ(0)
218
219    #v0Wheel = Skew(omega0Wheel) @ initialRotationWheel @ [0,0,rWheel]   #initial angular velocity of center point
220    v0Wheel = v0Car #approx.
221
222    pOff = [dx,dy,0]
223
224
225    #add wheel body
226    dict0 = mbs.CreateRigidBody(referencePosition=VAdd(p0Wheel,pOff),
227                                referenceRotationMatrix=initialRotation,
228                                initialVelocity=v0Wheel,
229                                initialAngularVelocity=omega0Wheel,
230                                inertia=inertiaWheel,
231                                gravity=g,
232                                graphicsDataList=graphicsWheel,
233                                returnDict=True)
234    [n0, b0] = [dict0['nodeNumber'], dict0['bodyNumber']]
235
236    #markers for rigid body:
237    mWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0,0,0]))
238    markerWheels += [mWheel]
239
240    mCarAxle = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pOff))
241    markerCarAxles += [mCarAxle]
242
243    lockedAxis0 = 0
244    if doBreaking: lockedAxis0 = 1
245    #if iWheel==0 or iWheel==1: freeAxis = 1 #lock rotation
246    mbs.AddObject(GenericJoint(markerNumbers=[mWheel,mCarAxle],rotationMarker1=initialRotation,
247                               constrainedAxes=[1,1,1,lockedAxis0,1,1])) #revolute joint for wheel
248
249    #does not work, because revolute joint does not accept off-axis
250    #kSuspension = 1e4
251    #dSuspension = kSuspension*0.01
252    #mbs.AddObject(CartesianSpringDamper(markerNumbers=[mWheel,mCarAxle],stiffness=[0,0,kSuspension],damping=[0,0,dSuspension]))
253
254    nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
255    oRolling = mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[mGround, mWheel], nodeNumber = nGeneric,
256                                                  discRadius=rWheel, dryFriction=[1.,0.], dryFrictionAngle=frictionAngle,
257                                                  dryFrictionProportionalZone=1e-1,
258                                                  rollingFrictionViscous=0.2*0,
259                                                  contactStiffness=kRolling, contactDamping=dRolling,
260                                                  visualization=VObjectConnectorRollingDiscPenalty(discWidth=wWheel, color=graphics.color.blue)))
261    oRollingDiscs += [oRolling]
262
263    strNum = str(iWheel)
264    sAngularVelWheels += [mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,#fileName='solution/rollingDiscAngVelLocal'+strNum+'.txt',
265                               outputVariableType = exu.OutputVariableType.AngularVelocityLocal))]
266
267    if useGraphics:
268        sPos+=[mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,#fileName='solution/rollingDiscPos'+strNum+'.txt',
269                                   outputVariableType = exu.OutputVariableType.Position))]
270
271        sTrail+=[mbs.AddSensor(SensorObject(name='Trail'+strNum,objectNumber=oRolling, storeInternal=True,#fileName='solution/rollingDiscTrail'+strNum+'.txt',
272                                   outputVariableType = exu.OutputVariableType.Position))]
273
274        sForce+=[mbs.AddSensor(SensorObject(objectNumber=oRolling, storeInternal=True,#fileName='solution/rollingDiscForce'+strNum+'.txt',
275                                   outputVariableType = exu.OutputVariableType.ForceLocal))]
276
277
278torqueFactor = 100
279def UFBasicTorque(mbs, t, torque):
280    if t < 0.2:
281        return torque
282    else:
283        return [0,0,0]
284
285#takes as input the translational and angular velocity and outputs the velocities for all 4 wheels
286#wheel axis is mounted at x-axis; positive angVel rotates CCW in x/y plane viewed from top
287# car setup:
288# ^Y, lCar
289# | W2 +---+ W3
290# |    |   |
291# |    | + | car center point
292# |    |   |
293# | W0 +---+ W1
294# +---->X, wCar
295#values given for wheel0/3: frictionAngle=-pi/4, wheel 1/2: frictionAngle=pi/4; dryFriction=[1,0] (looks in lateral (x) direction)
296#==>direction of axis of roll on ground of wheel0: [1,-1] and of wheel1: [1,1]
297def MecanumXYphi2WheelVelocities(xVel, yVel, angVel, R, Lx, Ly):
298    LxLy2 = (Lx+Ly)/2
299    mat = (1/R)*np.array([[ 1,-1, LxLy2],
300                          [-1,-1,-LxLy2],
301                          [-1,-1, LxLy2],
302                          [ 1,-1,-LxLy2]])
303    return mat @ [xVel, yVel, angVel]
304
305#compute velocity trajectory
306def ComputeVelocity(t):
307    vel = [0,0,0] #vx, vy, angVel; these are the local velocities!!!
308    f=1
309    if t < 4:
310      vel = [f,0,0]
311    elif t < 8:
312      vel = [0,f,0]
313    elif t < 16:
314      vel = [0,0,0.125*np.pi]
315    elif t < 20:
316      vel = [f,0,0]
317    return vel
318
319pControl = 500
320#compute controlled torque; torque[0] contains wheel number
321def UFtorque(mbs, t, torque):
322    iWheel = int(torque[0]) #wheel number
323
324    v = ComputeVelocity(t) #desired velocity
325    vDesired = MecanumXYphi2WheelVelocities(v[0],v[1],v[2],rWheel,wCar,lCar)[iWheel]
326    vCurrent = mbs.GetSensorValues(sAngularVelWheels[iWheel])[0] #local x-axis = wheel axis
327
328    cTorque = pControl*(vDesired-vCurrent)
329    #print("W",iWheel, ": vDes=", vDesired, ", vCur=", vCurrent, ", torque=", cTorque)
330
331    return [cTorque,0,0]
332
333if False:
334    mbs.AddLoad(Torque(markerNumber=markerWheels[0],loadVector=[ torqueFactor,0,0], bodyFixed = True, loadVectorUserFunction=UFBasicTorque))
335    mbs.AddLoad(Torque(markerNumber=markerWheels[1],loadVector=[-torqueFactor,0,0], bodyFixed = True, loadVectorUserFunction=UFBasicTorque))
336    mbs.AddLoad(Torque(markerNumber=markerWheels[2],loadVector=[-torqueFactor,0,0], bodyFixed = True, loadVectorUserFunction=UFBasicTorque))
337    mbs.AddLoad(Torque(markerNumber=markerWheels[3],loadVector=[ torqueFactor,0,0], bodyFixed = True, loadVectorUserFunction=UFBasicTorque))
338
339if True:
340    for i in range(4):
341        mbs.AddLoad(Torque(markerNumber=markerWheels[i],loadVector=[ i,0,0], bodyFixed = True, loadVectorUserFunction=UFtorque))
342
343#mbs.AddSensor(SensorObject(objectNumber=oRolling, fileName='solution/rollingDiscTrailVel.txt',
344#                           outputVariableType = exu.OutputVariableType.VelocityLocal))
345
346
347# print('start')
348mbs.Assemble()
349# print('end')
350
351simulationSettings = exu.SimulationSettings() #takes currently set values or default values
352
353tEnd = 0.5
354if useGraphics:
355    tEnd = 20#24
356
357h=0.002
358
359simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
360simulationSettings.timeIntegration.endTime = tEnd
361#simulationSettings.solutionSettings.solutionWritePeriod = 0.01
362simulationSettings.solutionSettings.sensorsWritePeriod = 0.1
363simulationSettings.timeIntegration.verboseMode = 1
364simulationSettings.displayComputationTime = False
365simulationSettings.displayStatistics = False
366
367simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
368simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
369simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5#0.5
370simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
371
372simulationSettings.timeIntegration.newton.useModifiedNewton = True
373simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
374simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
375simulationSettings.linearSolverType=exu.LinearSolverType.EigenSparse
376
377speedup=True
378if speedup:
379    simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
380    simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
381
382SC.visualizationSettings.general.graphicsUpdateInterval = 0.01
383SC.visualizationSettings.nodes.show = True
384SC.visualizationSettings.nodes.drawNodesAsPoint  = False
385SC.visualizationSettings.nodes.showBasis = True
386SC.visualizationSettings.nodes.basisSize = 0.015
387
388SC.visualizationSettings.openGL.lineWidth = 2
389SC.visualizationSettings.openGL.shadow = 0.3
390SC.visualizationSettings.openGL.multiSampling = 4
391SC.visualizationSettings.openGL.perspective = 0.7
392# useGraphics=True
393#create animation:
394if useGraphics:
395    SC.visualizationSettings.window.renderWindowSize=[1920,1080]
396    SC.visualizationSettings.openGL.multiSampling = 4
397
398    if False: #save images
399        simulationSettings.solutionSettings.sensorsWritePeriod = 0.01 #to avoid laggy visualization
400        simulationSettings.solutionSettings.recordImagesInterval = 0.04
401        SC.visualizationSettings.exportImages.saveImageFileName = "images/frame"
402
403if useGraphics:
404    exu.StartRenderer()
405    mbs.WaitForUserToContinue()
406
407mbs.SolveDynamic(simulationSettings)
408
409p0=mbs.GetObjectOutputBody(bCar, exu.OutputVariableType.Position, localPosition=[0,0,0])
410
411u = 0+p0[0]
412for s in sLidar+sLidarInc:
413    u += mbs.GetSensorValues(s)
414
415u/=len(sLidar+sLidarInc)*10
416
417exu.Print('solution of mecanumWheelRollingDiscTest=',u)
418exudynTestGlobals.testError = u - (0.27142672383243405) #2020-06-20: 0.2714267238324345
419exudynTestGlobals.testResult = u
420
421
422if useGraphics:
423    SC.WaitForRenderEngineStopFlag()
424    exu.StopRenderer() #safely close rendering window!
425
426##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
427#plot results
428if useGraphics and False:
429
430
431    mbs.PlotSensor(sTrail, componentsX=[0]*4, components=[1]*4, title='wheel trails', closeAll=True,
432               markerStyles=['x ','o ','^ ','D '], markerSizes=12)
433    mbs.PlotSensor(sForce, components=[1]*4, title='wheel forces')