rigidBodySpringDamperIntrinsic.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test rigid body formulation for different center of mass (COM)
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2023-11-30
  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 *
 15
 16import numpy as np
 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
 33#intrinsic is independent of order of markers; force/torque computed at mid-frame of joint
 34intrinsicFormulation=True
 35
 36k=10000
 37d= k*0.05 * 0
 38kr = 500*0.5
 39dr = kr*0.05 * 0
 40
 41L = 1
 42#++++++++++++++++++++++++++++++++++++++++++++++
 43#create two ground and two bodies, rigid body spring damper using different marker order
 44gGround = GraphicsDataOrthoCubePoint(size = [L,0.1,0.25], color=color4blue)
 45oGround = mbs.CreateGround(referencePosition = [0,0,0], graphicsDataList=[gGround])
 46
 47gBody1 = GraphicsDataOrthoCubePoint(size = [L,0.1,0.2], color=color4red)
 48oBody1 = mbs.CreateRigidBody(referencePosition = [L,0.,0.],
 49                       referenceRotationMatrix = np.eye(3),
 50                       initialVelocity = [0.,1.,2.],
 51                       initialAngularVelocity = 5.*np.array([1.7,2.1,3.2]),
 52                       inertia=InertiaCuboid(density=1000,sideLengths=[L,0.1,0.2]),
 53                       gravity = [0.,0.,0.],
 54                       graphicsDataList = [gBody1],)
 55
 56gBody2 = GraphicsDataOrthoCubePoint(size = [L,0.1,0.2], color=color4green)
 57oBody2 = mbs.CreateRigidBody(referencePosition = [L,0.,0.],
 58                       referenceRotationMatrix = np.eye(3),
 59                       initialVelocity = [0.,1.,2.],
 60                       initialAngularVelocity = 5.*np.array([1.7,2.1,3.2]),
 61                       inertia=InertiaCuboid(density=1000,sideLengths=[L,0.1,0.2]),
 62                       gravity = [0.,0.,0.],
 63                       graphicsDataList = [gBody2],)
 64
 65oRBSD1 = mbs.CreateRigidBodySpringDamper(bodyList = [oGround, oBody1],
 66                                        localPosition0 = [0.5*L,0.,0.], #global position
 67                                        localPosition1 = [-0.5*L,0.,0.], #global position
 68                                        stiffness = np.diag([k*0.4,k*0.5,k*0.7, kr,kr*2,kr*1.3]),
 69                                        damping = np.diag([d,d,d, dr,dr,dr]),
 70                                        offset = [0.,0.,0.,0.,0.,0.],
 71                                         intrinsicFormulation=intrinsicFormulation,
 72                                        )
 73
 74oRBSD2 = mbs.CreateRigidBodySpringDamper(
 75                                         bodyList = [oBody2, oGround],
 76                                         localPosition0 = [-0.5*L,0.,0.], #global position
 77                                         localPosition1 = [0.5*L,0.,0.], #global position
 78                                         stiffness = np.diag([k*0.4,k*0.5,k*0.7, kr,kr*2,kr*1.3]),
 79                                         damping = np.diag([d,d,d, dr,dr,dr]),
 80                                         offset = [0.,0.,0.,0.,0.,0.],
 81                                         intrinsicFormulation=intrinsicFormulation,
 82                                        )
 83
 84#++++++++++++++++++++++++++++++++++++++++++++++
 85#create two connected bodies, freely rotating:
 86oBody3a = mbs.CreateRigidBody(referencePosition = [0,1.,0.],
 87                       referenceRotationMatrix = np.eye(3),
 88                       initialVelocity = 0.*np.array([0.,1.,2.]),
 89                       initialAngularVelocity = 5*np.array([1.7,2.1,3.2]),
 90                       inertia=InertiaCuboid(density=1000,sideLengths=[L,0.1,0.2]),
 91                       gravity = [0.,0.,0.],
 92                       graphicsDataList = [gBody1],)
 93
 94oBody3b = mbs.CreateRigidBody(referencePosition = [L,1.,0.],
 95                       referenceRotationMatrix = np.eye(3),
 96                       initialVelocity = 0.*np.array([0.,-1.,-2.]),
 97                       initialAngularVelocity = -5*np.array([1.7,2.1,3.2]),
 98                       inertia=InertiaCuboid(density=1000,sideLengths=[L,0.1,0.2]),
 99                       gravity = [0.,0.,0.],
100                       graphicsDataList = [gBody2],)
101
102oRBSD3 = mbs.CreateRigidBodySpringDamper(
103                                         bodyList = [oBody3a, oBody3b],
104                                         localPosition0 = [0.5*L,0.,0.], #global position
105                                         localPosition1 = [-0.5*L,0.,0.], #global position
106                                         stiffness = 1*np.diag([k*0.4,k*0.5,k*0.7, kr,kr*2,kr*1.3]),
107                                         damping = np.diag([d,d,d, dr,dr,dr]),
108                                         offset = [0.,0.,0.,0.,0.,0.],
109                                         intrinsicFormulation=intrinsicFormulation,
110                                        )
111
112# mbs.SetObjectParameter(oRBSD1, 'intrinsicFormulation', True)
113# mbs.SetObjectParameter(oRBSD2, 'intrinsicFormulation', True)
114# mbs.SetObjectParameter(oRBSD3, 'intrinsicFormulation', True)
115
116sBody1 = mbs.AddSensor(SensorBody(bodyNumber=oBody1, storeInternal=True, outputVariableType=exu.OutputVariableType.Position))
117sBody2 = mbs.AddSensor(SensorBody(bodyNumber=oBody2, storeInternal=True, outputVariableType=exu.OutputVariableType.Position))
118sBody3 = mbs.AddSensor(SensorBody(bodyNumber=oBody3a, storeInternal=True, outputVariableType=exu.OutputVariableType.AngularVelocity))
119
120
121#+++++++++++++++++++++++++++++++++++++++++++++++++++++++
122mbs.Assemble()
123
124endTime = 1 #for stepSize=0.002: non-intrinsic formulation gets unstable around 7.5 seconds for body3 and for body1/2 around 73 seconds
125if useGraphics:
126    endTime = 100
127
128stepSize = 0.002
129simulationSettings = exu.SimulationSettings()
130#simulationSettings.displayComputationTime = True
131simulationSettings.timeIntegration.verboseMode = useGraphics
132simulationSettings.solutionSettings.writeSolutionToFile = useGraphics
133
134simulationSettings.timeIntegration.numberOfSteps = int(endTime/stepSize)
135simulationSettings.timeIntegration.endTime = endTime
136simulationSettings.timeIntegration.newton.useModifiedNewton = True
137
138
139if useGraphics:
140    exu.StartRenderer()
141    mbs.WaitForUserToContinue()
142
143mbs.SolveDynamic(simulationSettings)
144
145if useGraphics:
146    exu.StopRenderer() #safely close rendering window!
147
148
149    mbs.PlotSensor([sBody1,sBody2], components=[1,1])
150    mbs.PlotSensor([sBody1,sBody2], components=[2,2])
151    mbs.PlotSensor([sBody3], components=[0,1,2])
152
153    mbs.SolutionViewer()
154
155
156
157
158p1=mbs.GetObjectOutputBody(oBody1, exu.OutputVariableType.Displacement)
159exu.Print("p1=", p1)
160p2=mbs.GetObjectOutputBody(oBody2, exu.OutputVariableType.Displacement)
161exu.Print("p2=", p2)
162omega3=mbs.GetObjectOutputBody(oBody3a, exu.OutputVariableType.AngularVelocity)
163exu.Print("omega3=", omega3)
164
165
166#+++++++++++++++++++++++++++++++++++++++++++++
167u=NormL2(p1) + NormL2(p2) + NormL2(0.01*omega3)
168exu.Print('solution of rigidBodySpringDamperIntrinsic test=',u)
169
170exudynTestGlobals.testError = u - (0.5472368463500464)
171exudynTestGlobals.testResult = u
172
173
174if useGraphics:
175    SC.WaitForRenderEngineStopFlag()
176    exu.StopRenderer() #safely close rendering window!