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