solutionViewerTest.py
You can view and download this file on Github: solutionViewerTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Test for AddRevoluteJoint utility function
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.itemInterface import *
15from exudyn.utilities import *
16
17from math import sin, cos, pi
18import numpy as np
19
20SC = exu.SystemContainer()
21mbs = SC.AddSystem()
22
23
24#background
25color = [0.1,0.1,0.8,1]
26L = 0.4 #length of bodies
27d = 0.1 #diameter of bodies
28
29oGround=mbs.AddObject(ObjectGround(referencePosition= [-0.5*L,0,0]))
30mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround, localPosition=[0,0,0]))
31A0 = np.eye(3)
32Alast = A0 #previous marker
33bodyLast = oGround
34
35Alist=[]
36axisList=[]
37A=RotationMatrixX(0)
38
39nBodies = 100
40for i in range(nBodies):
41 delta = 0.01*pi
42 #A = RotationMatrixZ(delta)
43 v0= A@[0,0,1]
44 Alist+=[A]
45 axisList+=[v0]
46 A = A @ RotationMatrixX(delta)@RotationMatrixZ(2*delta)
47
48
49p0 = [0.,0.,0] #reference position
50vLoc = np.array([L,0,0]) #last to next joint
51g = [0,0,9.81]
52#g = [0,9.81,0]
53
54#create a chain of bodies:
55for i in range(nBodies):
56 #print("Build Object", i)
57 inertia = InertiaCuboid(density=1000, sideLengths=[L,d,d])
58 p0 += Alist[i] @ (0.5*vLoc)
59 #p0 += (0.5*vLoc)
60
61 ep0 = eulerParameters0 #no rotation
62 graphicsBody = GraphicsDataOrthoCubePoint([0,0,0], [0.96*L,d,d], color4steelblue)
63 oRB = mbs.CreateRigidBody(inertia=inertia,
64 referencePosition=p0,
65 referenceRotationMatrix=Alist[i],
66 gravity=g,
67 graphicsDataList=[graphicsBody])
68 nRB= mbs.GetObject(oRB)['nodeNumber']
69
70 body0 = bodyLast
71 body1 = oRB
72 # point = mbs.GetObjectOutputBody(oRB,exu.OutputVariableType.Position,
73 # localPosition=[-0.5*L,0,0],
74 # configuration=exu.ConfigurationType.Reference)
75 #axis = [0,0,1]
76 axis = axisList[i]
77 mbs.CreateRevoluteJoint(bodyNumbers=[body0, body1], position=[0.5*L,0,0],
78 axis=Alast.T@axis, useGlobalFrame=False,
79 axisRadius=0.6*d, axisLength=1.2*d)
80 # mbs.CreateRevoluteJoint(bodyNumbers=[body0, body1], position=point,
81 # axis=axis, useGlobalFrame=True,
82 # axisRadius=0.6*d, axisLength=1.2*d)
83
84 bodyLast = oRB
85
86 p0 += Alist[i] @ (0.5*vLoc)
87 #p0 += (0.5*vLoc)
88 Alast = Alist[i]
89
90#mbs.AddLoad(LoadForceVector(markerNumber=mPosLast, loadVector=[0,0,20]))
91
92mbs.Assemble()
93
94simulationSettings = exu.SimulationSettings() #takes currently set values or default values
95
96tEnd = 1
97h=0.0005 #use small step size to detext contact switching
98
99simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
100simulationSettings.timeIntegration.endTime = tEnd
101simulationSettings.solutionSettings.solutionWritePeriod = 0.005
102simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
103#simulationSettings.timeIntegration.simulateInRealtime = True
104simulationSettings.timeIntegration.realtimeFactor = 0.5
105simulationSettings.timeIntegration.verboseMode = 1
106
107simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8
108simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
109simulationSettings.timeIntegration.newton.useModifiedNewton = True
110#simulationSettings.timeIntegration.newton.modifiedNewtonJacUpdatePerStep = True
111simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
112# simulationSettings.parallel.numberOfThreads=4
113
114SC.visualizationSettings.nodes.show = True
115SC.visualizationSettings.nodes.drawNodesAsPoint = False
116SC.visualizationSettings.nodes.showBasis = True
117SC.visualizationSettings.nodes.basisSize = 0.015
118SC.visualizationSettings.connectors.showJointAxes = True
119
120#for snapshot:
121SC.visualizationSettings.openGL.multiSampling=4
122SC.visualizationSettings.openGL.lineWidth=2
123SC.visualizationSettings.window.renderWindowSize = [800,600]
124SC.visualizationSettings.general.drawCoordinateSystem=False
125SC.visualizationSettings.general.drawWorldBasis=True
126# SC.visualizationSettings.general.useMultiThreadedRendering = False
127SC.visualizationSettings.general.autoFitScene = False #use loaded render state
128useGraphics = True
129if useGraphics:
130 simulationSettings.displayComputationTime = True
131 simulationSettings.displayStatistics = True
132 exu.StartRenderer()
133 if 'renderState' in exu.sys:
134 SC.SetRenderState(exu.sys[ 'renderState' ])
135 #mbs.WaitForUserToContinue()
136else:
137 simulationSettings.solutionSettings.writeSolutionToFile = False
138
139#mbs.SolveDynamic(simulationSettings, solverType=exu.DynamicSolverType.TrapezoidalIndex2)
140mbs.SolveDynamic(simulationSettings, showHints=True)
141
142if True: #use this to reload the solution and use SolutionViewer
143 #sol = LoadSolutionFile('coordinatesSolution.txt')
144
145 mbs.SolutionViewer() #can also be entered in IPython ...
146
147
148u0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Displacement)
149rot0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Rotation)
150exu.Print('u0=',u0,', rot0=', rot0)
151
152result = (abs(u0)+abs(rot0)).sum()
153exu.Print('solution of addRevoluteJoint=',result)
154
155
156
157#%%+++++++++++++++++++++++++++++
158if useGraphics:
159 SC.WaitForRenderEngineStopFlag()
160 exu.StopRenderer() #safely close rendering window!