createFunctionsTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test model for MainSystem Create functions; the model itself is not very meaningful, but it uses many Create functions
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2025-05-11
  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
 16
 17import numpy as np
 18
 19useGraphics = True #without test
 20#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 21#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 22try: #only if called from test suite
 23    from modelUnitTests import exudynTestGlobals #for globally storing test results
 24    useGraphics = exudynTestGlobals.useGraphics
 25except:
 26    class ExudynTestGlobals:
 27        pass
 28    exudynTestGlobals = ExudynTestGlobals()
 29#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 30
 31
 32#set up new multibody system to work with
 33SC = exu.SystemContainer()
 34mbs = SC.AddSystem()
 35
 36graphicsGround = graphics.CheckerBoard(point=[0,2,-1], size=8)
 37oGround = mbs.CreateGround(referencePosition=[0,0,0], graphicsDataList=[graphicsGround])
 38
 39oMass = mbs.CreateMassPoint(physicsMass=5, referencePosition=[1,0,0],
 40                            initialDisplacement=[0,0,0],
 41                            initialVelocity=[0,0.5,0],
 42                            gravity=[0,-9.81,0],
 43                            drawSize=0.3, color=graphics.color.orange,
 44                            )
 45
 46oSpringDamper = mbs.CreateSpringDamper(bodyNumbers=[oGround, oMass], #[body0,body1]
 47                                      localPosition0=[0,0,0], #locally on body0
 48                                      localPosition1=[0,0,0], #locally on body1
 49                                      referenceLength=None, #usually set to None (default) => takes the (relaxed) length in reference configuration
 50                                      stiffness=1e2, damping=1)
 51
 52oDistance = mbs.CreateDistanceConstraint(bodyNumbers=[oGround, oMass], #[body0,body1]
 53                                         localPosition0 = [0,0,0], #locally on body0
 54                                         localPosition1 = [0,0,0], #locally on body1
 55                                         distance=None)
 56
 57#%%++++++++++++++++++++++++++++++++++++++++++++++++++
 58
 59mass = 10
 60outerRadius = 0.45
 61length = 2
 62volume = np.pi * outerRadius**2 * length
 63inertiaCylinder = InertiaCylinder(density=mass/volume, length=length, outerRadius=outerRadius,
 64                                  axis=0, innerRadius=0.7*outerRadius)
 65
 66oBody = mbs.CreateRigidBody(inertia=inertiaCylinder,
 67                            referencePosition=[3,0,0],
 68                            initialVelocity=[0,0,0],
 69                            initialAngularVelocity=[2*np.pi,0,0],
 70                            gravity=[0,-9.81,0],
 71                            #graphicsDataList=[graphicsCyl]
 72                            )
 73
 74
 75rbX = 2
 76rbY = 0.5
 77rbZ = 0.2
 78
 79#create inertia instance for cuboid (brick) shape
 80mass=10 #kg
 81volume=rbX*rbY*rbZ
 82inertiaCube2 = InertiaCuboid(density=mass/volume, sideLengths=[rbX,rbY,rbZ])
 83inertiaCube2 = inertiaCube2.Translated([0.5,0,0])
 84
 85oBody2 = mbs.CreateRigidBody(inertia = inertiaCube2,
 86                             referencePosition = [4,-0.5*rbX,0],
 87                             referenceRotationMatrix = RotationVector2RotationMatrix([0,0,-0.5*np.pi]),
 88                             initialAngularVelocity = [0,0,0.2*2*np.pi],
 89                             initialVelocity = [0.2*np.pi*0.5*rbX,0,0],
 90                             gravity = [0,-9.81,0],
 91                             color=graphics.color.blue,
 92                             #graphicsDataList=[graphicsCube, graphics.Basis(origin=inertiaCube2.COM())],
 93                             )
 94
 95loadMassPoint = mbs.CreateForce(bodyNumber=oMass, loadVector=[10,0,0])
 96loadRigidBody = mbs.CreateForce(bodyNumber=oBody, localPosition=[0,0,0], loadVector=[10,0,0])
 97
 98def UFforce(mbs, t, loadVector):
 99    return (10+5*np.sin(t*10*2*np.pi))*np.array([0,5,0])
