mobileMecanumWheelRobotWithLidar.py

#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN example
#
# Details:  Simple vehicle model with Mecanum wheels and 'rotating' laser scanner (Lidar)
#           Model supports simple trajectories, calculate Odometry and transform 
#           Lidar data into global frame. 
#
# Author:   Peter Manzl
# Date:     2024-04-23
#
# 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.
#
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

import exudyn
import exudyn as exu
from exudyn.utilities import *
from exudyn.robotics.utilities import AddLidar
from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration

import numpy as np
from math import sin, cos, tan
import matplotlib.pyplot as plt

useGraphics=True


#%% adjust the following parameters
useGroundTruth = True  # read position/orientation data from simulation for transforming Lidar Data into global frame
flagOdometry = True    # calculate Odometry; if flagReadPosRot
flagLidarNoise = False # add normally distributed noise to Lidar data with std. deviation below 
lidarNoiseLevel = np.array([0.05, 0.01]) # std deviation on length and angle of lidar

# normally distributed noise added to angular velocity of wheels to estimate odometry
# Note that slipping can already occur because of the implemented non-ideal contact between Mecanum wheel and ground
flagVelNoise = False
velNoiseLevel = 0.025



#%% predefined trajectory
# trajectory is defined as´[x, y, phi_z] in global system
trajectory = Trajectory(initialCoordinates=[0   ,0   ,0], initialTime=0)
trajectory.Add(ProfileConstantAcceleration([0   ,-4  ,0], 3))
trajectory.Add(ProfileConstantAcceleration([0   ,-4  ,2.1*np.pi], 6))
#trajectory.Add(ProfileConstantAcceleration([0   ,0   ,2*np.pi], 6))
trajectory.Add(ProfileConstantAcceleration([0   ,0   ,2.1*np.pi], 4))
trajectory.Add(ProfileConstantAcceleration([10  ,0   ,2.1*np.pi], 4))
trajectory.Add(ProfileConstantAcceleration([10  ,0   ,0*np.pi], 4))
# trajectory.Add(ProfileConstantAcceleration([3.6 ,0   ,2.1*np.pi], 0.5)) # wait for 0.5 seconds
# trajectory.Add(ProfileConstantAcceleration([3.6 ,4.2 ,2.1*np.pi], 6))
trajectory.Add(ProfileConstantAcceleration([10  ,7 ,0*np.pi], 4))
trajectory.Add(ProfileConstantAcceleration([10  ,7 ,0.5*np.pi], 4))
trajectory.Add(ProfileConstantAcceleration([12  ,14 ,0.5*np.pi], 4))
# trajectory.Add(ProfileConstantAcceleration([2   ,7 ,0*np.pi], 4))
# trajectory.Add(ProfileConstantAcceleration([3.6 ,4.2 ,0*np.pi], 6))



#%%
SC = exu.SystemContainer()
mbs = SC.AddSystem()

g = [0,0,-9.81]     #gravity in m/s^2

#++++++++++++++++++++++++++++++
#wheel parameters:
rhoWheel = 500          # density kg/m^3
rWheel = 0.4            # radius of disc in m
wWheel = 0.2            # width of disc in m, just for drawing
p0Wheel = [0, 0, rWheel]        # origin of disc center point at reference, such that initial contact point is at [0,0,0]
initialRotationCar = RotationMatrixZ(0)

v0 = 0  # initial car velocity in y-direction
omega0Wheel = [v0/rWheel, 0, 0]                   # initial angular velocity around z-axis


# %% ++++++++++++++++++++++++++++++
# define car (mobile robot) parameters:
# car setup:
# ^Y, lCar
# | W2 +---+ W3
# |    |   |
# |    | + | car center point
# |    |   |
# | W0 +---+ W1
# +---->X, wCar

p0Car = [0, 0, rWheel]   # origin of disc center point at reference, such that initial contact point is at [0,0,0]
lCar = 2                # y-direction
wCar = 1.5              # x-direction
hCar = rWheel           # z-direction
mCar = 500
omega0Car = [0,0,0]                   #initial angular velocity around z-axis
v0Car = [0,-v0,0]                  #initial velocity of car center point

#inertia for infinitely small ring:
inertiaWheel = InertiaCylinder(density=rhoWheel, length=wWheel, outerRadius=rWheel, axis=0)

inertiaCar = InertiaCuboid(density=mCar/(lCar*wCar*hCar),sideLengths=[wCar, lCar, hCar])

