mobileMecanumWheelRobotWithLidar.py
You can view and download this file on Github: mobileMecanumWheelRobotWithLidar.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Simple vehicle model with Mecanum wheels and 'rotating' laser scanner (Lidar)
5# Model supports simple trajectories, calculate Odometry and transform
6# Lidar data into global frame.
7#
8# Author: Peter Manzl
9# Date: 2024-04-23
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
16import exudyn as exu
17from exudyn.utilities import *
18from exudyn.robotics.utilities import AddLidar
19from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration
20
21import numpy as np
22from math import sin, cos, tan
23import matplotlib.pyplot as plt
24
25useGraphics=True
26
27
28#%% adjust the following parameters
29useGroundTruth = True # read position/orientation data from simulation for transforming Lidar Data into global frame
30flagOdometry = True # calculate Odometry; if flagReadPosRot
31flagLidarNoise = False # add normally distributed noise to Lidar data with std. deviation below
32lidarNoiseLevel = np.array([0.05, 0.01]) # std deviation on length and angle of lidar
33
34# normally distributed noise added to angular velocity of wheels to estimate odometry
35# Note that slipping can already occur because of the implemented non-ideal contact between Mecanum wheel and ground
36flagVelNoise = False
37velNoiseLevel = 0.025
38
39
40
41#%% predefined trajectory
42# trajectory is defined as´[x, y, phi_z] in global system
43trajectory = Trajectory(initialCoordinates=[0 ,0 ,0], initialTime=0)
44trajectory.Add(ProfileConstantAcceleration([0 ,-4 ,0], 3))
45trajectory.Add(ProfileConstantAcceleration([0 ,-4 ,2.1*np.pi], 6))
46#trajectory.Add(ProfileConstantAcceleration([0 ,0 ,2*np.pi], 6))
47trajectory.Add(ProfileConstantAcceleration([0 ,0 ,2.1*np.pi], 4))
48trajectory.Add(ProfileConstantAcceleration([10 ,0 ,2.1*np.pi], 4))
49trajectory.Add(ProfileConstantAcceleration([10 ,0 ,0*np.pi], 4))
50# trajectory.Add(ProfileConstantAcceleration([3.6 ,0 ,2.1*np.pi], 0.5)) # wait for 0.5 seconds
51# trajectory.Add(ProfileConstantAcceleration([3.6 ,4.2 ,2.1*np.pi], 6))
52trajectory.Add(ProfileConstantAcceleration([10 ,7 ,0*np.pi], 4))
53trajectory.Add(ProfileConstantAcceleration([10 ,7 ,0.5*np.pi], 4))
54trajectory.Add(ProfileConstantAcceleration([12 ,14 ,0.5*np.pi], 4))
55# trajectory.Add(ProfileConstantAcceleration([2 ,7 ,0*np.pi], 4))
56# trajectory.Add(ProfileConstantAcceleration([3.6 ,4.2 ,0*np.pi], 6))
57
58
59
60#%%
61SC = exu.SystemContainer()
62mbs = SC.AddSystem()
63
64g = [0,0,-9.81] #gravity in m/s^2
65
66#++++++++++++++++++++++++++++++
67#wheel parameters:
68rhoWheel = 500 # density kg/m^3
69rWheel = 0.4 # radius of disc in m
70wWheel = 0.2 # width of disc in m, just for drawing
71p0Wheel = [0, 0, rWheel] # origin of disc center point at reference, such that initial contact point is at [0,0,0]
72initialRotationCar = RotationMatrixZ(0)
73
74v0 = 0 # initial car velocity in y-direction
75omega0Wheel = [v0/rWheel, 0, 0] # initial angular velocity around z-axis
76
77
78# %% ++++++++++++++++++++++++++++++
79# define car (mobile robot) parameters:
80# car setup:
81# ^Y, lCar
82# | W2 +---+ W3
83# | | |
84# | | + | car center point
85# | | |
86# | W0 +---+ W1
87# +---->X, wCar
88
89p0Car = [0, 0, rWheel] # origin of disc center point at reference, such that initial contact point is at [0,0,0]
90lCar = 2 # y-direction
91wCar = 1.5 # x-direction
92hCar = rWheel # z-direction
93mCar = 500
94omega0Car = [0,0,0] #initial angular velocity around z-axis
95v0Car = [0,-v0,0] #initial velocity of car center point
96
97#inertia for infinitely small ring:
98inertiaWheel = InertiaCylinder(density=rhoWheel, length=wWheel, outerRadius=rWheel, axis=0)
99
100inertiaCar = InertiaCuboid(density=mCar/(lCar*wCar*hCar),sideLengths=[wCar, lCar, hCar])
101
102rLidar = 0.5*rWheel
103pLidar1 = [(-wCar*0.5-rLidar)*0, 0*(lCar*0.5+rWheel+rLidar), hCar*0.8]
104# pLidar2 = [ wCar*0.5+rLidar,-lCar*0.5-rWheel-rLidar,hCar*0.5]
105graphicsCar = [GraphicsDataOrthoCubePoint(centerPoint=[0,0,0],size=[wCar-1.1*wWheel, lCar+2*rWheel, hCar],
106 color=color4steelblue)]
107
108
109graphicsCar += [GraphicsDataCylinder(pAxis=pLidar1, vAxis=[0,0,0.5*rLidar], radius=rLidar, clor=color4darkgrey)]
110graphicsCar += [GraphicsDataBasis(headFactor = 4, length=2)]
111# graphicsCar += [GraphicsDataCylinder(pAxis=pLidar2, vAxis=[0,0,0.5*rLidar], radius=rLidar, clor=color4darkgrey)]
112
113[nCar,bCar]=AddRigidBody(mainSys = mbs,
114 inertia = inertiaCar,
115 nodeType = str(exu.NodeType.RotationEulerParameters),
116 position = p0Car,
117 rotationMatrix = initialRotationCar,
118 angularVelocity = omega0Car,
119 velocity=v0Car,
120 gravity = g,
121 graphicsDataList = graphicsCar)
122
123markerCar = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=[0,0,hCar*0.5]))
124
125
126markerCar1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pLidar1))
127
128nWheels = 4
129markerWheels=[]
130markerCarAxles=[]
131oRollingDiscs=[]
132sAngularVelWheels=[]
133
134# car setup:
135# ^Y, lCar
136# | W2 +---+ W3
137# | | |
138# | | + | car center point
139# | | |
140# | W0 +---+ W1
141# +---->X, wCar
142
143#ground body and marker
144LL = 8
145gGround = GraphicsDataCheckerBoard(point=[0.25*LL,0.25*LL,0],size=2*LL)
146
147#obstacles:
148zz=1
149gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0,7,0.5*zz],size=[2*zz,zz,1*zz], color=color4dodgerblue), gGround)
150gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[6,5,1.5*zz],size=[zz,2*zz,3*zz], color=color4dodgerblue), gGround)
151gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[3,-2.5,0.5*zz],size=[2*zz,zz,1*zz], color=color4dodgerblue), gGround)
152gGround = MergeGraphicsDataTriangleList(GraphicsDataCylinder(pAxis=[-3,0,0],vAxis=[0,0,zz], radius=1.5, color=color4dodgerblue, nTiles=64), gGround)
153
154#walls:
155tt=0.2
156gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0.25*LL,0.25*LL-LL,0.5*zz],size=[2*LL,tt,zz], color=color4dodgerblue), gGround)
157gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0.25*LL,0.25*LL+LL,0.5*zz],size=[2*LL,tt,zz], color=color4dodgerblue), gGround)
158gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0.25*LL-LL,0.25*LL,0.5*zz],size=[tt,2*LL,zz], color=color4dodgerblue), gGround)
159gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0.25*LL+LL,0.25*LL,0.5*zz],size=[tt,2*LL,zz], color=color4dodgerblue), gGround)
160
161
162oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
163mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
164
165
166#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
167#set up general contact geometry where sensors measure
168# helper function to create 2D rotation Matrix
169def Rot2D(phi):
170 return np.array([[np.cos(phi),-np.sin(phi)],
171 [np.sin(phi), np.cos(phi)]])
172
173[meshPoints, meshTrigs] = GraphicsData2PointsAndTrigs(gGround)
174
175ngc = mbs.CreateDistanceSensorGeometry(meshPoints, meshTrigs, rigidBodyMarkerIndex=mGround, searchTreeCellSize=[8,8,1])
176maxDistance = 20 #max. distance of sensors; just large enough to reach everything; take care, in zoom all it will show this large area
177
178# dict mbs.variables can be accessed globally in the "control" functions
179mbs.variables['Lidar'] = [pi*0.25, pi*0.75, 50]
180mbs.variables['LidarAngles'] = np.linspace(mbs.variables['Lidar'][0], mbs.variables['Lidar'][1], mbs.variables['Lidar'] [2])
181mbs.variables['R'] = []
182for phi in mbs.variables['LidarAngles']:
183 mbs.variables['R'] += [Rot2D(phi)] # zero-angle of Lidar is at x-axis
184
185
186mbs.variables['sLidarList'] = AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar1, minDistance=0, maxDistance=maxDistance,
187 numberOfSensors=mbs.variables['Lidar'][2], addGraphicsObject=True,
188 angleStart=mbs.variables['Lidar'][0],
189 angleEnd=mbs.variables['Lidar'][1], # 1.5*pi-pi,
190 lineLength=1, storeInternal=True, color=color4red, inclination=0., rotation=RotationMatrixZ(np.pi/2*0))
191
192if False: # here additional Sensors could be created to have e.g. two markers diagonally on the car (robot)
193 AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar2, minDistance=0, maxDistance=maxDistance,
194 numberOfSensors=100,angleStart=0, angleEnd=1.5*pi, inclination=-4/180*pi,
195 lineLength=1, storeInternal=True, color=color4grey )
196
197
198
199
200#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
201if useGraphics:
202 sCarVel = mbs.AddSensor(SensorBody(bodyNumber=bCar, storeInternal=True, #fileName='solution/rollingDiscCarVel.txt',
203 outputVariableType = exu.OutputVariableType.Velocity))
204
205mbs.variables['sRot'] = mbs.AddSensor(SensorBody(bodyNumber=bCar, storeInternal=True, outputVariableType=exu.OutputVariableType.RotationMatrix))
206mbs.variables['sPos'] = mbs.AddSensor(SensorBody(bodyNumber=bCar, storeInternal=True, outputVariableType=exu.OutputVariableType.Position))
207sPos = []
208sTrail=[]
209sForce=[]
210
211# create Mecanum wheels and ground contact
212for iWheel in range(nWheels):
213 frictionAngle = 0.25*np.pi # 45°
214 if iWheel == 0 or iWheel == 3: # difference in diagonal
215 frictionAngle *= -1
216
217 #additional graphics for visualization of rotation (JUST FOR DRAWING!):
218 graphicsWheel = [GraphicsDataOrthoCubePoint(centerPoint=[0,0,0],size=[wWheel*1.1,0.7*rWheel,0.7*rWheel], color=color4lightred)]
219 nCyl = 12
220 rCyl = 0.1*rWheel
221 for i in range(nCyl): #draw cylinders on wheels
222 iPhi = i/nCyl*2*np.pi
223 pAxis = np.array([0,rWheel*np.sin(iPhi),-rWheel*np.cos(iPhi)])
224 vAxis = [0.5*wWheel*np.cos(frictionAngle),0.5*wWheel*np.sin(frictionAngle),0]
225 vAxis2 = RotationMatrixX(iPhi)@vAxis
226 rColor = color4grey
227 if i >= nCyl/2: rColor = color4darkgrey
228 graphicsWheel += [GraphicsDataCylinder(pAxis=pAxis-vAxis2, vAxis=2*vAxis2, radius=rCyl,
229 color=rColor)]
230 graphicsWheel+= [GraphicsDataBasis()]
231
232 dx = -0.5*wCar
233 dy = -0.5*lCar
234 if iWheel > 1: dy *= -1
235 if iWheel == 1 or iWheel == 3: dx *= -1
236
237 kRolling = 1e5
238 dRolling = kRolling*0.01
239
240 initialRotation = RotationMatrixZ(0)
241
242 #v0Wheel = Skew(omega0Wheel) @ initialRotationWheel @ [0,0,rWheel] #initial angular velocity of center point
243 v0Wheel = v0Car #approx.
244
245 pOff = [dx,dy,0]
246
247 #add wheel body
248 [n0,b0]=AddRigidBody(mainSys = mbs,
249 inertia = inertiaWheel,
250 nodeType = str(exu.NodeType.RotationEulerParameters),
251 position = VAdd(p0Wheel,pOff),
252 rotationMatrix = initialRotation, #np.diag([1,1,1]),
253 angularVelocity = omega0Wheel,
254 velocity=v0Wheel,
255 gravity = g,
256 graphicsDataList = graphicsWheel)
257
258 #markers for rigid body:
259 mWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0,0,0]))
260 markerWheels += [mWheel]
261
262 mCarAxle = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pOff))
263 markerCarAxles += [mCarAxle]
264
265 lockedAxis0 = 0 # could be used to lock an Axis
266 #if iWheel==0 or iWheel==1: freeAxis = 1 #lock rotation
267 mbs.AddObject(GenericJoint(markerNumbers=[mWheel,mCarAxle],rotationMarker1=initialRotation,
268 constrainedAxes=[1,1,1,lockedAxis0,1,1])) #revolute joint for wheel
269
270 nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
271 oRolling = mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[mGround, mWheel], nodeNumber = nGeneric,
272 discRadius=rWheel, dryFriction=[1.,0.001], dryFrictionAngle=frictionAngle,
273 dryFrictionProportionalZone=1e-1,
274 rollingFrictionViscous=0.01,
275 contactStiffness=kRolling, contactDamping=dRolling,
276 visualization=VObjectConnectorRollingDiscPenalty(discWidth=wWheel, color=color4blue)))
277 oRollingDiscs += [oRolling]
278
279 strNum = str(iWheel)
280 sAngularVelWheels += [mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,#fileName='solution/rollingDiscAngVelLocal'+strNum+'.txt',
281 outputVariableType = exu.OutputVariableType.AngularVelocityLocal))]
282
283 if useGraphics:
284 sPos+=[mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,#fileName='solution/rollingDiscPos'+strNum+'.txt',
285 outputVariableType = exu.OutputVariableType.Position))]
286
287 sTrail+=[mbs.AddSensor(SensorObject(name='Trail'+strNum,objectNumber=oRolling, storeInternal=True,#fileName='solution/rollingDiscTrail'+strNum+'.txt',
288 outputVariableType = exu.OutputVariableType.Position))]
289
290 sForce+=[mbs.AddSensor(SensorObject(objectNumber=oRolling, storeInternal=True,#fileName='solution/rollingDiscForce'+strNum+'.txt',
291 outputVariableType = exu.OutputVariableType.ForceLocal))]
292
293
294
295# takes as input the translational and angular velocity and outputs the velocities for all 4 wheels
296# wheel axis is mounted at x-axis; positive angVel rotates CCW in x/y plane viewed from top
297# car setup:
298# ^Y, lCar
299# | W2 +---+ W3
300# | | |
301# | | + | car center point
302# | | |
303# | W0 +---+ W1
304# +---->X, wCar
305# values given for wheel0/3: frictionAngle=-pi/4, wheel 1/2: frictionAngle=pi/4; dryFriction=[1,0] (looks in lateral (x) direction)
306# ==>direction of axis of roll on ground of wheel0: [1,-1] and of wheel1: [1,1]
307def MecanumXYphi2WheelVelocities(xVel, yVel, angVel, R, Lx, Ly):
308 LxLy2 = (Lx+Ly)/2
309 mat = (1/R)*np.array([[ 1,-1, LxLy2],
310 [-1,-1,-LxLy2],
311 [-1,-1, LxLy2],
312 [ 1,-1,-LxLy2]])
313 return mat @ [xVel, yVel, angVel]
314
315def WheelVelocities2MecanumXYphi(w, R, Lx, Ly):
316 LxLy2 = (Lx+Ly)/2
317 mat = (1/R)*np.array([[ 1,-1, LxLy2],
318 [-1,-1,-LxLy2],
319 [-1,-1, LxLy2],
320 [ 1,-1,-LxLy2]])
321 return np.linalg.pinv(mat) @ w
322
323
324pControl = 100 # P-control on wheel velocity
325mbs.variables['wheelMotor'] = []
326mbs.variables['loadWheel'] = []
327for i in range(4):
328 # Torsional springdamper always acts in z-Axis
329 RM1 = RotationMatrixY(np.pi/2)
330 RM0 = RotationMatrixY(np.pi/2)
331 nData = mbs.AddNode(NodeGenericData(numberOfDataCoordinates = 1, initialCoordinates=[0])) # records multiples of 2*pi
332 mbs.variables['wheelMotor'] += [mbs.AddObject(TorsionalSpringDamper(name='Wheel{}Motor'.format(i),
333 # mobileRobotBackDic['mAxlesList'][i]
334 markerNumbers=[markerCarAxles[i], markerWheels[i]],
335 nodeNumber= nData, # for continuous Rotation
336 stiffness = 0, damping = pControl,
337 rotationMarker0=RM0,
338 rotationMarker1=RM1))]
339#%%
340# function to read data from Lidar sensors into array of global [x,y] values.
341def GetCurrentData(mbs, Rot, pos):
342 data = np.zeros([mbs.variables['nLidar'] , 2])
343 if not(flagLidarNoise):
344 for i, sensor in enumerate(mbs.variables['sLidarList']):
345 if useGroundTruth:
346 R_ = np.eye(3)
347 R_[0:2, 0:2] = mbs.variables['R'][i]
348 data[i,:] = (pos + Rot @ R_ @ [mbs.GetSensorValues(sensor), 0, 0])[0:2] # GetSensorValues contains X-value
349 else:
350 data[i,:] = pos[0:2] + Rot[0:2,0:2] @ mbs.variables['R'][i] @ [mbs.GetSensorValues(sensor), 0]
351 else:
352 noise_distance = np.random.normal(0, lidarNoiseLevel[0], mbs.variables['nLidar'])
353 noise_angle = np.random.normal(0, lidarNoiseLevel[1], mbs.variables['nLidar'])
354 for i, sensor in enumerate(mbs.variables['sLidarList']):
355 data[i,:] = pos[0:2] + Rot2D(noise_angle[i]) @ Rot[0:2,0:2] @ mbs.variables['R'][i] @ (mbs.GetSensorValues(sensor) + [noise_distance[i],0]).tolist() # + [0.32]
356
357 return data
358
359
360#%% PreStepUF is called before every step. There odometry is calculated, velocity
361def PreStepUF(mbs, t):
362 # using Prestep instead of UFLoad reduced simulation time fopr 24 seconds from 6.11887 to 4.02554 seconds (~ 34%)
363 u, v, a = trajectory.Evaluate(t) #
364 wDesired = MecanumXYphi2WheelVelocities(v[0],v[1],v[2],rWheel,wCar,lCar)
365 dt = mbs.sys['dynamicSolver'].it.currentStepSize # for integration of values
366
367 # wheel control
368 for iWheel in range(4):
369 wCurrent = mbs.GetSensorValues(sAngularVelWheels[iWheel])[0] #local x-axis = wheel axis
370 mbs.variables['wWheels'][iWheel] = wCurrent # save current wheel velocity
371 mbs.SetObjectParameter(mbs.variables['wheelMotor'][iWheel], 'velocityOffset', wDesired[iWheel]) # set wheel velocity for control
372
373 # calculate odometry
374 if flagOdometry:
375 # odometry: vOdom = pinv(J) @ wWheels
376 # obtain position from vOdom by integration
377 if flagVelNoise:
378 vOdom = WheelVelocities2MecanumXYphi(mbs.variables['wWheels'] + np.random.normal(0, velNoiseLevel, 4),
379 rWheel, wCar, lCar)
380 else:
381 vOdom = WheelVelocities2MecanumXYphi(mbs.variables['wWheels'], rWheel, wCar, lCar)
382 mbs.variables['rotOdom'] += vOdom[-1] * dt # (t - mbs.variables['tLast'])
383 mbs.variables['posOdom'] += Rot2D(mbs.variables['rotOdom']) @ vOdom[0:2] * dt
384 # print('pos: ', mbs.variables['posOdom'])
385
386 if (t - mbs.variables['tLast']) > mbs.variables['dtLidar']:
387 mbs.variables['tLast'] += mbs.variables['dtLidar']
388
389 if useGroundTruth:
390 # position and rotation taken from the gloabl data --> accurate!
391 Rot = mbs.GetSensorValues(mbs.variables['sRot']).reshape([3,3])
392 pos = mbs.GetSensorValues(mbs.variables['sPos'])
393 elif flagOdometry:
394 Rot = Rot2D(mbs.variables['rotOdom'])
395 pos = mbs.variables['posOdom']
396 data = GetCurrentData(mbs, Rot, pos)
397 k = int(t/mbs.variables['dtLidar'])
398 # print('data {} at t: {}'.format(k, round(t, 2)))
399 mbs.variables['lidarDataHistory'][k,:,:] = data
400 mbs.variables['posHistory'][k] = pos[0:2]
401 mbs.variables['RotHistory'][k] = Rot[0:2,0:2]
402
403
404 return True
405
406# allocate dictionary values for Lidar measurements
407h=0.005
408tEnd = trajectory.GetTimes()[-1] + 2 + h # add +h to call preStepFunction at tEnd
409mbs.variables['wWheels'] = np.zeros([4])
410mbs.variables['posOdom'], mbs.variables['rotOdom'], mbs.variables['tLast'] = np.array([0,0], dtype=np.float64), 0, 0
411mbs.variables['phiWheels'] = np.zeros(4)
412mbs.variables['tLast'] = 0
413mbs.variables['dtLidar'] = 0.1 #50e-3
414mbs.variables['nLidar'] = len(mbs.variables['sLidarList'])
415nMeasure = int(tEnd/mbs.variables['dtLidar']) + 1
416mbs.variables['lidarDataHistory'] = np.zeros([nMeasure, mbs.variables['nLidar'], 2])
417mbs.variables['RotHistory'] = np.zeros([nMeasure, 2,2])
418mbs.variables['RotHistory'][0] = np.eye(2)
419mbs.variables['posHistory'] = np.zeros([nMeasure, 2])
420
421mbs.SetPreStepUserFunction(PreStepUF)
422mbs.Assemble() # Assemble creats system equations and enables reading data for timestep 0
423data0 = GetCurrentData(mbs, mbs.GetSensorValues(mbs.variables['sRot']).reshape([3,3]), mbs.GetSensorValues(mbs.variables['sPos']))
424mbs.variables['lidarDataHistory'][0] = data0
425
426
427#%%create simulation settings
428simulationSettings = exu.SimulationSettings() #takes currently set values or default values
429simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
430simulationSettings.timeIntegration.endTime = tEnd
431simulationSettings.solutionSettings.sensorsWritePeriod = 0.1
432simulationSettings.timeIntegration.verboseMode = 1
433simulationSettings.displayComputationTime = False
434simulationSettings.displayStatistics = False
435
436simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
437simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
438simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 # 0.5
439simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
440
441simulationSettings.timeIntegration.newton.useModifiedNewton = True
442simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
443simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
444simulationSettings.linearSolverType=exu.LinearSolverType.EigenSparse
445
446speedup=True
447if speedup:
448 simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
449 simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
450
451SC.visualizationSettings.general.graphicsUpdateInterval = 0.01
452SC.visualizationSettings.nodes.show = True
453SC.visualizationSettings.nodes.drawNodesAsPoint = False
454SC.visualizationSettings.nodes.showBasis = True
455SC.visualizationSettings.nodes.basisSize = 0.015
456
457SC.visualizationSettings.openGL.lineWidth = 2
458SC.visualizationSettings.openGL.shadow = 0.3
459SC.visualizationSettings.openGL.multiSampling = 4
460SC.visualizationSettings.openGL.perspective = 0.7
461
462#create animation:
463if useGraphics:
464 SC.visualizationSettings.window.renderWindowSize=[1920,1080]
465 SC.visualizationSettings.openGL.multiSampling = 4
466
467 if False: #save images
468 simulationSettings.solutionSettings.sensorsWritePeriod = 0.01 #to avoid laggy visualization
469 simulationSettings.solutionSettings.recordImagesInterval = 0.04
470 SC.visualizationSettings.exportImages.saveImageFileName = "images/frame"
471
472if useGraphics:
473 exu.StartRenderer()
474 mbs.WaitForUserToContinue()
475
476mbs.SolveDynamic(simulationSettings)
477
478if useGraphics:
479 SC.WaitForRenderEngineStopFlag()
480 exu.StopRenderer() #safely close rendering window!
481
482
483#%%
484p0=mbs.GetObjectOutputBody(bCar, exu.OutputVariableType.Position, localPosition=[0,0,0])
485
486if useGraphics: #
487 plt.close('all')
488 plt.figure()
489 from matplotlib import colors as mcolors
490 myColors = dict(mcolors.BASE_COLORS, **mcolors.CSS4_COLORS)
491 col1 = mcolors.to_rgb(myColors['red'])
492 col2 = mcolors.to_rgb(myColors['green'])
493 for i in range(0, mbs.variables['lidarDataHistory'].shape[0]):
494 col_i = np.array(col1)* (1 - i/(nMeasure-1)) + np.array(col2)* (i/(nMeasure-1))
495 plt.plot(mbs.variables['lidarDataHistory'][i,:,0], mbs.variables['lidarDataHistory'][i,:,1],
496 'x', label='lidar m' + str(i), color=col_i.tolist())
497 e1 = mbs.variables['RotHistory'][i][:,1]
498 p = mbs.variables['posHistory'][i]
499 plt.plot(p[0], p[1], 'o', color=col_i)
500 plt.arrow(p[0], p[1], e1[0], e1[1], color=col_i, head_width=0.2)
501
502 plt.title('lidar data: using ' + 'accurate data' * bool(useGroundTruth) + 'inaccurate Odometry' * bool(not(useGroundTruth) and flagOdometry))
503 plt.grid()
504 plt.axis('equal')
505 plt.xlabel('x in m')
506 plt.ylabel('y in m')
507
508##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
509#plot results
510# if useGraphics and False:
511# mbs.PlotSensor(sTrail, componentsX=[0]*4, components=[1]*4, title='wheel trails', closeAll=True,
512# markerStyles=['x ','o ','^ ','D '], markerSizes=12)
513# mbs.PlotSensor(sForce, components=[1]*4, title='wheel forces')