rigidBodyAsUserFunctionTest.py
You can view and download this file on Github: rigidBodyAsUserFunctionTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: 3D rigid body implemented by user function and compared to C++ implementation;
5# Test model for 3D rigid body with Euler parameters modeled with GenericODE2 and CoordinateVectorConstraint;
6# One of the challenges of the example is the inclusion of the Euler parameter constraint
7#
8# Author: Johannes Gerstmayr
9# Date: 2021-06-28
10#
11# 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.
12#
13#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
15import exudyn as exu
16from exudyn.utilities import *
17
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
35
36
37zz = 1 #max size
38s = 0.1 #size of cube
39sx = 3*s #x-size
40
41background0 = GraphicsDataRectangle(-zz,-zz,zz,zz,color4white)
42oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
43 visualization=VObjectGround(graphicsData= [background0])))
44mPosLast = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oGround,
45 localPosition=[-2*sx,0,0]))
46
47omega0 = [0,50.,20] #arbitrary initial angular velocity
48ep0 = eulerParameters0 #no rotation
49
50ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
51
52p0 = [0.,0.,0] #reference position
53p1 = [s*5,0.,0] #reference position
54v0 = [0.2,0.,0.] #initial translational velocity
55
56nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p1+ep0,
57 initialVelocities=v0+list(ep_t0)))
58
59mass = 2
60inertia6D = [6,1,6,0,1,0]
61g = 9.81
62
63oGraphics = GraphicsDataOrthoCubePoint(centerPoint=[0,0,0], size=[sx,s,s], color=color4red)
64oRB = mbs.AddObject(ObjectRigidBody(physicsMass=mass,
65 physicsInertia=inertia6D,
66 nodeNumber=nRB,
67 visualization=VObjectRigidBody(graphicsData=[oGraphics])))
68
69mMassRB = mbs.AddMarker(MarkerBodyMass(bodyNumber = oRB))
70mbs.AddLoad(Gravity(markerNumber = mMassRB, loadVector=[0.,-g,0.])) #gravity in negative z-direction
71
72
73if True: #rigid body as user function
74 #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
75 #node for mass point:
76 useDummyObject = False #set true for an alternative way: use dummy rigid body to realize constraint
77 qRef2 = np.array(p0+ep0)
78 nRB2 = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=np.array(p0+ep0), #reference coordinates for node2
79 initialVelocities=v0+list(ep_t0),
80 addConstraintEquation=useDummyObject)) #do not add algebraic variable here!
81
82 #dummy object, replacement for constraint by using a rigid body with zero mass:
83 if useDummyObject:
84 oRB2 = mbs.AddObject(ObjectRigidBody(physicsMass=mass*0,
85 physicsInertia=np.array(inertia6D)*0,
86 nodeNumber=nRB2,
87 visualization=VObjectRigidBody(graphicsData=[oGraphics])))
88
89 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
90 #equations of motion for rigid body, with COM=[0,0,0]
91 M=np.diag([mass,mass,mass]) #translatoric part of mass matrix
92 J = Inertia6D2InertiaTensor(inertia6D) #local inertia tensor
93 MRB = np.zeros((7,7))
94 exu.Print("M =",M)
95 exu.Print("J =",J)
96 fG = np.array([0,-g*mass,0]+[0]*4)
97
98 def UFgenericODE2(mbs, t, itemIndex, q, q_t):
99 f = np.copy(fG)
100 #slower, but without global variable: qRef2 = mbs.GetNodeParameter(mbs.GetObjectParameter(itemIndex,'nodeNumbers')[0], 'referenceCoordinates')
101 q2 = np.array(q) + qRef2 #q only contains 'change', reference coordinates must be added
102
103 qEP = q2[3:7] #Euler parameters for node
104 qEP_t = q_t[3:7] #time derivative of Euler parameters for node
105 G = EulerParameters2GLocal(qEP)
106 omega = G @ qEP_t
107
108 f[3:7] += -G.T @ Skew(omega) @ J @ omega
109 return f
110 #exu.Print("t =", t, ", f =", f)
111
112 def UFmassGenericODE2(mbs, t, itemIndex, q, q_t):
113 #slower, but without global variable: qRef2 = mbs.GetNodeParameter(mbs.GetObjectParameter(itemIndex,'nodeNumbers')[0], 'referenceCoordinates')
114 q2 = np.array(q) + qRef2 #q only contains 'change', reference coordinates must be added
115 qEP = q2[3:7] #Euler parameters for node
116 G = EulerParameters2GLocal(qEP)
117
118 MRB[0:3,0:3] = M #translational part
119 MRB[3:7,3:7] = G.T @ J @ G #rotational part
120 return MRB
121
122 #add visualization for rigid body: note that transformation from local to global coordinates needs to be done as well
123 def UFgraphics(mbs, itemNumber):
124 n = mbs.GetObjectParameter(itemNumber, 'nodeNumbers')[0]
125 p0 = mbs.GetNodeOutput(nodeNumber=n, variableType=exu.OutputVariableType.Position, configuration=exu.ConfigurationType.Visualization)
126 A = mbs.GetNodeOutput(nodeNumber=n, variableType=exu.OutputVariableType.RotationMatrix, configuration=exu.ConfigurationType.Visualization)
127
128 A0 = np.reshape(A, (3,3))
129 graphics1 = MoveGraphicsData(oGraphics, p0, A0)
130 return [graphics1]
131
132 mbs.AddObject(ObjectGenericODE2(nodeNumbers = [nRB2],
133 forceUserFunction=UFgenericODE2, massMatrixUserFunction=UFmassGenericODE2,
134 visualization=VObjectGenericODE2(graphicsDataUserFunction=UFgraphics)))
135
136 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
137 #add Euler parameter constraint
138 if not useDummyObject:
139 nG = mbs.AddNode(NodePointGround(visualization=VNodePointGround(show=False)))
140 mNodeGround = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nG))
141 mRB2 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nRB2))
142
143 #q0^2+q1^2+q2^2+q3^2 - 1 = 0
144 mbs.AddObject(CoordinateVectorConstraint(markerNumbers=[mNodeGround, mRB2],
145 scalingMarker0=[], scalingMarker1=[],
146 quadraticTermMarker0=[], quadraticTermMarker1=np.array([[0,0,0,1,1,1,1]]),
147 offset=[1],
148 visualization=VCoordinateVectorConstraint(show=False)))
149#end: user function for rigid body
150#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
151
152
153mbs.Assemble()
154exu.Print(mbs)
155
156simulationSettings = exu.SimulationSettings()
157
158#useGraphics=False
159tEnd = 0.05
160h = 1e-3
161if useGraphics:
162 tEnd = 1
163
164simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
165simulationSettings.timeIntegration.endTime = tEnd
166#simulationSettings.solutionSettings.solutionWritePeriod = h
167simulationSettings.timeIntegration.verboseMode = 1
168#simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
169
170simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8 #SHOULD work with 0.9 as well
171
172SC.visualizationSettings.nodes.showBasis=True
173
174if useGraphics:
175 exu.StartRenderer()
176
177mbs.SolveDynamic(simulationSettings)
178
179
180u0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Displacement)
181rot0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Rotation)
182exu.Print('u0=',p0,', rot0=', rot0)
183
184u1 = mbs.GetNodeOutput(nRB2, exu.OutputVariableType.Displacement)
185rot1 = mbs.GetNodeOutput(nRB2, exu.OutputVariableType.Rotation)
186exu.Print('u1=',p1,', rot1=', rot1)
187
188result = (abs(u1+u0)+abs(rot1+rot0)).sum()
189exu.Print('solution of rigidBodyAsUserFunctionTest=',result)
190
191exudynTestGlobals.testError = result - (8.950865271552146) #2020-06-28: 8.950865271552146
192exudynTestGlobals.testResult = result
193
194if useGraphics:
195 SC.WaitForRenderEngineStopFlag()
196 exu.StopRenderer() #safely close rendering window!