plotSensorTest.py
You can view and download this file on Github: plotSensorTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Test model and example for exudyn.plot.PlotSensor, using several sensors and plotting results
5#
6# Author: Johannes Gerstmayr
7# Date: 2021-07-01
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 *
15
16from math import sin, cos, pi
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#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
31SC = exu.SystemContainer()
32mbs = SC.AddSystem()
33# useGraphics=False
34
35#background
36color = [0.1,0.1,0.8,1]
37L = 0.4 #length of bodies
38d = 0.1 #diameter of bodies
39
40#create background, in order to have according zoom all
41zz=2*L
42#background0 = GraphicsDataRectangle(-zz,-zz,zz,zz,color4white)
43oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0])) #,visualization=VObjectGround(graphicsData= [background0])))
44mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround, localPosition=[0,0,0]))
45
46p0 = [0.,0.,0] #reference position
47vLoc = np.array([L,0,0]) #last to next joint
48
49g = [0,-9.81*0,0]
50
51inertia = InertiaCuboid(density=1000, sideLengths=[L,d,d])
52#p0 += Alist[i] @ (0.5*vLoc)
53p0 += (0.5*vLoc)
54
55ep0 = eulerParameters0 #no rotation
56graphicsBody = GraphicsDataOrthoCubePoint([0,0,0], [L,d,d], color4steelblue)
57oRB = mbs.CreateRigidBody(inertia=inertia,
58 referencePosition=p0,
59 gravity=g,
60 graphicsDataList=[graphicsBody])
61nRB= mbs.GetObject(oRB)['nodeNumber']
62
63mPos0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [-0.5*L,0,0]))
64mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [0.5*L,0,0]))
65
66oRJ = mbs.AddObject(ObjectJointRevoluteZ(markerNumbers = [mGround, mPos0],
67 visualization=VObjectJointRevoluteZ(axisRadius=0.5*d, axisLength=1.2*d) ))
68
69def UFload(mbs,t,load):
70 return [0,load[1]*np.cos(t*2*np.pi),0]
71
72lForce = mbs.AddLoad(LoadForceVector(markerNumber=mPosLast, loadVector=[0,2,0], loadVectorUserFunction=UFload))
73
74filedir = 'solution/'
75#these test are really written to files (most other tests use internal storage):
76sLoad = mbs.AddSensor(SensorLoad(loadNumber=lForce, fileName=filedir+'plotSensorLoad.txt'))
77sNode = mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName=filedir+'plotSensorNode.txt', outputVariableType=exu.OutputVariableType.Coordinates))
78sNode2 = mbs.AddSensor(SensorNode(nodeNumber=nRB, fileName=filedir+'plotSensorNodeRotation.txt', outputVariableType=exu.OutputVariableType.Rotation))
79sBody = mbs.AddSensor(SensorBody(bodyNumber=oRB, fileName=filedir+'plotSensorBody.txt', localPosition=[0.5*L,0,0], outputVariableType=exu.OutputVariableType.Position))
80sObject = mbs.AddSensor(SensorObject(objectNumber=oRJ, fileName=filedir+'plotSensorObject.txt', outputVariableType=exu.OutputVariableType.ForceLocal))
81sMarker = mbs.AddSensor(SensorMarker(markerNumber=mPosLast, fileName=filedir+'plotSensorMarker.txt', outputVariableType=exu.OutputVariableType.Position))
82
83mbs.Assemble()
84
85simulationSettings = exu.SimulationSettings() #takes currently set values or default values
86
87tEnd = 1
88h=0.01 #use small step size to detext contact switching
89
90simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
91simulationSettings.timeIntegration.endTime = tEnd
92simulationSettings.solutionSettings.solutionWritePeriod = 0.01
93simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
94simulationSettings.timeIntegration.verboseMode = 1
95# simulationSettings.timeIntegration.simulateInRealtime = True
96
97# simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8
98# simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
99simulationSettings.timeIntegration.newton.useModifiedNewton = True
100
101# SC.visualizationSettings.nodes.show = True
102# SC.visualizationSettings.nodes.drawNodesAsPoint = False
103# SC.visualizationSettings.nodes.showBasis = True
104# SC.visualizationSettings.nodes.basisSize = 0.015
105# SC.visualizationSettings.connectors.showJointAxes = True
106
107SC.visualizationSettings.general.autoFitScene = False #use loaded render state
108#useGraphics = False
109if useGraphics:
110 exu.StartRenderer()
111 if 'renderState' in exu.sys:
112 SC.SetRenderState(exu.sys[ 'renderState' ])
113 #mbs.WaitForUserToContinue()
114else:
115 simulationSettings.solutionSettings.writeSolutionToFile = False
116
117#mbs.SolveDynamic(simulationSettings, solverType=exu.DynamicSolverType.TrapezoidalIndex2)
118mbs.SolveDynamic(simulationSettings, showHints=True)
119
120#%%+++++++++++++++++++++++++++++
121if useGraphics:
122 #SC.WaitForRenderEngineStopFlag()
123 exu.StopRenderer() #safely close rendering window!
124
125
126exudynTestGlobals.testError = 0
127exudynTestGlobals.testResult = 1
128
129import matplotlib.pyplot as plt
130
131
132closeAll = not useGraphics
133mbs.PlotSensor(sensorNumbers=sLoad, components=[0,1,2], closeAll=closeAll)
134mbs.PlotSensor(sensorNumbers=sNode, components=[0,1,2,3,4,5,6],
135 yLabel='Coordinates with offset 1\nand scaled with $\\frac{1}{1000}$',
136 factors=1e-3, offsets=1,fontSize=12, closeAll=closeAll)
137mbs.PlotSensor(sensorNumbers=sNode2, components=[0,1,2], closeAll=closeAll)
138mbs.PlotSensor(sensorNumbers=sNode2, components=[0,1,2], closeAll=closeAll)
139mbs.PlotSensor(sensorNumbers=[sBody]*3+[sMarker]*3, components=[0,1,2,0,1,2],
140 colorCodeOffset=3, newFigure=closeAll, fontSize=10,
141 yLabel='Rotation $\\alpha, \\beta, \\gamma$ and\n Position $x,y,z$', closeAll=closeAll)
142mbs.PlotSensor(sensorNumbers=sObject, components=[0,1,2], title='Revolute joint forces', closeAll=closeAll)
143mbs.PlotSensor(sensorNumbers=[sNode]*3+ [filedir+'plotSensorNode.txt']*3, components=[0,1,2]*2, closeAll=closeAll)
144
145if closeAll:
146 plt.close('all')
147
148import os
149for s in range(mbs.systemData.NumberOfSensors()):
150 fileName=mbs.GetSensor(s)['fileName']
151 exu.Print('remove file:', fileName)
152 os.remove(fileName)