simulationSettings.timeIntegration.numberOfSteps = int(tEnd / h)
simulationSettings.timeIntegration.endTime = tEnd
simulationSettings.timeIntegration.verboseMode = 1
simulationSettings.timeIntegration.newton.useModifiedNewton = True
simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
simulationSettings.displayStatistics = True
#simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
#SC.visualizationSettings.nodes.defaultSize = 0.05
simulationSettings.solutionSettings.solutionInformation = "Finite segment method"
exu.StartRenderer()
if mode == "Trap":
exu.SolveDynamic(mbs,
simulationSettings,
solverType=exu.DynamicSolverType.TrapezoidalIndex2)
else:
exu.SolveDynamic(mbs, simulationSettings)
SC.WaitForRenderEngineStopFlag()
#SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!
if True and useANCF:
from exudyn.plot import PlotSensor
PlotSensor(mbs, sensorNumbers=[sTipCable, sTipSegment],
components=[1, 1]) #plot y components
simulationSettings = exu.SimulationSettings() #takes currently set values or default values
tEnd = 4 #simulation time
h = 1e-3 #step size
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
simulationSettings.timeIntegration.endTime = tEnd
simulationSettings.timeIntegration.verboseMode = 1
simulationSettings.timeIntegration.simulateInRealtime = True
SC.visualizationSettings.window.renderWindowSize=[1600,1200]
SC.visualizationSettings.openGL.multiSampling = 4
SC.visualizationSettings.general.autoFitScene = False
exu.StartRenderer()
if 'renderState' in exu.sys: #reload old view
SC.SetRenderState(exu.sys['renderState'])
mbs.WaitForUserToContinue() #stop before simulating
exu.SolveDynamic(mbs, simulationSettings = simulationSettings,
solverType=exu.DynamicSolverType.TrapezoidalIndex2)
SC.WaitForRenderEngineStopFlag() #stop before closing
exu.StopRenderer() #safely close rendering window!
if True:
from exudyn.plot import PlotSensor
PlotSensor(mbs, [sens1],[1])
def UFsensor(mbs, t, sensorNumbers, factors, configuration):
val = mbs.GetSensorValues(sensorNumbers[0]) #x,y,z
phi = atan2(val[1], val[0]) #compute angle from x,y: atan2(y,x)
#print("phi=", factors[0]*phi)
#print("x,y,z", val)
return [factors[0] * phi] #return angle in degree
sUser = mbs.AddSensor(
SensorUserFunction(sensorNumbers=[sNode],
factors=[180 / pi],
fileName='solution/sensorTestPhi.txt',
writeToFile=exudynTestGlobals.useGraphics,
sensorUserFunction=UFsensor))
#assemble and solve system for default parameters
mbs.Assemble()
exu.SolveDynamic(mbs)
#evaluate final (=current) output values
u = mbs.GetSensorValues(sUser)
exu.Print('sensor=', u)
exudynTestGlobals.testResult = u #should be 45 degree finally
#+++++++++++++++++++++++++++++++++++++++++++++++++++++
if exudynTestGlobals.useGraphics:
from exudyn.plot import PlotSensor
PlotSensor(mbs, [sNode, sNode, sUser], [0, 1, 0])
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
simulationSettings.timeIntegration.startTime = 1
simulationSettings.solutionSettings.appendToFile = True #continue solution
simulationSettings.timeIntegration.endTime = tEnd
success = exu.SolveDynamic(mbs, simulationSettings,
exudyn.DynamicSolverType.TrapezoidalIndex2)
if useGraphics:
SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!
plt.close('all')
if True:
from exudyn.plot import PlotSensor
plt.figure("ALE pos/vel")
PlotSensor(mbs, sensorNumbers=[mbs.variables['sALEpos'],mbs.variables['sALEvel']], components=[0,0])
plt.figure("midpoint")
data0 = np.loadtxt('solution/beamALEmidPoint.txt', comments='#', delimiter=',')
y0 = data0[0,2]
plt.plot(data0[:,0],data0[:,2]-y0,'b-',label='midPointDeflection')
ax=plt.gca()
ax.grid(True,'major','both')
plt.tight_layout()
plt.legend()
plt.show()
# exu.SolveDynamic(mbs, solverType=exu.DynamicSolverType.TrapezoidalIndex2,
# simulationSettings=simulationSettings)
exu.SolveDynamic(mbs, simulationSettings=simulationSettings)
else:
exu.SolveStatic(mbs, simulationSettings=simulationSettings)
# uTip = mbs.GetSensorValues(sensTipDispl)[1]
# print("nModes=", nModes, ", tip displacement=", uTip)
if useGraphics:
SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!
if False:
from exudyn.plot import PlotSensor
PlotSensor(mbs, sensorNumbers=[sensBushingVel], components=[1])
#%%
if False:
import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
import exudyn as exu
from exudyn.utilities import *
CC = PlotLineCode
comp = 1 #1=x, 2=y, ...
var = ''
# data = np.loadtxt('solution/hingePartBushing'+var+'2.txt', comments='#', delimiter=',')
# plt.plot(data[:,0], data[:,comp], CC(7), label='2 eigenmodes')
# data = np.loadtxt('solution/hingePartBushing'+var+'4.txt', comments='#', delimiter=',')
# plt.plot(data[:,0], data[:,comp], CC(8), label='4 eigenmodes')
data = np.loadtxt('solution/hingePartBushing'+var+'8.txt', comments='#', delimiter=',')
useGraphics = True
if useGraphics:
exu.StartRenderer()
if 'renderState' in exu.sys:
SC.SetRenderState(exu.sys['renderState'])
mbs.WaitForUserToContinue()
exu.SolveDynamic(mbs, simulationSettings)
if useGraphics:
SC.WaitForRenderEngineStopFlag()
SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!
##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
#plot results
if addSensors:
from exudyn.plot import PlotSensor
PlotSensor(mbs,
sensorNumbers=[
listSensors[0],
listSensors[1],
],
components=[0, 0])
if addSensors:
x = np.loadtxt('solution/energyDoubleFourBar.txt',
comments='#',
delimiter=',')
print("max energy error=", max(abs(x[:, 1])))
if addSensors:
#plot results
from exudyn.plot import PlotSensor
import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
plt.close('all')
# PlotSensor(mbs, sensorNumbers=[sBpos,sBpos,sBpos], components=[0,1,2])
#plt.figure('lateral position')
#PlotSensor(mbs, sensorNumbers=[sBpos], components=[1])
plt.figure('forward velocity')
PlotSensor(mbs, sensorNumbers=[sForwardVel], components=[0])
# PlotSensor(mbs, sensorNumbers=[sBvelLocal,sBvelLocal,sBvelLocal], components=[0,1,2])
# plt.figure('local ang velocities')
# PlotSensor(mbs, sensorNumbers=[sBAngVelLocal,sBAngVelLocal,sBAngVelLocal], components=[0,1,2])
# if False:
# import matplotlib.pyplot as plt
# import matplotlib.ticker as ticker
# 1=roll angle, 2=roll angular velocity, 3=forward speed, 4=potential energy, 5=kinetic energy, 6=mechanical energy, 7=steer angle, and 8=steer velocity
data = np.loadtxt('solution/uncontrolledBicycleGonzalez.txt')#, comments='#', delimiter='')
plt.plot(data[:,0], data[:,9], 'b:',label='')
data2 = np.loadtxt('solution/uncontrolledBicycleSanjurjo.txt')#, comments='#', delimiter='')
plt.plot(data2[:,0], data2[:,3+8], 'g:',label='')
#RK22:
#h=10**-1 , val= -9.614173942611721
#h=10**-2 , val= -0.029910241095991663
#h=10**-3 , val= -0.0003033091724236603
#h=10**-4 , val= -3.0421319984763606e-06
#h=10**-5 , val= -3.037840295982974e-08
#mbs.GetSensorValues(sCoords2),
#h=1e-4, tEnd = 2:
#Expl. Euler: [0.4121895 0.01004991]
#Trapez. rule:[0.40808358 0.01004968]
#h=1e-5, tEnd = 2:
#Expl. Euler: [0.40849041 0.01004971]
#Trapez. rule:[0.40808208 0.01004969]
#h=1e-6, tEnd = 2:
#Expl. Euler: [0.40812287 0.01004969]
#RK4: [0.40808206 0.01004969]
#exu.Print("ODE1 end values=",mbs.GetSensorValues(sCoords1))
exu.Print("solverExplicitODE1ODE2 err=", err)
exudynTestGlobals.testError = err - (3.3767933275918964
) #2021-01-25: 3.3767933275918964
exudynTestGlobals.testResult = err
#+++++++++++++++++++++++++++++++++++++++++++++++++++++
if False:
from exudyn.plot import PlotSensor
PlotSensor(mbs, [sCoords1], [1])
PlotSensor(mbs, [sCoords2], [1])
if useGraphics:
exu.StartRenderer()
if 'renderState' in exu.sys:
SC.SetRenderState(exu.sys[ 'renderState' ])
mbs.WaitForUserToContinue()
exu.SolveDynamic(mbs, simulationSettings)
if useGraphics:
SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!
##++++++++++++++++++++++++++++++++++++++++++++++q+++++++
#plot results
from exudyn.plot import PlotSensor
PlotSensor(mbs, sensorNumbers=[sLeg,sFemoral], components=[0,0])
if False:
import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
data = np.loadtxt('solution/rollingDiscPos.txt', comments='#', delimiter=',')
plt.plot(data[:,0], data[:,1], 'r-',label='coin pos x')
plt.plot(data[:,0], data[:,2], 'g-',label='coin pos y')
plt.plot(data[:,0], data[:,3], 'b-',label='coin pos z')
ax=plt.gca() # get current axes
ax.grid(True, 'major', 'both')
ax.xaxis.set_major_locator(ticker.MaxNLocator(10)) #use maximum of 8 ticks on y-axis
ax.yaxis.set_major_locator(ticker.MaxNLocator(10)) #use maximum of 8 ticks on y-axis
exu.StartRenderer() #start graphics visualization
#mbs.WaitForUserToContinue() #wait for pressing SPACE bar to continue
#start solver:
exu.SolveDynamic(mbs,
solverType=exu.DynamicSolverType.TrapezoidalIndex2,
simulationSettings=simulationSettings)
#exu.SolveDynamic(mbs, solverType=exu.DynamicSolverType.RK67, simulationSettings=simulationSettings)
if exudynTestGlobals.useGraphics:
#SC.WaitForRenderEngineStopFlag()#wait for pressing 'Q' to quit
exu.StopRenderer() #safely close rendering window!
u = mbs.GetNodeOutput(n1, exu.OutputVariableType.Position)
exu.Print('postNewtonStepContactTest=', u[1])
exudynTestGlobals.testError = u[1] - (0.057286638346409235)
exudynTestGlobals.testResult = u[1]
if exudynTestGlobals.useGraphics:
from exudyn.plot import PlotSensor
import matplotlib.pyplot as plt
plt.close('all')
plt.figure('Pos')
PlotSensor(mbs, sensorNumbers=[sPos, sPosRef], components=[1, 1])
plt.figure('Vel')
PlotSensor(mbs, sensorNumbers=[sVel, sVelRef], components=[1, 1])
plt.figure('Acc')
PlotSensor(mbs, sensorNumbers=[sAcc, sAccRef], components=[1, 1])
def ParameterFunction(parameterSet):
s1=L1*0.5
s2=L2*0.5
if False:
s1 = s1opt
s2 = s2opt
if 's1' in parameterSet:
s1 = parameterSet['s1']
h=0.002
if 'h' in parameterSet:
h = parameterSet['h']
if 's2' in parameterSet:
s2 = parameterSet['s2']
iCalc = 'Ref' #needed for parallel computation ==> output files are different for every computation
if 'computationIndex' in parameterSet:
iCalc = str(parameterSet['computationIndex'])
#print("computation index=",iCalc, flush=True)
SC = exu.SystemContainer()
mbs = SC.AddSystem()
testCases = 1 #floating body
nGround = mbs.AddNode(NodePointGround(referenceCoordinates=[0,0,0])) #ground node for coordinate constraint
mGround = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nGround, coordinate=0)) #Ground node ==> no action
#++++++++++++++++++++++++++++++++
#floating body to mount slider-crank mechanism
constrainGroundBody = (testCases == 0) #use this flag to fix ground body
#graphics for floating frame:
gFloating = GraphicsDataOrthoCube(-0.25, -0.25, -0.1, 0.8, 0.25, -0.05, color=[0.3,0.3,0.3,1.])
if constrainGroundBody:
floatingRB = mbs.AddObject(ObjectGround(referencePosition=[0,0,0], visualization=VObjectGround(graphicsData=[gFloating])))
mFloatingN = mbs.AddMarker(MarkerBodyPosition(bodyNumber = floatingRB, localPosition=[0,0,0]))
else:
nFloating = mbs.AddNode(Rigid2D(referenceCoordinates=[0,0,0], initialVelocities=[0,0,0]));
mFloatingN = mbs.AddMarker(MarkerNodePosition(nodeNumber=nFloating))
floatingRB = mbs.AddObject(RigidBody2D(physicsMass=2, physicsInertia=1, nodeNumber=nFloating, visualization=VObjectRigidBody2D(graphicsData=[gFloating])))
mRB0 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nFloating, coordinate=0))
mRB1 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nFloating, coordinate=1))
mRB2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nFloating, coordinate=2))
#add spring dampers for reference frame:
k=5000 #stiffness of floating body
d=k*0.01
mbs.AddObject(CoordinateSpringDamper(markerNumbers=[mGround,mRB0], stiffness=k, damping=d))
mbs.AddObject(CoordinateSpringDamper(markerNumbers=[mGround,mRB1], stiffness=k, damping=d))
mbs.AddObject(CoordinateSpringDamper(markerNumbers=[mGround,mRB2], stiffness=k, damping=d))
mbs.AddObject(CoordinateConstraint(markerNumbers=[mGround,mRB2]))
#++++++++++++++++++++++++++++++++
#nodes and bodies
omega=2*pi/60*300 #3000 rpm
M=0.1 #torque (default: 0.1)
s1L=-s1
s1R=L1-s1
s2L=-s2
s2R=L2-s2
#lambda=L1/L2
J1=(m1/12.)*L1**2 #inertia w.r.t. center of mass
J2=(m2/12.)*L2**2 #inertia w.r.t. center of mass
ty = 0.05 #thickness
tz = 0.05 #thickness
graphics1 = GraphicsDataRigidLink(p0=[s1L,0,-0.5*tz],p1=[s1R,0,-0.5*tz],
axis0=[0,0,1], axis1=[0,0,1],radius=[0.5*ty,0.5*ty],
thickness=0.8*ty, width=[tz,tz], color=color4steelblue,nTiles=16)
graphics2 = GraphicsDataRigidLink(p0=[s2L,0,0.5*tz],p1=[s2R,0,0.5*tz],
axis0=[0,0,1], axis1=[0,0,1],radius=[0.5*ty,0.5*ty],
thickness=0.8*ty, width=[tz,tz], color=color4lightred,nTiles=16)
#crank:
nRigid1 = mbs.AddNode(Rigid2D(referenceCoordinates=[s1,0,0],
initialVelocities=[0,0,0]));
oRigid1 = mbs.AddObject(RigidBody2D(physicsMass=m1,
physicsInertia=J1,
nodeNumber=nRigid1,
visualization=VObjectRigidBody2D(graphicsData= [graphics1])))
#connecting rod:
nRigid2 = mbs.AddNode(Rigid2D(referenceCoordinates=[L1+s2,0,0],
initialVelocities=[0,0,0]));
oRigid2 = mbs.AddObject(RigidBody2D(physicsMass=m2,
physicsInertia=J2,
nodeNumber=nRigid2,
visualization=VObjectRigidBody2D(graphicsData= [graphics2])))
#++++++++++++++++++++++++++++++++
#slider:
c=0.025 #dimension of mass
graphics3 = GraphicsDataOrthoCube(-c,-c,-c*2,c,c,0,color4grey)
#nMass = mbs.AddNode(Point2D(referenceCoordinates=[L1+L2,0]))
#oMass = mbs.AddObject(MassPoint2D(physicsMass=m3, nodeNumber=nMass,visualization=VObjectMassPoint2D(graphicsData= [graphics3])))
nMass = mbs.AddNode(Rigid2D(referenceCoordinates=[L1+L2,0,0]))
oMass = mbs.AddObject(RigidBody2D(physicsMass=m3, physicsInertia=0.001*m3, nodeNumber=nMass,visualization=VObjectRigidBody2D(graphicsData= [graphics3])))
#++++++++++++++++++++++++++++++++
#markers for joints:
mR1Left = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRigid1, localPosition= [s1L,0.,0.])) #support point # MUST be a rigidBodyMarker, because a torque is applied
mR1Right = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid1, localPosition=[s1R,0.,0.])) #end point; connection to connecting rod
mR2Left = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition= [s2L,0.,0.])) #connection to crank
mR2Right = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid2, localPosition=[s2R,0.,0.])) #end point; connection to slider
mMass = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oMass, localPosition=[ 0.,0.,0.]))
mG0 = mFloatingN
#++++++++++++++++++++++++++++++++
#joints:
mbs.AddObject(RevoluteJoint2D(markerNumbers=[mG0,mR1Left]))
mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR1Right,mR2Left]))
mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR2Right,mMass]))
#prismatic joint:
mRigidGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber = floatingRB, localPosition = [L1+L2,0,0]))
mRigidSlider = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oMass, localPosition = [0,0,0]))
mbs.AddObject(PrismaticJoint2D(markerNumbers=[mRigidGround,mRigidSlider], constrainRotation=True))
#user function for load; switch off load after 1 second
userLoadOn = True
def userLoad(mbs, t, load):
setLoad = 0
if userLoadOn:
setLoad = load
omega = mbs.GetNodeOutput(nRigid1,variableType = exu.OutputVariableType.AngularVelocity)[2]
if omega > 2*pi*2:
#print("t=",t)
userLoadOn = False
return setLoad
#loads and driving forces:
mRigid1CoordinateTheta = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber = nRigid1, coordinate=2)) #angle coordinate is constrained
#mbs.AddLoad(LoadCoordinate(markerNumber=mRigid1CoordinateTheta, load = M, loadUserFunction=userLoad)) #torque at crank
mbs.AddLoad(LoadCoordinate(markerNumber=mRigid1CoordinateTheta, load = M)) #torque at crank
#write motion of support frame:
sensorFileName = 'solution/floatingPos'+iCalc+'.txt'
sFloating = mbs.AddSensor(SensorNode(nodeNumber=nFloating, fileName=sensorFileName,
outputVariableType=exu.OutputVariableType.Position))
#++++++++++++++++++++++++++++++++
#assemble, adjust settings and start time integration
mbs.Assemble()
simulationSettings = exu.SimulationSettings() #takes currently set values or default values
tEnd = 3
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
simulationSettings.timeIntegration.endTime = tEnd
#simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8 #10000
#simulationSettings.timeIntegration.verboseMode = 1 #10000
simulationSettings.solutionSettings.solutionWritePeriod = 2e-3
simulationSettings.solutionSettings.writeSolutionToFile = useGraphics
simulationSettings.timeIntegration.newton.useModifiedNewton = True
simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8
simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-8
#++++++++++++++++++++++++++++++++++++++++++
#solve index 2 / trapezoidal rule:
simulationSettings.timeIntegration.generalizedAlpha.useNewmark = True
simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
dSize = 0.02
SC.visualizationSettings.nodes.defaultSize = dSize
SC.visualizationSettings.markers.defaultSize = dSize
SC.visualizationSettings.bodies.defaultSize = [dSize, dSize, dSize]
SC.visualizationSettings.connectors.defaultSize = dSize
#data obtained from SC.GetRenderState(); use np.round(d['modelRotation'],4)
SC.visualizationSettings.openGL.initialModelRotation = [[ 0.87758, 0.04786, -0.47703],
[ 0. , 0.995 , 0.09983],
[ 0.47943, -0.08761, 0.8732]]
SC.visualizationSettings.openGL.initialZoom = 0.47
SC.visualizationSettings.openGL.initialCenterPoint = [0.192, -0.0039,-0.075]
SC.visualizationSettings.openGL.initialMaxSceneSize = 0.4
SC.visualizationSettings.general.autoFitScene = False
#mbs.WaitForUserToContinue()
if useGraphics:
exu.StartRenderer()
exu.SolveDynamic(mbs, simulationSettings)
if useGraphics:
#+++++++++++++++++++++++++++++++++++++
#animate solution
# mbs.WaitForUserToContinue
# fileName = 'coordinatesSolution.txt'
# solution = LoadSolutionFile('coordinatesSolution.txt')
# AnimateSolution(mbs, solution, 10, 0.025, True)
#+++++++++++++++++++++++++++++++++++++
SC.WaitForRenderEngineStopFlag()
exu.StopRenderer() #safely close rendering window!
#++++++++++++++++++++++++++++++++++++++++++
#evaluate error:
data = np.loadtxt(sensorFileName, comments='#', delimiter=',')
errorNorm = max(abs(data[:,1])) + max(abs(data[:,2])) #max displacement in x and y direction
#++++++++++++++++++++++++++++++++++++++++++
#clean up optimization
if True: #delete files; does not work for parallel, consecutive operation
if iCalc != 'Ref':
os.remove(sensorFileName) #remove files in order to clean up
while(os.path.exists(sensorFileName)): #wait until file is really deleted -> usually some delay
sleep(0.001) #not nice, but there is no other way than that
if useGraphics:
print("max. oszillation=", errorNorm)
from exudyn.plot import PlotSensor
PlotSensor(mbs, sensorNumbers=[sFloating,sFloating], components=[0,1])
del mbs
del SC
return errorNorm