genericJointUserFunctionTest.py
You can view and download this file on Github: genericJointUserFunctionTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Test for GenericJoint with userFunction
5#
6# Author: Johannes Gerstmayr
7# Date: 2020-04-22
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 *
15from exudyn.FEM import *
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#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
31SC = exu.SystemContainer()
32mbs = SC.AddSystem()
33
34nBodies = 5
35color = [0.1,0.1,0.8,1]
36s = 0.1 #width of cube
37sx = 3*s #length of cube/body
38cPosZ = 0. #offset of constraint in z-direction
39zz = sx * (nBodies+1)*2 #max size of background
40
41background0 = GraphicsDataRectangle(-zz,-zz,zz,2.5*sx,color)
42oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
43 visualization=VObjectGround(graphicsData= [background0])))
44mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround,
45 localPosition=[-sx,0,0]))
46oRB = -1 #for output value
47
48def UFgenericJoint(mbs, t, itemIndex, param):
49 phiZ = -2*pi*0.5*(1.-np.cos(t*2*pi / 4)) / nBodies
50 phiX = -0.5*pi*0.5*(1.-np.cos(t*2*pi / 4)) / nBodies
51 #exu.Print("phi=", phi)
52 return [0,0,0, phiX,0,phiZ]
53
54#create a chain of bodies:
55for i in range(nBodies):
56 f = 0 #factor for initial velocities
57 omega0 = [0,50.*f,20*f] #arbitrary initial angular velocity
58 ep0 = eulerParameters0 #no rotation
59 ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
60
61 p0 = [-sx*0+i*2*sx,0.,0] #reference position
62 v0 = [0.2*f,0.,0.] #initial translational velocity
63
64 nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0,
65 initialVelocities=v0+list(ep_t0)))
66 #nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=[0,0,0,1,0,0,0], initialVelocities=[0,0,0,0,0,0,0]))
67 oGraphics = GraphicsDataOrthoCubeLines(-sx,-s,-s, sx,s,s, [0.8,0.1,0.1,1])
68 oRB = mbs.AddObject(ObjectRigidBody(physicsMass=2,
69 physicsInertia=[6,1,6,0,0,0],
70 nodeNumber=nRB,
71 physicsCenterOfMass=[0,0,0],
72 visualization=VObjectRigidBody(graphicsData=[oGraphics])))
73
74 mMassRB = mbs.AddMarker(MarkerBodyMass(bodyNumber = oRB))
75 mbs.AddLoad(Gravity(markerNumber = mMassRB, loadVector=[0.,-9.81,0.])) #gravity in negative z-direction
76
77 if i==0:
78 mPos = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [-sx,0.,cPosZ*0]))
79 else:
80 mPos = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [-sx,0.,cPosZ]))
81
82 val=1
83 if i==0: val=0
84 if i < 2:
85 mbs.AddObject(GenericJoint(markerNumbers = [mPos, mPosLast], constrainedAxes=[val,1,1, 1,1,1],
86 offsetUserFunctionParameters=[0,0,0, 0,0,0],
87 offsetUserFunction=UFgenericJoint))
88 else:
89 if i==3:
90 mbs.AddObject(GenericJoint(markerNumbers = [mPosLast, mPos], constrainedAxes=[1,1,1, 1,0,1]))
91 elif i==4:
92 mbs.AddObject(GenericJoint(markerNumbers = [mPosLast, mPos], constrainedAxes=[1,1,1, 1,1,0]))
93 else:
94 mbs.AddObject(SphericalJoint(markerNumbers = [mPosLast, mPos], constrainedAxes=[1,1,1]))
95
96 #marker for next chain body
97 mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [sx,0.,cPosZ]))
98
99
100mbs.Assemble()
101#exu.Print(mbs)
102
103simulationSettings = exu.SimulationSettings() #takes currently set values or default values
104
105fact = 500
106simulationSettings.timeIntegration.numberOfSteps = 1*fact
107simulationSettings.timeIntegration.endTime = 0.005*fact
108simulationSettings.solutionSettings.solutionWritePeriod = simulationSettings.timeIntegration.endTime/fact*10
109simulationSettings.timeIntegration.verboseMode = 1
110
111simulationSettings.timeIntegration.newton.useModifiedNewton = True
112simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = False
113simulationSettings.timeIntegration.generalizedAlpha.useNewmark = False
114simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well
115
116simulationSettings.solutionSettings.solutionInformation = "rigid body tests"
117SC.visualizationSettings.nodes.defaultSize = 0.05
118simulationSettings.displayComputationTime = False
119#simulationSettings.displayStatistics = True
120
121
122
123if useGraphics:
124 exu.StartRenderer()
125 mbs.WaitForUserToContinue()
126
127mbs.SolveDynamic(simulationSettings)#, experimentalNewSolver=True)
128
129
130#+++++++++++++++++++++++++++++++++++++++++++++
131#compute TestModel error for EulerParameters and index2 solver
132pos = mbs.GetObjectOutputBody(oRB,exu.OutputVariableType.Position, localPosition=[0,0,0])
133exu.Print('pos=',pos)
134u = 0
135for i in range(3): #take sum of all coordinates
136 u += abs(pos[i])
137
138exu.Print('solution of GenericJointTest=',u)
139
140exudynTestGlobals.testError = u - (1.1878327690760586) #2020-04-22: 1.1878327690760586
141exudynTestGlobals.testResult = u
142
143
144if useGraphics:
145 #SC.WaitForRenderEngineStopFlag()
146 exu.StopRenderer() #safely close rendering window!