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