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