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')