100
101mbs.CreateForce(bodyNumber=oBody,
102                localPosition=[0,1.2,0.5], #position at body
103                loadVectorUserFunction=UFforce)
104
105def UserFunctionTorque(mbs, t, loadVector):
106    return np.cos(t*2*np.pi)*np.array(loadVector)
107
108#add torque with user function
109mbs.CreateTorque(bodyNumber=oBody, loadVector=[5,0,0],
110                 loadVectorUserFunction=UserFunctionTorque)
111
112#+++++++++
113inertiaSphere = InertiaSphere(mass=2, radius=0.25)
114gSphere = inertiaSphere.GetGraphics(graphics.color.red)
115
116oMass1 = mbs.CreateMassPoint(physicsMass=2, referencePosition=[1,0,0],
117                             gravity=[0,-9.81,0],
118                             # drawSize=0.25, color=graphics.color.red,
119                             graphicsDataList=[gSphere]
120                             )
121
122#create spherical joint between ground and mass point; could also be applied to two bodies; possible bodies: mass point or rigid body
123mbs.CreateSphericalJoint(bodyNumbers=[oGround, oMass1],
124                         position=[1,0,0], #global position of joint (in reference configuration)
125                         constrainedAxes=[1,0,0]) #x,y,z directions: 1 = fixed, 0 = free motion
126
127#constrain Y and Z coordinate of mass point to move z=10*y
128mbs.CreateCoordinateConstraint(bodyNumbers=[oMass1, oMass1],
129                               coordinates=[2,1],
130                               factorValue1=10)
131
132mbs.CreatePrismaticJoint(bodyNumbers=[oGround, oBody],
133                         position=[3,0,0], #global position of joint
134                         axis=[1,0,0], #global axis of joint, can move in global x-direction
135                         useGlobalFrame=True) #use local coordinates for joint definition
136
137mbs.CreateRevoluteJoint(bodyNumbers=[oBody, oBody2],
138                        position=[4,-0.5,0], #global position of joint
139                        axis=[0,0,1], #rotation along global z-axis
140                        useGlobalFrame=True)
141
142mbs.CreateTorsionalSpringDamper(bodyNumbers=[oBody, oBody2],
143                                position=[4,-0.5,0], #global position of spring-damper
144                                axis=[0,0,1],        #global rotation axis
145                                stiffness=1000,
146                                damping=20,
147                                useGlobalFrame=True)
148
149mass = 5
150mu = 0.05 #static friction
151rDisc = 0.5
152length = 0.1
153volume = np.pi * rDisc**2 * length
154inertiaCylinder = InertiaCylinder(density=mass/volume, length=length, outerRadius=rDisc, axis=0)
155
156oDisc = mbs.CreateRigidBody(inertia=inertiaCylinder,
157                            nodeType=exu.NodeType.RotationRxyz, #this allows to constrain Z-rotation
158                            referencePosition=[0,2,rDisc-1], #reference position x/y/z of COM
159                            initialVelocity=[0,2*np.pi*rDisc,0],
160                            initialAngularVelocity=[-2*np.pi,0.2,0],
161                            gravity=[0,0,-9.81])
162
163mbs.CreateRollingDiscPenalty(bodyNumbers=[oGround, oDisc], discRadius = rDisc,
164                             axisPosition=[0,0,0], axisVector=[1,0,0],
165                             planePosition = [0,0,-1], planeNormal = [0,0,1],
166                             contactStiffness = 1e4, contactDamping = 2e2,
167                             dryFriction = [mu,mu], color=graphics.color.red)
168
169#constrain Z and Y-rotation of disc, so it rotates only around X
170mbs.CreateCoordinateConstraint(bodyNumbers=[oDisc, None], coordinates=[5,None])
171mbs.CreateCoordinateConstraint(bodyNumbers=[oDisc, oGround], coordinates=[4,None])#also works with ground
172
173mass = 5
174rDisc = 0.5
175length = 0.1
176volume = np.pi * rDisc**2 * length
177inertiaCylinder = InertiaCylinder(density=mass/volume, length=length, outerRadius=rDisc, axis=0)
178
179#create a free rigid body using the defined inertia properties and applies gravity (y-direction).
180oDisc1 = mbs.CreateRigidBody(inertia=inertiaCylinder,
181                            referencePosition=[1,2,rDisc-1], #reference position x/y/z of COM
182                            initialVelocity=[0,2*np.pi*rDisc,0],
183                            initialAngularVelocity=[-2*np.pi,0.2,0],
184                            gravity=[0,0,-9.81])
185
186mbs.CreateForce(bodyNumber=oDisc1, loadVector=[10,0,0])
187
188#create a 'rolling' joint between flat ground defined by plane, lying on oGround, and rigid body given as oDisc:
189mbs.CreateRollingDisc(bodyNumbers=[oGround, oDisc1], discRadius = rDisc,
190                      axisPosition=[0,0,0], axisVector=[1,0,0], #relative to oDisc
191                      planePosition = [0,0,-1], planeNormal = [0,0,1], #defines plane
192                      constrainedAxes=[1,1,1] #constrain 3 axes: lateral motion, forward motion and normal contact
193                      )
194
195#constrain the two discs to have same X-motion
196mbs.CreateCoordinateConstraint(bodyNumbers=[oDisc,oDisc1],
197                               velocityLevel=True,
198                               coordinates=[0,0])
199
200mbs.CreateCartesianSpringDamper(
201    bodyNumbers=[oGround, oBody2], #[body0,body1]
202    localPosition0=[4,-0.5*rbX,0], #for body0
203    localPosition1=[0,0,0],        #for body1
204    stiffness = [100,10,10], #x,y,z stiffness
205    damping = [5,2,2], #x,y,z damping
206)
207
208#%%++++++++++++++++++++++++++++++++++++++++++++++
209mbs.Assemble()
210
211endTime = 0.5 #simulation time in seconds
212if useGraphics:
213    endTime = 2.5
214
215stepSize = 0.002 #should be small enough to achieve sufficient accuracy
216
217#some simulation parameters:
218simulationSettings = exu.SimulationSettings() #takes currently set values or default values
219simulationSettings.timeIntegration.numberOfSteps = int(endTime/stepSize)
220simulationSettings.timeIntegration.endTime = endTime
221simulationSettings.timeIntegration.verboseMode = 1
222simulationSettings.timeIntegration.newton.useModifiedNewton = True
223
224#for redundant constraints, use these two settings
225simulationSettings.linearSolverSettings.ignoreSingularJacobian=True
226simulationSettings.linearSolverType = exu.LinearSolverType.EigenDense #use EigenSparse for larger systems alternatively
227#simulationSettings.linearSolverType = exu.LinearSolverType.EigenSparse
228# simulationSettings.timeIntegration.newton.useModifiedNewton = True
229SC.visualizationSettings.openGL.multiSampling = 4
230
231mbs.SolveDynamic(simulationSettings)
232
233
234#%%
235#TAG: visualization
236#DIFFICULTY: 10000
237SC.visualizationSettings.nodes.drawNodesAsPoint = False
238SC.visualizationSettings.nodes.showBasis = True
239
240if useGraphics:
241    mbs.SolutionViewer()
242
243#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
244ode2 = mbs.systemData.GetODE2Coordinates()
245
246u = 0.01*np.linalg.norm(ode2)
247exu.Print('solution of createFunctionsTest=',u)
248
249exudynTestGlobals.testResult = u
250#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++