rollingDiscTangentialForces.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test for tangential forces of rolling disc
  5#
  6# Author:   Johannes Gerstmayr, Michael Pieber
  7# Date:     2024-04-29
  8#
  9# 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.
 10#
 11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 12
 13import exudyn as exu
 14from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 15import exudyn.graphics as graphics #only import if it does not conflict
 16
 17import numpy as np
 18
 19useGraphics = True #without test
 20#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 21#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 22try: #only if called from test suite
 23    from modelUnitTests import exudynTestGlobals #for globally storing test results
 24    useGraphics = exudynTestGlobals.useGraphics
 25except:
 26    class ExudynTestGlobals:
 27        pass
 28    exudynTestGlobals = ExudynTestGlobals()
 29#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 30SC = exu.SystemContainer()
 31mbs = SC.AddSystem()
 32
 33
 34#Dimensions
 35mass = 30  #kg
 36r = 1.8 #m
 37d = 3   #m
 38kAB=1.2 #inertia radius
 39
 40JradAB = mass*kAB**2
 41#wheel
 42w=0.1 #for drawing
 43
 44
 45
 46
 47#####################################
 48Fn=400 #N
 49g=9.81
 50
 51omegaY = np.sqrt(r*(Fn-mass*g)/JradAB)
 52omegaZ = d/r*omegaY #rad/s # = angular velocity omega_y
 53
 54exu.Print('omegaY=',omegaY)
 55exu.Print('omegaZ=',omegaZ)
 56
 57
 58theta = np.array([[mass*r**2,0,0], [0,mass*r**2,0], [0,0,JradAB]])
 59wV= np.array([0,omegaY*0,-omegaZ])
 60
 61
 62wL=np.array([0,omegaY,0])
 63M=Skew(wL)@theta@wV
 64
 65exu.Print('necessary torque=',d*(Fn-mass*g), ', gyro torque=', -M[0])
 66exu.Print(d*(mass*g-Fn))
 67#centrifugal force in simulation: -396.37
 68exu.Print('centrifugal force:',d * omegaY**2 * mass)
 69
 70#%%
 71#####################################
 72#####################################
 73omegaZ=omegaY*1
 74omegaX=-d/r*omegaZ #rad/s angular velocity of the wheel
 75g = [0,0,-9.81] #gravity
 76bodyDim = [d,0.1,0.1] #body dimensions
 77
 78
 79v0 = omegaZ * d
 80v0Vec = np.array([0,v0,0])
 81p0 = [0,0,0] #origin of pendulum
 82# p1 = [bodyDim[0],0,0] #origin of wheel
 83
 84pMid0 = [d*0.5,0,r] #center of mass, body0
 85pMid1 = [d,0,r] #center of mass, body1
 86
 87theta = np.array([[JradAB,0,0], [0,mass*r**2,0], [0,0,mass*r**2]])
 88iWheel = RigidBodyInertia(mass,theta,p0)
 89# exu.Print(iWheel)
 90iCube = InertiaCuboid(1, sideLengths=[0.1,0.1,0.1])
 91# exu.Print(iCube)
 92
 93
 94w1=np.array([-omegaZ,0,omegaX])     #wheel + frame
 95w2=np.array([0,0,omegaX])           #frame
 96
 97M=Skew(w2)@theta@w1                 #possible for symmetric rotor, but dangerous!
 98
 99Mgyro = w2[2]*theta[0,0]*w1[0]      #rotor solution; condition: rotor is symmetric
