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
 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=(np.array(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, localHT=exu.HT(rotation=initialRotation, translation=pOff))) #the joint frame on the car
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],
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                                                  rollingViscousFriction=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'] = []
331def RotatedMarker(marker, rotation):
332    """a marker on the same body, its frame turned by rotation: the frame of a joint or connector"""
333    data = mbs.GetMarker(marker)
334    return mbs.AddMarker(MarkerBodyRigid(bodyNumber=data['bodyNumber'],
335                                         localHT=exu.HT(data['localHT']) * exu.HT(rotation=rotation)))
336
337for i in range(4):
338    # Torsional springdamper always acts in z-Axis: its markers are turned about y, so that z is the wheel axis
339    RM = RotationMatrixY(np.pi/2)
340    nData = mbs.AddNode(NodeGenericData(numberOfDataCoordinates = 1, initialCoordinates=[0])) # records multiples of 2*pi
341    mbs.variables['wheelMotor'] += [mbs.AddObject(TorsionalSpringDamper(name='Wheel{}Motor'.format(i), 
342                                            # mobileRobotBackDic['mAxlesList'][i]
343                                            markerNumbers=[RotatedMarker(markerCarAxles[i], RM), RotatedMarker(markerWheels[i], RM)],
344                                            nodeNumber= nData, # for continuous Rotation
345                                            stiffness = 0, damping = pControl))]
346#%% 
347# function to read data from Lidar sensors into array of global [x,y] values. 
348def GetCurrentData(mbs, Rot, pos): 
349    data = np.zeros([mbs.variables['nLidar'] , 2])
350    if not(flagLidarNoise): 
351        for i, sensor in enumerate(mbs.variables['sLidarList']): 
352            if useGroundTruth: 
353                R_ = np.eye(3)
354                R_[0:2, 0:2] = mbs.variables['R'][i]
355                data[i,:] =  (pos + Rot @ R_ @ [mbs.GetSensorValues(sensor), 0, 0])[0:2] #  GetSensorValues contains X-value
356            else: 
357                data[i,:] =  pos[0:2] + Rot[0:2,0:2] @ mbs.variables['R'][i] @ [mbs.GetSensorValues(sensor), 0]
358    else: 
359        noise_distance = np.random.normal(0, lidarNoiseLevel[0], mbs.variables['nLidar'])
360        noise_angle = np.random.normal(0, lidarNoiseLevel[1], mbs.variables['nLidar'])
361        for i, sensor in enumerate(mbs.variables['sLidarList']): 
362            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]
363            
364    return data
365
366
367#%% PreStepUF is called before every step. There odometry is calculated, velocity
368def PreStepUF(mbs, t):
369    # using Prestep instead of UFLoad reduced simulation time fopr 24 seconds from 6.11887 to 4.02554 seconds (~ 34%)
370    u, v, a = trajectory.Evaluate(t) # 
371    wDesired = MecanumXYphi2WheelVelocities(v[0],v[1],v[2],rWheel,wCar,lCar)
372    dt = mbs.sys['dynamicSolver'].it.currentStepSize # for integration of values
373    
374    # wheel control
375    for iWheel in range(4):
376        wCurrent = mbs.GetSensorValues(sAngularVelWheels[iWheel])[0] #local x-axis = wheel axis
377        mbs.variables['wWheels'][iWheel] = wCurrent # save current wheel velocity
378        mbs.SetObjectParameter(mbs.variables['wheelMotor'][iWheel], 'velocityOffset', wDesired[iWheel]) # set wheel velocity for control
379        
380    # calculate odometry
381    if flagOdometry: 
382        # odometry: vOdom = pinv(J) @ wWheels
383        # obtain position from vOdom by integration
384        if flagVelNoise: 
385            vOdom = WheelVelocities2MecanumXYphi(mbs.variables['wWheels'] + np.random.normal(0, velNoiseLevel, 4), 
386                                                 rWheel, wCar, lCar)
387        else: 
388            vOdom = WheelVelocities2MecanumXYphi(mbs.variables['wWheels'], rWheel, wCar, lCar)
389        mbs.variables['rotOdom'] += vOdom[-1] * dt  # (t - mbs.variables['tLast'])
390        mbs.variables['posOdom'] += Rot2D(mbs.variables['rotOdom']) @ vOdom[0:2] * dt
391        # print('pos: ', mbs.variables['posOdom'])
392        
393    if (t - mbs.variables['tLast']) > mbs.variables['dtLidar']: 
394        mbs.variables['tLast'] += mbs.variables['dtLidar']
395        
396        if useGroundTruth: 
397            # position and rotation taken from the gloabl data --> accurate! 
398            Rot = mbs.GetSensorValues(mbs.variables['sRot']).reshape([3,3])
399            pos = mbs.GetSensorValues(mbs.variables['sPos'])
400        elif flagOdometry: 
401            Rot = Rot2D(mbs.variables['rotOdom'])
402            pos = mbs.variables['posOdom']
403        data = GetCurrentData(mbs, Rot, pos)
404        k = int(t/mbs.variables['dtLidar'])
405        # print('data {} at t: {}'.format(k, round(t, 2))) 
406        mbs.variables['lidarDataHistory'][k,:,:] = data
407        mbs.variables['posHistory'][k] = pos[0:2]
408        mbs.variables['RotHistory'][k] = Rot[0:2,0:2]
409
410        
411    return True
412
413# allocate dictionary values for Lidar measurements
414h=0.005
415tEnd = trajectory.GetTimes()[-1] + 2 + h # add +h to call preStepFunction at tEnd
416mbs.variables['wWheels'] = np.zeros([4])
417mbs.variables['posOdom'], mbs.variables['rotOdom'], mbs.variables['tLast'] = np.array([0,0], dtype=np.float64), 0, 0
418mbs.variables['phiWheels'] = np.zeros(4)
419mbs.variables['tLast'] = 0
420mbs.variables['dtLidar'] = 0.1 #50e-3
421mbs.variables['nLidar'] = len(mbs.variables['sLidarList'])
422nMeasure = int(tEnd/mbs.variables['dtLidar']) + 1
423mbs.variables['lidarDataHistory'] = np.zeros([nMeasure, mbs.variables['nLidar'], 2])
424mbs.variables['RotHistory'] = np.zeros([nMeasure, 2,2])
425mbs.variables['RotHistory'][0] = np.eye(2)
426mbs.variables['posHistory'] = np.zeros([nMeasure, 2])
427
428mbs.SetPreStepUserFunction(PreStepUF)
429mbs.Assemble() # Assemble creats system equations and enables reading data for timestep 0
430data0 = GetCurrentData(mbs, mbs.GetSensorValues(mbs.variables['sRot']).reshape([3,3]), mbs.GetSensorValues(mbs.variables['sPos']))
431mbs.variables['lidarDataHistory'][0] = data0
432
433
434#%%create simulation settings
435simulationSettings = exu.SimulationSettings() #takes currently set values or default values
436simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
437simulationSettings.timeIntegration.endTime = tEnd
438simulationSettings.solution.sensors.writePeriod = 0.1
439simulationSettings.timeIntegration.verboseMode = 1
440simulationSettings.show.computationTime = False
441simulationSettings.show.statistics = False
442
443simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
444simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
445simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 # 0.5
446simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
447
448simulationSettings.timeIntegration.newton.useModifiedNewton = True
449simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
450simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
451simulationSettings.linearSolver.solverType=exu.LinearSolverType.EigenSparse
452
453speedup=True
454if speedup:
455    simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
456    simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
457    
458SC.visualizationSettings.general.graphicsUpdateInterval = 0.01
459SC.visualizationSettings.nodes.show = True
460SC.visualizationSettings.nodes.drawNodesAsPoint  = False
461SC.visualizationSettings.nodes.showBasis = True
462SC.visualizationSettings.nodes.basisSize = 0.015
463
464SC.visualizationSettings.openGL.lineWidth = 2
465SC.visualizationSettings.openGL.light0.shadow = 0.3
466SC.visualizationSettings.openGL.multiSampling = 4
467SC.visualizationSettings.view0.camera.perspective = 0.7
468
469#create animation:
470if useGraphics:
471    SC.visualizationSettings.view0.window.renderWindowSize=[1920,1080]
472    SC.visualizationSettings.openGL.multiSampling = 4
473
474    if False: #save images
475        simulationSettings.solution.sensors.writePeriod = 0.01 #to avoid laggy visualization
476        simulationSettings.solution.recordImagesInterval = 0.04
477        SC.visualizationSettings.exportImages.saveImageFileName = "images/frame"
478
479if useGraphics:
480    SC.renderer.Start()
481    SC.renderer.DoIdleTasks()
482
483mbs.SolveDynamic(simulationSettings)
484
485if useGraphics:
486    SC.renderer.DoIdleTasks()
487    SC.renderer.Stop() #safely close rendering window!
488
489
490#%% 
491p0=mbs.GetObjectOutputBody(bCar, exu.OutputVariableType.Position, localPosition=[0,0,0])
492
493if useGraphics: #
494    plt.close('all')
495    plt.figure()
496    from matplotlib import colors as mcolors
497    myColors = dict(mcolors.BASE_COLORS, **mcolors.CSS4_COLORS)
498    col1 = mcolors.to_rgb(myColors['red'])
499    col2 = mcolors.to_rgb(myColors['green'])    
500    for i in range(0, mbs.variables['lidarDataHistory'].shape[0]):   
501        col_i = np.array(col1)* (1 - i/(nMeasure-1)) + np.array(col2)* (i/(nMeasure-1))
502        plt.plot(mbs.variables['lidarDataHistory'][i,:,0], mbs.variables['lidarDataHistory'][i,:,1], 
503                             'x', label='lidar m' + str(i), color=col_i.tolist())
504        e1 = mbs.variables['RotHistory'][i][:,1]
505        p = mbs.variables['posHistory'][i]
506        plt.plot(p[0], p[1], 'o', color=col_i)
507        plt.arrow(p[0], p[1], e1[0], e1[1], color=col_i, head_width=0.2)
508        
509    plt.title('lidar data: using ' + 'accurate data' * bool(useGroundTruth) + 'inaccurate Odometry' * bool(not(useGroundTruth) and flagOdometry))
510    plt.grid()
511    plt.axis('equal')
512    plt.xlabel('x in m')
513    plt.ylabel('y in m')
514
515##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
516#plot results
517# if useGraphics and False:
518#     mbs.PlotSensor(sTrail, componentsX=[0]*4, components=[1]*4, title='wheel trails', closeAll=True,
519#                markerStyles=['x ','o ','^ ','D '], markerSizes=12)
520#     mbs.PlotSensor(sForce, components=[1]*4, title='wheel forces')