rLidar = 0.5*rWheel
pLidar1 = [(-wCar*0.5-rLidar)*0, 0*(lCar*0.5+rWheel+rLidar), hCar*0.8]
# pLidar2 = [ wCar*0.5+rLidar,-lCar*0.5-rWheel-rLidar,hCar*0.5]
graphicsCar = [GraphicsDataOrthoCubePoint(centerPoint=[0,0,0],size=[wCar-1.1*wWheel, lCar+2*rWheel, hCar], 
                                         color=color4steelblue)]


graphicsCar += [GraphicsDataCylinder(pAxis=pLidar1, vAxis=[0,0,0.5*rLidar], radius=rLidar, clor=color4darkgrey)]
graphicsCar += [GraphicsDataBasis(headFactor = 4, length=2)]
# graphicsCar += [GraphicsDataCylinder(pAxis=pLidar2, vAxis=[0,0,0.5*rLidar], radius=rLidar, clor=color4darkgrey)]

[nCar,bCar]=AddRigidBody(mainSys = mbs, 
                         inertia = inertiaCar, 
                         nodeType = str(exu.NodeType.RotationEulerParameters), 
                         position = p0Car, 
                         rotationMatrix = initialRotationCar,
                         angularVelocity = omega0Car,
                         velocity=v0Car,
                         gravity = g, 
                         graphicsDataList = graphicsCar)

markerCar = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=[0,0,hCar*0.5]))


markerCar1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pLidar1))

nWheels = 4
markerWheels=[]
markerCarAxles=[]
oRollingDiscs=[]
sAngularVelWheels=[]

# car setup:
# ^Y, lCar
# | W2 +---+ W3
# |    |   |
# |    | + | car center point
# |    |   |
# | W0 +---+ W1
# +---->X, wCar

#ground body and marker
LL = 8
gGround = GraphicsDataCheckerBoard(point=[0.25*LL,0.25*LL,0],size=2*LL)

#obstacles:
zz=1
gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0,7,0.5*zz],size=[2*zz,zz,1*zz], color=color4dodgerblue), gGround)
gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[6,5,1.5*zz],size=[zz,2*zz,3*zz], color=color4dodgerblue), gGround)
gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[3,-2.5,0.5*zz],size=[2*zz,zz,1*zz], color=color4dodgerblue), gGround)
gGround = MergeGraphicsDataTriangleList(GraphicsDataCylinder(pAxis=[-3,0,0],vAxis=[0,0,zz], radius=1.5, color=color4dodgerblue, nTiles=64), gGround)

#walls:
tt=0.2
gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0.25*LL,0.25*LL-LL,0.5*zz],size=[2*LL,tt,zz], color=color4dodgerblue), gGround)
gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0.25*LL,0.25*LL+LL,0.5*zz],size=[2*LL,tt,zz], color=color4dodgerblue), gGround)
gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0.25*LL-LL,0.25*LL,0.5*zz],size=[tt,2*LL,zz], color=color4dodgerblue), gGround)
gGround = MergeGraphicsDataTriangleList(GraphicsDataOrthoCubePoint(centerPoint=[0.25*LL+LL,0.25*LL,0.5*zz],size=[tt,2*LL,zz], color=color4dodgerblue), gGround)


oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[gGround])))
mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))


#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#set up general contact geometry where sensors measure
# helper function to create 2D rotation Matrix
def Rot2D(phi): 
    return np.array([[np.cos(phi),-np.sin(phi)],
                     [np.sin(phi), np.cos(phi)]])

[meshPoints, meshTrigs] = GraphicsData2PointsAndTrigs(gGround)

ngc = mbs.CreateDistanceSensorGeometry(meshPoints, meshTrigs, rigidBodyMarkerIndex=mGround, searchTreeCellSize=[8,8,1])
maxDistance = 20 #max. distance of sensors; just large enough to reach everything; take care, in zoom all it will show this large area

# dict mbs.variables can be accessed globally in the "control" functions
mbs.variables['Lidar'] = [pi*0.25, pi*0.75, 50]
mbs.variables['LidarAngles'] = np.linspace(mbs.variables['Lidar'][0], mbs.variables['Lidar'][1], mbs.variables['Lidar'] [2])
mbs.variables['R'] = []
for phi in mbs.variables['LidarAngles']: 
    mbs.variables['R']  += [Rot2D(phi)] # zero-angle of Lidar is at x-axis


