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