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