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!