mbs.variables['sLidarList'] = AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar1, minDistance=0, maxDistance=maxDistance, 
          numberOfSensors=mbs.variables['Lidar'][2], addGraphicsObject=True,
          angleStart=mbs.variables['Lidar'][0], 
          angleEnd=mbs.variables['Lidar'][1], # 1.5*pi-pi,
          lineLength=1, storeInternal=True, color=color4red, inclination=0., rotation=RotationMatrixZ(np.pi/2*0))

if False: # here additional Sensors could be created to have e.g. two markers diagonally on the car (robot)
    AddLidar(mbs, generalContactIndex=ngc, positionOrMarker=markerCar2, minDistance=0, maxDistance=maxDistance, 
              numberOfSensors=100,angleStart=0, angleEnd=1.5*pi, inclination=-4/180*pi,
              lineLength=1, storeInternal=True, color=color4grey )
    

    

#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if useGraphics:
    sCarVel = mbs.AddSensor(SensorBody(bodyNumber=bCar, storeInternal=True, #fileName='solution/rollingDiscCarVel.txt', 
                                outputVariableType = exu.OutputVariableType.Velocity))

mbs.variables['sRot'] = mbs.AddSensor(SensorBody(bodyNumber=bCar, storeInternal=True, outputVariableType=exu.OutputVariableType.RotationMatrix))
mbs.variables['sPos'] = mbs.AddSensor(SensorBody(bodyNumber=bCar, storeInternal=True, outputVariableType=exu.OutputVariableType.Position))
sPos = []
sTrail=[]
sForce=[]

# create Mecanum wheels and ground contact
for iWheel in range(nWheels):
    frictionAngle = 0.25*np.pi # 45°
    if iWheel == 0 or iWheel == 3: # difference in diagonal
        frictionAngle *= -1

    #additional graphics for visualization of rotation (JUST FOR DRAWING!):
    graphicsWheel = [GraphicsDataOrthoCubePoint(centerPoint=[0,0,0],size=[wWheel*1.1,0.7*rWheel,0.7*rWheel], color=color4lightred)]
    nCyl = 12
    rCyl = 0.1*rWheel
    for i in range(nCyl): #draw cylinders on wheels
        iPhi = i/nCyl*2*np.pi
        pAxis = np.array([0,rWheel*np.sin(iPhi),-rWheel*np.cos(iPhi)])
        vAxis = [0.5*wWheel*np.cos(frictionAngle),0.5*wWheel*np.sin(frictionAngle),0]
        vAxis2 = RotationMatrixX(iPhi)@vAxis
        rColor = color4grey
        if i >= nCyl/2: rColor = color4darkgrey
        graphicsWheel += [GraphicsDataCylinder(pAxis=pAxis-vAxis2, vAxis=2*vAxis2, radius=rCyl, 
                                               color=rColor)]
        graphicsWheel+= [GraphicsDataBasis()]

    dx = -0.5*wCar
    dy = -0.5*lCar
    if iWheel > 1: dy *= -1
    if iWheel == 1 or iWheel == 3: dx *= -1

    kRolling = 1e5
    dRolling = kRolling*0.01

    initialRotation = RotationMatrixZ(0)

    #v0Wheel = Skew(omega0Wheel) @ initialRotationWheel @ [0,0,rWheel]   #initial angular velocity of center point
    v0Wheel = v0Car #approx.

    pOff = [dx,dy,0]

    #add wheel body
    [n0,b0]=AddRigidBody(mainSys = mbs, 
                         inertia = inertiaWheel, 
                         nodeType = str(exu.NodeType.RotationEulerParameters), 
                         position = VAdd(p0Wheel,pOff), 
                         rotationMatrix = initialRotation, #np.diag([1,1,1]),
                         angularVelocity = omega0Wheel,
                         velocity=v0Wheel,
                         gravity = g, 
                         graphicsDataList = graphicsWheel)

    #markers for rigid body:
    mWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0,0,0]))
    markerWheels += [mWheel]

    mCarAxle = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bCar, localPosition=pOff))
    markerCarAxles += [mCarAxle]

    lockedAxis0 = 0 # could be used to lock an Axis
    #if iWheel==0 or iWheel==1: freeAxis = 1 #lock rotation
    mbs.AddObject(GenericJoint(markerNumbers=[mWheel,mCarAxle],rotationMarker1=initialRotation,
                               constrainedAxes=[1,1,1,lockedAxis0,1,1])) #revolute joint for wheel

    nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
    oRolling = mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[mGround, mWheel], nodeNumber = nGeneric,
                                                  discRadius=rWheel, dryFriction=[1.,0.001], dryFrictionAngle=frictionAngle,
                                                  dryFrictionProportionalZone=1e-1, 
                                                  rollingFrictionViscous=0.01,
                                                  contactStiffness=kRolling, contactDamping=dRolling,
                                                  visualization=VObjectConnectorRollingDiscPenalty(discWidth=wWheel, color=color4blue)))
    oRollingDiscs += [oRolling]

    strNum = str(iWheel)
    sAngularVelWheels += [mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,#fileName='solution/rollingDiscAngVelLocal'+strNum+'.txt', 
                               outputVariableType = exu.OutputVariableType.AngularVelocityLocal))]

    if useGraphics:
        sPos+=[mbs.AddSensor(SensorBody(bodyNumber=b0, storeInternal=True,#fileName='solution/rollingDiscPos'+strNum+'.txt', 
                                   outputVariableType = exu.OutputVariableType.Position))]
    
        sTrail+=[mbs.AddSensor(SensorObject(name='Trail'+strNum,objectNumber=oRolling, storeInternal=True,#fileName='solution/rollingDiscTrail'+strNum+'.txt', 
                                   outputVariableType = exu.OutputVariableType.Position))]
    
        sForce+=[mbs.AddSensor(SensorObject(objectNumber=oRolling, storeInternal=True,#fileName='solution/rollingDiscForce'+strNum+'.txt', 
                                   outputVariableType = exu.OutputVariableType.ForceLocal))]



