sphericalJointTest.py

You can view and download this file on Github: sphericalJointTest.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Simulate Chain with 3D rigid bodies and SphericalJoint;
  5#           Also test MarkerNodePosition
  6#
  7# Author:   Johannes Gerstmayr
  8# Date:     2020-04-09
  9#
 10# 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.
 11#
 12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 13
 14import exudyn as exu
 15from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 16import exudyn.graphics as graphics #only import if it does not conflict
 17
 18useGraphics = True #without test
 19#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 20#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 21try: #only if called from test suite
 22    from modelUnitTests import exudynTestGlobals #for globally storing test results
 23    useGraphics = exudynTestGlobals.useGraphics
 24except:
 25    class ExudynTestGlobals:
 26        pass
 27    exudynTestGlobals = ExudynTestGlobals()
 28#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 29
 30SC = exu.SystemContainer()
 31mbs = SC.AddSystem()
 32
 33nBodies = 4
 34color = [0.1,0.1,0.8,1]
 35s = 0.1 #width of cube
 36sx = 3*s #lengt of cube/body
 37cPosZ = 0.1 #offset of constraint in z-direction
 38zz = sx * (nBodies+1)*2 #max size of background
 39
 40background0 = GraphicsDataRectangle(-zz,-zz,zz,sx,color)
 41oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
 42                                   visualization=VObjectGround(graphicsData= [background0])))
 43mPosLast = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oGround,
 44                                            localPosition=[-sx,0,cPosZ*0]))
 45
 46#create a chain of bodies:
 47for i in range(nBodies):
 48    f = 0 #factor for initial velocities
 49    omega0 = [0,50.*f,20*f] #arbitrary initial angular velocity
 50    ep0 = eulerParameters0 #no rotation
 51    ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
 52
 53    p0 = [-sx+i*2*sx,0.,0] #reference position
 54    v0 = [0.2*f,0.,0.] #initial translational velocity
 55
 56    nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0,
 57                                      initialVelocities=v0+list(ep_t0)))
 58    #nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=[0,0,0,1,0,0,0], initialVelocities=[0,0,0,0,0,0,0]))
 59    oGraphics = GraphicsDataOrthoCubeLines(-sx,-s,-s, sx,s,s, [0.8,0.1,0.1,1])
 60    oRB = mbs.AddObject(ObjectRigidBody(physicsMass=2,
 61                                        physicsInertia=[6,1,6,0,0,0],
 62                                        nodeNumber=nRB,
 63                                        visualization=VObjectRigidBody(graphicsData=[oGraphics])))
 64
 65    mMassRB = mbs.AddMarker(MarkerBodyMass(bodyNumber = oRB))
 66    mbs.AddLoad(Gravity(markerNumber = mMassRB, loadVector=[0.,-9.81,0.])) #gravity in negative z-direction
 67
 68    if i==0:
 69        #mPos = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition = [-sx*0,0.,cPosZ*0]))
 70        mPos = mbs.AddMarker(MarkerNodePosition(nodeNumber=nRB))
 71    else:
 72        mPos = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition = [-sx,0.,cPosZ]))
 73
 74    #alternative with spring-damper:
 75    #mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers = [mPosLast, mPos],
 76    #                                                   stiffness=[k,k,k], damping=[d,d,d])) #gravity in negative z-direction
 77    axes = [1,1,1]
 78    if (i==0):
 79        axes = [0,1,1]
 80
 81    mbs.AddObject(SphericalJoint(markerNumbers = [mPosLast, mPos], constrainedAxes=axes))
 82
 83    #marker for next chain body
 84    mPosLast = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition = [sx,0.,cPosZ]))
 85
 86
 87mbs.Assemble()
 88#exu.Print(mbs)
 89
 90simulationSettings = exu.SimulationSettings() #takes currently set values or default values
 91
 92fact = 1000
 93simulationSettings.timeIntegration.numberOfSteps = 1*fact
 94simulationSettings.timeIntegration.endTime = 0.001*fact
 95simulationSettings.solutionSettings.solutionWritePeriod = simulationSettings.timeIntegration.endTime/fact*10
 96simulationSettings.timeIntegration.verboseMode = 1
 97
 98simulationSettings.timeIntegration.newton.useModifiedNewton = True
 99simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = False
100simulationSettings.timeIntegration.generalizedAlpha.useNewmark = False
101simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well
102
103simulationSettings.solutionSettings.solutionInformation = "rigid body tests"
104SC.visualizationSettings.nodes.defaultSize = 0.05
105#simulationSettings.displayComputationTime = True
106#simulationSettings.displayStatistics = True
107
108if useGraphics:
109    exu.StartRenderer()
110    mbs.WaitForUserToContinue()
111
112mbs.SolveDynamic(simulationSettings)
113
114#+++++++++++++++++++++++++++++++++++++++++++++
115sol = mbs.systemData.GetODE2Coordinates();
116solref = mbs.systemData.GetODE2Coordinates(configuration=exu.ConfigurationType.Reference);
117#exu.Print('sol=',sol)
118u = 0
119for i in range(14): #take coordinates of first two bodies
120    u += abs(sol[i]+solref[i])
121
122exu.Print('solution of sphericalJointTest=',u)
123
124exudynTestGlobals.testError = u - (4.409080446574593) #up to 2021-06-28: 4.409080446580333; 2020-04-04: 4.409004179180698
125exudynTestGlobals.testResult = u
126
127
128if useGraphics:
129    #SC.WaitForRenderEngineStopFlag()
130    exu.StopRenderer() #safely close rendering window!