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