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!