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