lugreFrictionODE1.py

You can view and download this file on Github: lugreFrictionODE1.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 = False
 39useLugreRef = False
 40
 41nODE1=3 #U,V,Z
 42qInit = [0]*nODE1
 43#qInit[0] = 1
 44nodeODE1 = mbs.AddNode(NodeGenericODE1(referenceCoordinates=[0]*nODE1,
 45                                    initialCoordinates=qInit,
 46                                    numberOfODE1Coordinates=nODE1))
 47
 48#this user function represents the RHS of the first order differential equation for LuGre model:
 49#q_t = f(q,t)
 50def UFode1(mbs, t, itemNumber, q):
 51    q_t=np.zeros(nODE1)
 52    U=0.1*t
 53    FL=0
 54    X=q[0]
 55    V=q[1]
 56    Z=q[2]
 57    G=1/sigma0*(Fc+(Fs-Fc)*exp(-(V/Vs)**2))
 58
 59    Z_t=V-Z*abs(V)/G
 60    FL=sigma0*Z+sigma1*Z_t+sigma2*V #force
 61
 62    q_t[0] = V
 63    q_t[1] = (K*(U-X) - FL)/M
 64    q_t[2] = Z_t
 65
 66    return q_t
 67
 68oGenericODE1 = mbs.AddObject(ObjectGenericODE1(nodeNumbers=[nodeODE1],
 69                                               rhsUserFunction=UFode1))
 70
 71
 72sCoords1 = mbs.AddSensor(SensorNode(nodeNumber = nodeODE1,
 73                                    storeInternal=True,
 74                                    fileName='solution/lugreCoords'+'Ref'*useLugreRef+'.txt',
 75                                    outputVariableType=exu.OutputVariableType.Coordinates))
 76
 77#user force which computes the friction force
 78def UFsensorFrictionForce(mbs, t, sensorNumbers, factors, configuration):
 79    q = mbs.GetSensorValues(sensorNumbers[0])
 80    X=q[0]
 81    V=q[1]
 82    Z=q[2]
 83    G=1/sigma0*(Fc+(Fs-Fc)*exp(-(V/Vs)**2))
 84
 85    Z_t=V-Z*abs(V)/G
 86    FL=sigma0*Z+sigma1*Z_t+sigma2*V
 87    return [FL]
 88
 89sFriction1 = mbs.AddSensor(SensorUserFunction(sensorNumbers=[sCoords1],
 90                                              fileName='solution/lugreForce'+'Ref'*useLugreRef+'.txt',
 91                                              storeInternal=True,sensorUserFunction=UFsensorFrictionForce))
 92#ODE23 integrator, aTol=rTol=1e-8:
 93#h=2e-4:
 94#coords1= [1.9088392241941983, 9.424153111977732e-06, 1.1816794956539981e-05]
 95#h=2.5e-5:
 96#coords1= [1.9088391993013991, 9.424154586579873e-06, 1.1816795454370936e-05]
 97#DOPRI5:
 98#h=5e-5:
 99#coords1= [1.908839199226505,  9.424154590959904e-06, 1.1816795455868868e-05]
100#h=1e-3:
101#coords1= [1.9088391995380227, 9.424154572220395e-06, 1.181679544963896e-05]
102
103
104#assemble and solve system for default parameters
105mbs.Assemble()
106
107sims=exu.SimulationSettings()
108tEnd = 25
109h=1e-4
110sims.timeIntegration.absoluteTolerance = 1e-8
111sims.timeIntegration.relativeTolerance = sims.timeIntegration.absoluteTolerance
112
113sims.timeIntegration.endTime = tEnd
114sims.solutionSettings.writeSolutionToFile = False
115sims.solutionSettings.sensorsWritePeriod = 1e-3
116sims.timeIntegration.verboseMode = 1
117
118solverType=exu.DynamicSolverType.ODE23 #adaptive
119#solverType=exu.DynamicSolverType.DOPRI5
120#solverType=exu.DynamicSolverType.RK67
121
122sims.timeIntegration.numberOfSteps = int(tEnd/h)
123sims.timeIntegration.endTime = tEnd
124#sims.timeIntegration.initialStepSize = 1e-5
125
126
127
128sims.timeIntegration.numberOfSteps = int(tEnd/h)
129mbs.SolveDynamic(solverType=solverType, simulationSettings=sims)
130
131
132#+++++++++++++++++++++++++++++++++++++++++++++++++++++
133if True:
134
135    mbs.PlotSensor([], closeAll=True)
136
137    mbs.PlotSensor(sCoords1,[0,1,2], labels=['LuGre pos','LuGre vel','Lugre Z'])
138    mbs.PlotSensor(sFriction1,0, colorCodeOffset=3, newFigure=False, labels=['LuGre force'])