# takes as input the translational and angular velocity and outputs the velocities for all 4 wheels
# wheel axis is mounted at x-axis; positive angVel rotates CCW in x/y plane viewed from top
# car setup:
# ^Y, lCar
# | W2 +---+ W3
# |    |   |
# |    | + | car center point
# |    |   |
# | W0 +---+ W1
# +---->X, wCar
# values given for wheel0/3: frictionAngle=-pi/4, wheel 1/2: frictionAngle=pi/4; dryFriction=[1,0] (looks in lateral (x) direction)
# ==>direction of axis of roll on ground of wheel0: [1,-1] and of wheel1: [1,1]
def MecanumXYphi2WheelVelocities(xVel, yVel, angVel, R, Lx, Ly):
    LxLy2 = (Lx+Ly)/2
    mat = (1/R)*np.array([[ 1,-1, LxLy2],
                          [-1,-1,-LxLy2],
                          [-1,-1, LxLy2],
                          [ 1,-1,-LxLy2]])    
    return mat @ [xVel, yVel, angVel]

def WheelVelocities2MecanumXYphi(w, R, Lx, Ly):
    LxLy2 = (Lx+Ly)/2
    mat = (1/R)*np.array([[ 1,-1, LxLy2],
                          [-1,-1,-LxLy2],
                          [-1,-1, LxLy2],
                          [ 1,-1,-LxLy2]])    
    return np.linalg.pinv(mat) @ w


pControl = 100 # P-control on wheel velocity
mbs.variables['wheelMotor'] = []
mbs.variables['loadWheel'] = []
for i in range(4):
    # Torsional springdamper always acts in z-Axis
    RM1 = RotationMatrixY(np.pi/2) 
    RM0 = RotationMatrixY(np.pi/2)
    nData = mbs.AddNode(NodeGenericData(numberOfDataCoordinates = 1, initialCoordinates=[0])) # records multiples of 2*pi
    mbs.variables['wheelMotor'] += [mbs.AddObject(TorsionalSpringDamper(name='Wheel{}Motor'.format(i), 
                                            # mobileRobotBackDic['mAxlesList'][i]
                                            markerNumbers=[markerCarAxles[i], markerWheels[i]],
                                            nodeNumber= nData, # for continuous Rotation
                                            stiffness = 0, damping = pControl, 
                                            rotationMarker0=RM0, 
                                            rotationMarker1=RM1))]
