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