lugreFrictionTest.py

You can view and download this file on Github: lugreFrictionTest.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  This model reproduces the results of Canudas de Wit et al. (1995),
  5#           A New Model for Control of Systems with Friction,
  6#           IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 40, NO. 3, MARCH 1995
  7#           uses exactly same ODE1 model, and compares to position based friction model
  8#
  9# Author:   Johannes Gerstmayr
 10# Date:     2022-03-01
 11#
 12# 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.
 13#
 14#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 15
 16import exudyn as exu
 17from exudyn.itemInterface import *
 18from exudyn.utilities import *
 19
 20import numpy as np
 21from math import sin, cos, exp, sqrt, pi
 22
 23SC = exu.SystemContainer()
 24mbs = SC.AddSystem()
 25exu.Print('EXUDYN version='+exu.GetVersionString())
 26
 27#++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 28#Lugre friction text model: Canudas de Wit et al. (1995):
 29M=1
 30K=2
 31sigma0=1e5
 32sigma1=sqrt(sigma0)
 33sigma2=0.4
 34Fc=1
 35Fs=1.5
 36Vs=0.001
 37
 38useLugre = True         #compute ODE1 Lugre model
 39useLugreRef = False     #store as reference solution (with small step size)
 40useLugrePos = True      #alternative: uses a position level approach, being much more efficient for implicit solvers
 41useLugreFast = False    #with higher stiffness, but shorter time; shows good agreement, but requires extremely small time steps
 42doImplicit = True       #use implicit time integration
 43
 44#faster version with higher spring stiffness and "friction" stiffness sigma0 ==> gives closer results to idealized case:
 45if useLugreFast:
 46    K=100
 47    sigma0 = 1e7 #for LugrePos works also well with 1e6 and (with some step reductions) with 1e5
 48    sigma1=sqrt(sigma0)
 49
 50if useLugre:
 51    nODE1=3 #U,V,Z
 52    qInit = [0]*nODE1
 53    #qInit[0] = 1
 54    nodeODE1 = mbs.AddNode(NodeGenericODE1(referenceCoordinates=[0]*nODE1,
 55                                        initialCoordinates=qInit,
 56                                        numberOfODE1Coordinates=nODE1))
 57
 58    def UFode1(mbs, t, itemNumber, q):
 59        qt=np.zeros(nODE1)
 60        U=0.1*t
 61        FL=0
 62        X=q[0]
 63        V=q[1]
 64        Z=q[2]
 65        G=1/sigma0*(Fc+(Fs-Fc)*exp(-(V/Vs)**2))
 66
 67        Z_t=V-Z*abs(V)/G
 68        FL=sigma0*Z+sigma1*Z_t+sigma2*V
 69
 70        qt[0] = V
 71        qt[1] = (K*(U-X) - FL)/M
 72        qt[2] = Z_t
 73        #print('qt=',qt)
 74        return qt
 75
 76    oGenericODE1 = mbs.AddObject(ObjectGenericODE1(nodeNumbers=[nodeODE1],
 77                                                   rhsUserFunction=UFode1))
 78
 79
 80    sCoords1 = mbs.AddSensor(SensorNode(nodeNumber = nodeODE1,
 81                                        storeInternal=True,
 82                                        fileName='solution/lugreCoords'+'Ref'*useLugreRef+'.txt',
 83                                        outputVariableType=exu.OutputVariableType.Coordinates))
 84
 85    def UFsensorFrictionForce(mbs, t, sensorNumbers, factors, configuration):
 86        q = mbs.GetSensorValues(sensorNumbers[0])
 87        X=q[0]
 88        V=q[1]
 89        Z=q[2]
 90        G=1/sigma0*(Fc+(Fs-Fc)*exp(-(V/Vs)**2))
 91
 92        Z_t=V-Z*abs(V)/G
 93        FL=sigma0*Z+sigma1*Z_t+sigma2*V
 94        return [FL]
 95
 96    sFriction1 = mbs.AddSensor(SensorUserFunction(sensorNumbers=[sCoords1],
 97                                                  fileName='solution/lugreForce'+'Ref'*useLugreRef+'.txt',
 98                                                  storeInternal=True,sensorUserFunction=UFsensorFrictionForce))
 99    #ODE23 integrator, aTol=rTol=1e-8:
