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