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