100    #h=2e-4:
101    #coords1= [1.9088392241941983, 9.424153111977732e-06, 1.1816794956539981e-05]
102    #h=2.5e-5:
103    #coords1= [1.9088391993013991, 9.424154586579873e-06, 1.1816795454370936e-05]
104    #DOPRI5:
105    #h=5e-5:
106    #coords1= [1.908839199226505,  9.424154590959904e-06, 1.1816795455868868e-05]
107    #h=1e-3:
108    #coords1= [1.9088391995380227, 9.424154572220395e-06, 1.181679544963896e-05]
109
110if useLugrePos:
111    node1D = mbs.AddNode(Node1D(referenceCoordinates = [0],
112                                initialCoordinates=[0.],
113                                initialVelocities=[0.]))
114    mass1D = mbs.AddObject(Mass1D(nodeNumber = node1D, physicsMass=M,
115                                  visualization=VMass1D(graphicsData=[GraphicsDataSphere(radius=0.05, color=color4dodgerblue)])))
116
117    #+++++++++++++++++++++++++++++++++++++++++++
118    #friction model:
119
120    #data[0]: 0=slip, 1=stick; start with sticking at last position=0!
121    #data[1]: last sticking position
122    nData = mbs.AddNode(NodeGenericData(initialCoordinates=[1,0], numberOfDataCoordinates=2))
123
124    #sigma1=0 #this does not work without damping!!!
125    #markers for friction point (does not change)
126    nGroundFric = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0]))
127    groundMarkerFric=mbs.AddMarker(MarkerNodeCoordinate(nodeNumber= nGroundFric, coordinate = 0))
128    nodeMarker =mbs.AddMarker(MarkerNodeCoordinate(nodeNumber= node1D, coordinate = 0))
129
130    def springForce(mbs, t, itemNumber, u, v, k, d, offset, velocityOffset,
131                    dynamicFriction, staticFrictionOffset, exponentialDecayStatic, viscousFriction, frictionProportionalZone):
132                    #offset, dryFriction, dryFrictionProportionalZone):
133
134        data = mbs.GetNodeOutput(nData,variableType=exu.OutputVariableType.Coordinates)
135        if data[0] == 1:
136            F = sigma0*(u-data[1])+sigma1*v
137        else:
138            F = np.sign(v)*(Fc+(Fs-Fc)*exp(-(v/Vs)**2))
139
140        return d*v + F
141
142    #Spring-Damper between two marker coordinates
143    oCSD=mbs.AddObject(CoordinateSpringDamperExt(markerNumbers = [groundMarkerFric, nodeMarker],
144                                         stiffness = sigma0, damping = sigma2,
145                                         frictionProportionalZone=1e-16, #0 not possible right now
146                                         springForceUserFunction = springForce,
147                                         visualization=VCoordinateSpringDamper(show=False)))
148
149    #+++++++++++++++++++++++++++++++++++++++++++
150    #spring
151    #reference point for spring:
152    nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0]))
153    groundMarker=mbs.AddMarker(MarkerNodeCoordinate(nodeNumber= nGround, coordinate = 0))
154
155    oCSD2=mbs.AddObject(CoordinateSpringDamper(markerNumbers = [groundMarker, nodeMarker],
156                                         stiffness = K, damping = 0))
157
158    cnt=0
159    def PreStepUserFunction(mbs, t):
160        # global cnt
161        U=0.1*t #displacement
162        mbs.SetNodeParameter(nGround, 'referenceCoordinates', [U,0.,0.])
163        mbs.SetObjectParameter(oCSD2, 'offset', U)
164
165        #F = mbs.GetObjectOutput(oCSD,variableType=exu.OutputVariableType.Force)
166        u = mbs.GetObjectOutput(oCSD,variableType=exu.OutputVariableType.Displacement)
167        v = mbs.GetObjectOutput(oCSD,variableType=exu.OutputVariableType.Velocity)
168        F = (Fc+(Fs-Fc)*exp(-(v/Vs)**2))
169        #data = mbs.GetNodeOutput(nData,variableType=exu.OutputVariableType.Coordinates)
170        data = mbs.systemData.GetDataCoordinates()
171        u0 = data[1]
172
173        # cnt+=1
174        # if t>0 and cnt%5000==0:
175        #     print('friction spring force=',abs(sigma0*(u-u0)+sigma1*v), ', Ffric=', F)
176        #stick->slip:
177        if data[0] == 1 and abs(sigma0*(u-u0)+sigma1*v) > F:
178            data[0] = 0
179        #slip->stick:
180        #elif data[0] == 0 and abs(sigma0*(u-u0)+sigma1*v) < F:
181        # elif data[0] == 0 and np.sign(v) != np.sign(F): #this seems to be the best choice for larger Vs, also for Fc~Fs
182        elif data[0] == 0 and (np.sign(v) != np.sign(F) or abs(sigma0*(u-u0)+sigma1*v) < F):
183            data[0] = 1
184
185        if data[0] == 0:
186            data[1] = u #always update sticking position during slipping
187
188        mbs.systemData.SetDataCoordinates(data)
189
190        return True
191
192    mbs.SetPreStepUserFunction(PreStepUserFunction)
193
194    #sensors
195    sCoords2 = mbs.AddSensor(SensorNode(nodeNumber = node1D, storeInternal=True,
196                                        outputVariableType=exu.OutputVariableType.Coordinates))
197    sCoords2_t = mbs.AddSensor(SensorNode(nodeNumber = node1D, storeInternal=True,
198                                        outputVariableType=exu.OutputVariableType.Coordinates_t))
199    sCSD2 = mbs.AddSensor(SensorObject(objectNumber = oCSD,storeInternal=True,
200                                        outputVariableType=exu.OutputVariableType.Force))
201    sData2 = mbs.AddSensor(SensorNode(nodeNumber = nData, storeInternal=True,
202                                        outputVariableType=exu.OutputVariableType.Coordinates))
203
204#assemble and solve system for default parameters
205mbs.Assemble()
206
207# exu.Print(mbs.systemData.GetObjectLTGODE1(0))
208# exu.Print(mbs.systemData.GetObjectLTGODE2(1))
209
210sims=exu.SimulationSettings()
211tEnd = 25
212h=1e-4
213sims.timeIntegration.absoluteTolerance = 1e-6
214
215if useLugreFast:
216    tEnd = 2
217    h=1e-4
218    if useLugre:
219        h=1e-6
220        sims.timeIntegration.absoluteTolerance = 1e-6
221
222sims.timeIntegration.relativeTolerance = sims.timeIntegration.absoluteTolerance
223
224sims.timeIntegration.endTime = tEnd
225sims.solutionSettings.writeSolutionToFile = False
226#sims.solutionSettings.sensorsWritePeriod = h
227sims.solutionSettings.sensorsWritePeriod = 1e-3
228sims.timeIntegration.verboseMode = 1
229
230# solverType=exu.DynamicSolverType.ExplicitEuler
231solverType=exu.DynamicSolverType.ODE23
232#solverType=exu.DynamicSolverType.DOPRI5
233#solverType=exu.DynamicSolverType.RK67
234
235if doImplicit:
236    solverType=exu.DynamicSolverType.TrapezoidalIndex2
237    h=0.5e-3 #works quite well with 2e-2
238
239if useLugreRef:
240    sims.solutionSettings.sensorsWritePeriod = 2e-3
241    solverType=exu.DynamicSolverType.DOPRI5
242
243
244
245sims.timeIntegration.numberOfSteps = int(tEnd/h)
246sims.timeIntegration.endTime = tEnd
247#sims.timeIntegration.initialStepSize = 1e-5
248
249
250useGraphics = True
251if useGraphics:
252    SC.visualizationSettings.general.autoFitScene = False
253    exu.StartRenderer()
254    if 'renderState' in exu.sys:
255        SC.SetRenderState(exu.sys['renderState'])
256    mbs.WaitForUserToContinue()
257
258
259
260if True:
261    sims.timeIntegration.numberOfSteps = int(tEnd/h)
262    mbs.SolveDynamic(solverType=solverType, simulationSettings=sims)
263
264
265if useGraphics:
266    SC.WaitForRenderEngineStopFlag()
267    exu.StopRenderer() #safely close rendering window!
268
269if useLugre:
270    exu.Print('coords1=', list(mbs.GetSensorValues(sCoords1)) )
271
272#+++++++++++++++++++++++++++++++++++++++++++++++++++++
273if True:
274
275    mbs.PlotSensor([], closeAll=True)
276
277    if useLugre:
278        mbs.PlotSensor(sCoords1,[0,1,2])
279        mbs.PlotSensor(sFriction1,0, colorCodeOffset=3, newFigure=False)
280    else:
281        if useLugreFast:
282            mbs.PlotSensor('solution/lugreCoordsRef2.txt',[0,1,2],
283                       labels=['LuGre pos','LuGre vel','Lugre Z'])
284            mbs.PlotSensor('solution/lugreForceRef2.txt',0, colorCodeOffset=3, newFigure=False, labels=['LuGre force'])
285        else:
286            mbs.PlotSensor('solution/lugreCoordsRef1e7Impl.txt',[0,1,2],
287                       labels=['LuGre pos','LuGre vel','Lugre Z'])
288            mbs.PlotSensor('solution/lugreForceRef1e7Impl.txt',0, colorCodeOffset=3, newFigure=False, labels=['LuGre force'])
289    if useLugrePos:
290        mbs.PlotSensor([sCoords2,sCoords2_t,sCSD2,sData2,sData2],[0,0,0,0,1], lineStyles='--', yLabel='coordinates, force', newFigure=False,
291                   labels=['pos','vel','spring force','stick','last sticking pos'],
292                   markerStyles=['','','','x','o '], markerDensity=200)