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