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