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