rigidBodyTutorial3withMarkers.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  3D rigid body tutorial with 2 bodies and revolute joints, using Marker-style approach
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2021-08-05
  8# Date:     2024-06-04 (updated to MainSystem Python extensions)
  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, 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
 29L = 1               #length
 30w = 0.1             #width
 31bodyDim=[L,w,w] #body dimensions
 32p0 =    [0,0,0]     #origin of pendulum
 33pMid0 = np.array([L*0.5,0,0]) #center of mass, body0
 34
 35#ground body
 36oGround = mbs.CreateGround()
 37
 38#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 39#first link:
 40#create inertia paramters (mass, center of mass (COM) and inertia tensor at reference point)
 41iCube0 = InertiaCuboid(density=5000, sideLengths=bodyDim)
 42iCube0 = iCube0.Translated([-0.25*L,0,0]) #transform COM, COM not at reference point!
 43
 44#graphics for body
 45graphicsBody0 = graphics.RigidLink(p0=[-0.5*L,0,0],p1=[0.5*L,0,0],
 46                                     axis0=[0,0,1], axis1=[0,0,0], radius=[0.5*w,0.5*w],
 47                                     thickness = w, width = [1.2*w,1.2*w], color=graphics.color.red)
 48graphicsCOM0 = graphics.Basis(origin=iCube0.com, length=2*w)
 49
 50#create rigid body; we could use other formulation, e.g., by selecting nodeType = exu.NodeType.RotationRotationVector
 51b0=mbs.CreateRigidBody(inertia = iCube0, #includes COM
 52                       referencePosition = pMid0,
 53                       gravity = g,
 54                       graphicsDataList = [graphicsCOM0, graphicsBody0])
 55
 56
 57#%%++++++++++++++++++++++++++
 58#revolute joint (free z-axis)
 59
 60#markers for ground and rigid body (not needed for option 3):
 61markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
 62markerBody0J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[-0.5*L,0,0]))
 63
 64# revolute joint option 1:
 65mbs.AddObject(GenericJoint(markerNumbers=[markerGround, markerBody0J0],
 66                            constrainedAxes=[1,1,1,1,1,0],
 67                            visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
 68
 69#revolute joint option 2:
 70# mbs.AddObject(ObjectJointRevoluteZ(markerNumbers = [markerGround, markerBody0J0],
 71#                                   rotationMarker0=np.eye(3),
 72#                                   rotationMarker1=np.eye(3),
 73#                                   visualization=VObjectJointRevoluteZ(axisRadius=0.2*w, axisLength=1.4*w)
 74#                                   ))
 75
 76#%%++++++++++++++++++++++++++
 77#second link:
 78graphicsBody1 = graphics.RigidLink(p0=[0,0,-0.5*L],p1=[0,0,0.5*L],
 79                                     axis0=[1,0,0], axis1=[0,0,0], radius=[0.06,0.05],
 80                                     thickness = 0.1, width = [0.12,0.12], color=graphics.color.lightgreen)
 81
 82iCube1 = InertiaCuboid(density=5000, sideLengths=[0.1,0.1,1])
 83
 84pMid1 = np.array([L,0,0]) + np.array([0,0,0.5*L]) #center of mass, body1
 85b1=mbs.CreateRigidBody(inertia = iCube1,
 86                            referencePosition = pMid1,
 87                            gravity = g,
 88                            graphicsDataList = [graphicsBody1])
 89
 90#revolute joint (free x-axis)
 91# #alternative with GenericJoint:
 92# #markers for rigid body:
 93markerBody0J1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[ 0.5*L,0,0]))
 94markerBody1J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1, localPosition=[0,0,-0.5*L]))
 95mbs.AddObject(GenericJoint(markerNumbers=[markerBody0J1, markerBody1J0],
 96                            constrainedAxes=[1,1,1,0,1,1],
 97                            visualization=VObjectJointGeneric(axesRadius=0.2*w, axesLength=1.4*w)))
 98
 99#position sensor at tip of body1
100sens1=mbs.AddSensor(SensorBody(bodyNumber=b1, localPosition=[0,0,0.5*L],
101                               fileName='solution/sensorPos.txt',
102                               outputVariableType = exu.OutputVariableType.Position))
103
104#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
105#assemble system before solving
106mbs.Assemble()
107if False:
108    mbs.systemData.Info() #show detailed information
109if False:
110    mbs.DrawSystemGraph(useItemTypes=True) #draw nice graph of system
111
112simulationSettings = exu.SimulationSettings() #takes currently set values or default values
113
114tEnd = 4 #simulation time
115h = 1e-3 #step size
116simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
117simulationSettings.timeIntegration.endTime = tEnd
118simulationSettings.timeIntegration.verboseMode = 1
119#simulationSettings.timeIntegration.simulateInRealtime = True
120simulationSettings.solutionSettings.solutionWritePeriod = 0.005 #store every 5 ms
121
122SC.visualizationSettings.window.renderWindowSize=[1600,1200]
123SC.visualizationSettings.openGL.multiSampling = 4
124SC.visualizationSettings.general.autoFitScene = False
125
126SC.visualizationSettings.nodes.drawNodesAsPoint=False
127SC.visualizationSettings.nodes.showBasis=True
128
129# uncomment to start visualization during simulation
130# exu.StartRenderer()
131# if 'renderState' in exu.sys: #reload old view
132#     SC.SetRenderState(exu.sys['renderState'])
133
134#mbs.WaitForUserToContinue() #stop before simulating
135
136mbs.SolveDynamic(simulationSettings = simulationSettings,
137                 solverType=exu.DynamicSolverType.TrapezoidalIndex2)
138
139# SC.WaitForRenderEngineStopFlag() #stop before closing
140# exu.StopRenderer() #safely close rendering window!
141
142#start post processing
143mbs.SolutionViewer()
144
145if False:
146    #plot sensor sens1, y-component [1]
147    mbs.PlotSensor(sensorNumbers=[sens1],components=[1],closeAll=True)