#%% 
# function to read data from Lidar sensors into array of global [x,y] values. 
def GetCurrentData(mbs, Rot, pos): 
    data = np.zeros([mbs.variables['nLidar'] , 2])
    if not(flagLidarNoise): 
        for i, sensor in enumerate(mbs.variables['sLidarList']): 
            if useGroundTruth: 
                R_ = np.eye(3)
                R_[0:2, 0:2] = mbs.variables['R'][i]
                data[i,:] =  (pos + Rot @ R_ @ [mbs.GetSensorValues(sensor), 0, 0])[0:2] #  GetSensorValues contains X-value
            else: 
                data[i,:] =  pos[0:2] + Rot[0:2,0:2] @ mbs.variables['R'][i] @ [mbs.GetSensorValues(sensor), 0]
    else: 
        noise_distance = np.random.normal(0, lidarNoiseLevel[0], mbs.variables['nLidar'])
        noise_angle = np.random.normal(0, lidarNoiseLevel[1], mbs.variables['nLidar'])
        for i, sensor in enumerate(mbs.variables['sLidarList']): 
            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]
            
    return data


#%% PreStepUF is called before every step. There odometry is calculated, velocity
def PreStepUF(mbs, t):
    # using Prestep instead of UFLoad reduced simulation time fopr 24 seconds from 6.11887 to 4.02554 seconds (~ 34%)
    u, v, a = trajectory.Evaluate(t) # 
    wDesired = MecanumXYphi2WheelVelocities(v[0],v[1],v[2],rWheel,wCar,lCar)
    dt = mbs.sys['dynamicSolver'].it.currentStepSize # for integration of values
    
    # wheel control
    for iWheel in range(4):
        wCurrent = mbs.GetSensorValues(sAngularVelWheels[iWheel])[0] #local x-axis = wheel axis
        mbs.variables['wWheels'][iWheel] = wCurrent # save current wheel velocity
        mbs.SetObjectParameter(mbs.variables['wheelMotor'][iWheel], 'velocityOffset', wDesired[iWheel]) # set wheel velocity for control
        
    # calculate odometry
    if flagOdometry: 
        # odometry: vOdom = pinv(J) @ wWheels
        # obtain position from vOdom by integration
        if flagVelNoise: 
            vOdom = WheelVelocities2MecanumXYphi(mbs.variables['wWheels'] + np.random.normal(0, velNoiseLevel, 4), 
                                                 rWheel, wCar, lCar)
        else: 
            vOdom = WheelVelocities2MecanumXYphi(mbs.variables['wWheels'], rWheel, wCar, lCar)
        mbs.variables['rotOdom'] += vOdom[-1] * dt  # (t - mbs.variables['tLast'])
        mbs.variables['posOdom'] += Rot2D(mbs.variables['rotOdom']) @ vOdom[0:2] * dt
        # print('pos: ', mbs.variables['posOdom'])
        
    if (t - mbs.variables['tLast']) > mbs.variables['dtLidar']: 
        mbs.variables['tLast'] += mbs.variables['dtLidar']
        
        if useGroundTruth: 
            # position and rotation taken from the gloabl data --> accurate! 
            Rot = mbs.GetSensorValues(mbs.variables['sRot']).reshape([3,3])
            pos = mbs.GetSensorValues(mbs.variables['sPos'])
        elif flagOdometry: 
            Rot = Rot2D(mbs.variables['rotOdom'])
            pos = mbs.variables['posOdom']
        data = GetCurrentData(mbs, Rot, pos)
        k = int(t/mbs.variables['dtLidar'])
        # print('data {} at t: {}'.format(k, round(t, 2))) 
        mbs.variables['lidarDataHistory'][k,:,:] = data
        mbs.variables['posHistory'][k] = pos[0:2]
        mbs.variables['RotHistory'][k] = Rot[0:2,0:2]

        
    return True

# allocate dictionary values for Lidar measurements
h=0.005
tEnd = trajectory.GetTimes()[-1] + 2 + h # add +h to call preStepFunction at tEnd
mbs.variables['wWheels'] = np.zeros([4])
mbs.variables['posOdom'], mbs.variables['rotOdom'], mbs.variables['tLast'] = np.array([0,0], dtype=np.float64), 0, 0
mbs.variables['phiWheels'] = np.zeros(4)
mbs.variables['tLast'] = 0
mbs.variables['dtLidar'] = 0.1 #50e-3
mbs.variables['nLidar'] = len(mbs.variables['sLidarList'])
nMeasure = int(tEnd/mbs.variables['dtLidar']) + 1
mbs.variables['lidarDataHistory'] = np.zeros([nMeasure, mbs.variables['nLidar'], 2])
mbs.variables['RotHistory'] = np.zeros([nMeasure, 2,2])
mbs.variables['RotHistory'][0] = np.eye(2)
mbs.variables['posHistory'] = np.zeros([nMeasure, 2])