100
101exu.Print('omega1 x (Theta * omega2) = ',M[1])
102exu.Print('Mgyro                     = ',Mgyro)
103exu.Print('omega1 x (Theta * omega1) = ',Skew(w1)@theta@w1)
104exu.Print(d*(-Fn+mass*9.81))
105
106
107###############################################################################
108#
109
110
111#initial acceleration:
112angAcc = np.array([0., -14.28472222, 0. ])
113exu.Print('Theta * angAcc = ',theta@angAcc)
114exu.Print('Theta * angAcc + omega1 x (Theta * omega1) = ',theta@angAcc + Skew(w1)@theta@w1)
115
116planeNormal = RotationMatrixX(0.*pi)@np.array([0,0,1])
117vOff = -r*planeNormal+[0,0,r]
118
119#graphics for body
120graphicsBody = graphics.RigidLink(p0=[-0.5*bodyDim[0],0,0], p1=[0.5*bodyDim[0],0,0],
121                                     axis1=[0,0,1], radius=[0.01,0.01],
122                                     thickness = 0.2, width = [0.2,0.2], color=graphics.color.lightred)
123
124#%%
125[n0,b0]=AddRigidBody(mainSys = mbs,
126                     inertia = iCube,
127                     nodeType = str(exu.NodeType.RotationEulerParameters),
128                     position = pMid0,
129                     rotationMatrix = np.diag([1,1,1]),
130                     velocity = list(0.5*v0Vec),
131                     angularVelocity = [0,0,omegaZ],
132                     #gravity = g,
133                     graphicsDataList = [graphicsBody])
134
135
136graphicsBodyWheel = graphics.Brick(centerPoint=[0,0,0],size=[w*2,1.4*r,1.4*r], color=graphics.color.lightred)
137[n1,bWheel]=AddRigidBody(mainSys = mbs,
138                     inertia = iWheel,
139                     nodeType = str(exu.NodeType.RotationEulerParameters),
140                     position = pMid1,
141                     velocity = list(v0Vec),
142                     rotationMatrix = np.diag([1,1,1]),
143                     angularVelocity = [omegaX,0,omegaZ],
144                     gravity = g,
145                     graphicsDataList = [graphicsBodyWheel])
146
147
148
149#ground body and marker
150#graphicsPlane = graphics.Brick(centerPoint=[0,0,-0.1],size=[3*d,3*d,0.2], color=graphics.color.grey)
151graphicsPlane = graphics.CheckerBoard(point=vOff, normal=planeNormal, size=3*d)
152
153oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=[graphicsPlane])))
154markerSupportGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,r]))
155
156#markers for rigid body:
157markerBody0J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[-0.5*bodyDim[0],0,0]))
158markerBody0J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[0.5*bodyDim[0],0,0]))
159
160markerBodyWheel = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bWheel, localPosition=[0,0,0]))
161
162mbs.AddObject(SphericalJoint(markerNumbers=[markerSupportGround,markerBody0J0],
163                              constrainedAxes=[1,1,1],
164                              visualization=VObjectJointSpherical(jointRadius=0.025)))
165
166mbs.AddObject(GenericJoint(markerNumbers=[markerBody0J1, markerBodyWheel],
167                            constrainedAxes=[0*1,1,1,0,1,1],
168                            visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.1)))
169
170# rolling disc joint:
171markerRollingPlane = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=vOff))
172#oRolling=mbs.AddObject(ObjectJointRollingDisc(markerNumbers=[markerRollingPlane,markerBody0J1],
173
174if True:
175    oRolling=mbs.AddObject(ObjectJointRollingDisc(markerNumbers=[markerRollingPlane,markerBodyWheel],
176                                                  constrainedAxes=[1,1,1], #note that tangential constraints lead to additional forces on ground ==> changes force on ground!
177                                                  discRadius=r,
178                                                  #planeNormal = planeNormal,
179                                                  visualization=VObjectJointRollingDisc(discWidth=w,color=graphics.color.blue)))
180else:
181    nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
182    oRolling=mbs.AddObject(ObjectConnectorRollingDiscPenalty(markerNumbers=[markerRollingPlane,markerBodyWheel],
183                                                             nodeNumber=nGeneric,
184                                                             discRadius=r, discAxis=[1,0,0],
185                                                             planeNormal = planeNormal,
186                                                             contactStiffness=1e5, contactDamping=1e3,
187                                                             dryFriction=[1,1],
188                                                             visualization=VObjectConnectorRollingDiscPenalty(discWidth=w,color=graphics.color.blue)))
189
190sForce = mbs.AddSensor(SensorObject(objectNumber=oRolling, storeInternal=True,
191                                    outputVariableType = exu.OutputVariableType.ForceLocal))
192
193sTrailVel = mbs.AddSensor(SensorObject(objectNumber=oRolling, storeInternal=True,
194                                   outputVariableType = exu.OutputVariableType.Velocity))
195
196sAngVel = mbs.AddSensor(SensorBody(bodyNumber=bWheel, storeInternal=True,
197                                   outputVariableType = exu.OutputVariableType.AngularVelocity))
198sAngVelLocal = mbs.AddSensor(SensorBody(bodyNumber=bWheel, storeInternal=True,
199                                   outputVariableType = exu.OutputVariableType.AngularVelocityLocal))
200sAngAcc = mbs.AddSensor(SensorBody(bodyNumber=bWheel, storeInternal=True,
201                                   outputVariableType = exu.OutputVariableType.AngularAcceleration))
202
203mbs.Assemble()
204
205
206simulationSettings = exu.SimulationSettings() #takes currently set values or default values
207useGraphics=False
208tEnd = 0.1
209if useGraphics:
210    tEnd = 2
211
212h = 0.001
213simulationSettings.timeIntegration.endTime = tEnd #0.2 for testing
214simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
215#simulationSettings.solutionSettings.solutionWritePeriod = 0.01
216#simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
217simulationSettings.timeIntegration.verboseMode = 1
218
219simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
220
221simulationSettings.timeIntegration.simulateInRealtime = True
222
223SC.visualizationSettings.connectors.showJointAxes = True
224SC.visualizationSettings.connectors.jointAxesLength = 0.3
225SC.visualizationSettings.connectors.jointAxesRadius = 0.08
226SC.visualizationSettings.openGL.lineWidth=2 #maximum
227SC.visualizationSettings.openGL.shadow=0.2
228SC.visualizationSettings.openGL.multiSampling = 4
229
230if useGraphics:
231    exu.StartRenderer()
232    if 'renderState' in exu.sys:
233        SC.SetRenderState(exu.sys['renderState'])
234    mbs.WaitForUserToContinue()
235
236exu.SolveDynamic(mbs, simulationSettings)
237
238if useGraphics:
239    SC.WaitForRenderEngineStopFlag()
240    exu.StopRenderer() #safely close rendering window!
241
242force = mbs.GetSensorValues(sForce)
243u = 1e-3*(abs(force[0]) + abs(force[1]) + abs(force[2]))
244exu.Print('rollingDiscTangentialForces: F=',force) #use x-coordinate
245exu.Print('solution of rollingDiscTangentialForces=',u) #use x-coordinate
246
247exudynTestGlobals.testError = u - (1.0342017388721547)
248exudynTestGlobals.testResult = u
249
250
251if True:
252    from exudyn.plot import PlotSensor
253    PlotSensor(mbs,closeAll=True)
254    PlotSensor(mbs, sensorNumbers=[sForce], components=[0,1,2])
255    # PlotSensor(mbs, sensorNumbers=sAngVel, components=[0,1,2])
256    # PlotSensor(mbs, sensorNumbers=sAngVelLocal, components=[0,1,2])
257    # PlotSensor(mbs, sensorNumbers=sAngAcc, components=[0,1,2])
258
259    # PlotSensor(mbs, sensorNumbers=sTrailVel, components=[0,1])