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