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