mbs.SetPreStepUserFunction(PreStepUF)
mbs.Assemble() # Assemble creats system equations and enables reading data for timestep 0
data0 = GetCurrentData(mbs, mbs.GetSensorValues(mbs.variables['sRot']).reshape([3,3]), mbs.GetSensorValues(mbs.variables['sPos']))
mbs.variables['lidarDataHistory'][0] = data0


#%%create simulation settings
simulationSettings = exu.SimulationSettings() #takes currently set values or default values
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
simulationSettings.timeIntegration.endTime = tEnd
simulationSettings.solutionSettings.sensorsWritePeriod = 0.1
simulationSettings.timeIntegration.verboseMode = 1
simulationSettings.displayComputationTime = False
simulationSettings.displayStatistics = False

simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 # 0.5
simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True

simulationSettings.timeIntegration.newton.useModifiedNewton = True
simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
simulationSettings.linearSolverType=exu.LinearSolverType.EigenSparse

speedup=True
if speedup:
    simulationSettings.timeIntegration.discontinuous.ignoreMaxIterations = False #reduce step size for contact switching
    simulationSettings.timeIntegration.discontinuous.iterationTolerance = 0.1
    
SC.visualizationSettings.general.graphicsUpdateInterval = 0.01
SC.visualizationSettings.nodes.show = True
SC.visualizationSettings.nodes.drawNodesAsPoint  = False
SC.visualizationSettings.nodes.showBasis = True
SC.visualizationSettings.nodes.basisSize = 0.015

SC.visualizationSettings.openGL.lineWidth = 2
SC.visualizationSettings.openGL.shadow = 0.3
SC.visualizationSettings.openGL.multiSampling = 4
SC.visualizationSettings.openGL.perspective = 0.7

#create animation:
if useGraphics:
    SC.visualizationSettings.window.renderWindowSize=[1920,1080]
    SC.visualizationSettings.openGL.multiSampling = 4

    if False: #save images
        simulationSettings.solutionSettings.sensorsWritePeriod = 0.01 #to avoid laggy visualization
        simulationSettings.solutionSettings.recordImagesInterval = 0.04
        SC.visualizationSettings.exportImages.saveImageFileName = "images/frame"

if useGraphics:
    exu.StartRenderer()
    mbs.WaitForUserToContinue()

mbs.SolveDynamic(simulationSettings)

if useGraphics:
    SC.WaitForRenderEngineStopFlag()
    exu.StopRenderer() #safely close rendering window!


#%% 
p0=mbs.GetObjectOutputBody(bCar, exu.OutputVariableType.Position, localPosition=[0,0,0])

if useGraphics: #
    plt.close('all')
    plt.figure()
    from matplotlib import colors as mcolors
    myColors = dict(mcolors.BASE_COLORS, **mcolors.CSS4_COLORS)
    col1 = mcolors.to_rgb(myColors['red'])
    col2 = mcolors.to_rgb(myColors['green'])    
    for i in range(0, mbs.variables['lidarDataHistory'].shape[0]):   
        col_i = np.array(col1)* (1 - i/(nMeasure-1)) + np.array(col2)* (i/(nMeasure-1))
        plt.plot(mbs.variables['lidarDataHistory'][i,:,0], mbs.variables['lidarDataHistory'][i,:,1], 
                             'x', label='lidar m' + str(i), color=col_i.tolist())
        e1 = mbs.variables['RotHistory'][i][:,1]
        p = mbs.variables['posHistory'][i]
        plt.plot(p[0], p[1], 'o', color=col_i)
        plt.arrow(p[0], p[1], e1[0], e1[1], color=col_i, head_width=0.2)
        
    plt.title('lidar data: using ' + 'accurate data' * bool(useGroundTruth) + 'inaccurate Odometry' * bool(not(useGroundTruth) and flagOdometry))
    plt.grid()
    plt.axis('equal')
    plt.xlabel('x in m')
    plt.ylabel('y in m')

##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
#plot results
# if useGraphics and False:
#     mbs.PlotSensor(sTrail, componentsX=[0]*4, components=[1]*4, title='wheel trails', closeAll=True,
#                markerStyles=['x ','o ','^ ','D '], markerSizes=12)
#     mbs.PlotSensor(sForce, components=[1]*4, title='wheel forces')