rigidBodyTutorial2.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  3D rigid body tutorial with 2 bodies and revolute joints
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2021-03-22
  8# Modified: 2023-04-18
  9#
 10# 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.
 11#
 12#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 13
 14import exudyn as exu
 15from exudyn.utilities import * #includes itemInterface, graphicsDataUtilities and rigidBodyUtilities
 16import numpy as np
 17
 18SC = exu.SystemContainer()
 19mbs = SC.AddSystem()
 20
 21
 22#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 23#physical parameters
 24g =     [0,-9.81,0] #gravity
 25bodyDim=[1,0.1,0.1] #body dimensions
 26p0 =    [0,0,0]     #origin of pendulum
 27pMid0 = np.array([bodyDim[0]*0.5,0,0]) #center of mass, body0
 28
 29#first link:
 30#inertia for cubic body with dimensions in sideLengths
 31iCube0 = InertiaCuboid(density=5000, sideLengths=[1,0.1,0.1])
 32#print(iCube)
 33
 34#graphics for body
 35graphicsBody0 = GraphicsDataRigidLink(p0=[-0.5*bodyDim[0],0,0],p1=[0.5*bodyDim[0],0,0],
 36                                     axis0=[0,0,1], axis1=[0,0,0*1], radius=[0.05,0.05],
 37                                     thickness = 0.1, width = [0.12,0.12], color=color4red)
 38
 39[n0,b0]=AddRigidBody(mainSys = mbs,
 40                     inertia = iCube0,
 41                     nodeType = str(exu.NodeType.RotationEulerParameters),
 42                     position = pMid0,
 43                     rotationMatrix = np.diag([1,1,1]),
 44                     angularVelocity = [0,0,0],
 45                     gravity = g, #will automatically add a load on body
 46                     graphicsDataList = [graphicsBody0])
 47
 48#markers are needed to link joints and bodies; also needed for loads
 49#ground body and marker
 50oGround = mbs.AddObject(ObjectGround())
 51markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
 52
 53#markers for rigid body:
 54markerBody0J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[-0.5*bodyDim[0],0,0]))
 55
 56#revolute joint (free z-axis)
 57#could alternatively also be done with function AddRevoluteJoint
 58mbs.AddObject(GenericJoint(markerNumbers=[markerGround, markerBody0J0],
 59                           constrainedAxes=[1,1,1,1,1,0],
 60                           visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.1)))
 61
 62#%%++++++++++++++++++++++++++
 63#second link:
 64pMid1 = np.array([bodyDim[0],0,0]) + np.array([0,0,0.5*bodyDim[0]]) #center of mass, body1
 65
 66graphicsBody1 = GraphicsDataRigidLink(p0=[0,0,-0.5*bodyDim[0]],p1=[0,0,0.5*bodyDim[0]],
 67                                     axis0=[1,0,0], axis1=[0,0,0], radius=[0.06,0.05],
 68                                     thickness = 0.1, width = [0.12,0.12], color=color4steelblue)
 69
 70iCube1 = InertiaCuboid(density=5000, sideLengths=[0.1,0.1,1])
 71
 72[n1,b1]=AddRigidBody(mainSys = mbs,
 73                     inertia = iCube1,
 74                     nodeType = str(exu.NodeType.RotationEulerParameters),
 75                     position = pMid1,
 76                     rotationMatrix = np.diag([1,1,1]),
 77                     angularVelocity = [0,0,0],
 78                     gravity = g, #will automatically add a load on body
 79                     graphicsDataList = [graphicsBody1])
 80
 81#add sensor to body in order to measure and store global position over time
 82sens1=mbs.AddSensor(SensorBody(bodyNumber=b1, localPosition=[0,0,0.5*bodyDim[0]],
 83                               fileName='solution/sensorPos.txt',
 84                               outputVariableType = exu.OutputVariableType.Position))
 85
 86#markers for rigid body:
 87markerBody0J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[ 0.5*bodyDim[0],0,0]))
 88markerBody1J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[0,0,-0.5*bodyDim[0]]))
 89
 90#revolute joint (free z-axis)
 91mbs.AddObject(GenericJoint(markerNumbers=[markerBody0J1, markerBody1J0],
 92                            constrainedAxes=[1,1,1,0,1,1],
 93                            visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.1)))
 94
 95#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
 96#assemble system and solve
 97mbs.Assemble()
 98
 99simulationSettings = exu.SimulationSettings() #takes currently set values or default values
100
101tEnd = 4 #simulation time
102stepSize = 1e-3 #step size
103simulationSettings.timeIntegration.numberOfSteps = int(tEnd/stepSize)
104simulationSettings.timeIntegration.endTime = tEnd
105simulationSettings.timeIntegration.verboseMode = 1
106simulationSettings.timeIntegration.simulateInRealtime = True
107
108SC.visualizationSettings.window.renderWindowSize=[1600,1200]
109SC.visualizationSettings.openGL.multiSampling = 4
110SC.visualizationSettings.general.autoFitScene = False
111
112exu.StartRenderer()
113if 'renderState' in exu.sys: #reload previous model view
114    SC.SetRenderState(exu.sys['renderState'])
115
116mbs.WaitForUserToContinue() #stop before simulating
117
118mbs.SolveDynamic(simulationSettings = simulationSettings,
119                 solverType=exu.DynamicSolverType.TrapezoidalIndex2)
120
121SC.WaitForRenderEngineStopFlag() #stop before closing
122exu.StopRenderer() #safely close rendering window!
123
124#plot some sensor output
125
126mbs.PlotSensor([sens1],[1])