addPrismaticJoint.py
You can view and download this file on Github: addPrismaticJoint.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Create a chain of bodies connected with prismatic joints; Example for CreatePrismaticJoint utility function
5#
6# Model: N-link chain of rigid bodies connected by prismatic joints
7#
8# Author: Johannes Gerstmayr
9# Date: 2021-07-02
10#
11# 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.
12#
13# *clean example*
14#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
16## import libaries
17import exudyn as exu
18from exudyn.utilities import *
19
20from math import sin, cos, pi
21import numpy as np
22
23## set up mbs
24SC = exu.SystemContainer()
25mbs = SC.AddSystem()
26
27## define overall parameters
28L = 0.4 #length of bodies
29d = 0.1 #diameter of bodies
30p0 = [0.,0.,0] #reference position
31vLoc = np.array([L,0,0]) #last to next joint
32g = [0,-9.81,0]
33
34## create ground with marker
35oGround=mbs.AddObject(ObjectGround(referencePosition= [-0.5*L,0,0]))
36mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround, localPosition=[0,0,0]))
37bodyLast = oGround
38
39## set up rotation matrices for relative rotation of joints
40A0 = np.eye(3)
41Alast = A0 #previous marker
42
43A0 = RotationMatrixX(0)
44A1 = RotationMatrixY(0.5*pi)
45A2 = RotationMatrixZ(0.5*pi)
46A3 = RotationMatrixX(-0.5*pi)
47Alist=[A0,A1,A2,A3]
48
49## set up list of vectors defining axes
50v0=[0,0,1]
51v1=[1,1,1]
52v2=[1,0,0]
53v3=[0,0,1]
54axisList=[v0,v1,v2,v3]
55
56## loop to create a chain of 4 bodies under gravity connected with prismatic joints
57for i in range(4):
58 ### create inertia for block with dimensions [L,d,d]
59 inertia = InertiaCuboid(density=1000, sideLengths=[L,d,d])
60 ### create graphics for body as block with (body-fixed) centerPoint, size and color
61 graphicsBody = GraphicsDataOrthoCubePoint(centerPoint=[0,0,0], size=[0.96*L,d,d], color=color4steelblue)
62
63 ### create and add rigid body to mbs
64 p0 += Alist[i] @ (0.5*vLoc)
65 oRB = mbs.CreateRigidBody(inertia=inertia,
66 referencePosition=p0,
67 referenceRotationMatrix=Alist[i],
68 gravity=g,
69 graphicsDataList=[graphicsBody])
70 nRB= mbs.GetObject(oRB)['nodeNumber']
71
72 body0 = bodyLast
73 body1 = oRB
74 ### retrieve reference position for simpler definition of global joint position
75 point = mbs.GetObjectOutputBody(oRB,exu.OutputVariableType.Position,
76 localPosition=[-0.5*L,0,0],
77 configuration=exu.ConfigurationType.Reference)
78 axis = axisList[i]
79 ### set up prismatic joint between two bodies, at global position and with global axis
80 mbs.CreatePrismaticJoint(bodyNumbers=[body0, body1], position=point, axis=axis,
81 useGlobalFrame=True, axisRadius=0.6*d, axisLength=1.2*d)
82
83 bodyLast = oRB
84
85 p0 += Alist[i] @ (0.5*vLoc)
86 Alast = Alist[i]
87
88## assemble and set up simulation settings
89mbs.Assemble()
90
91simulationSettings = exu.SimulationSettings() #takes currently set values or default values
92
93tEnd = 2
94h=0.001 #use small step size to detext contact switching
95
96simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
97simulationSettings.timeIntegration.endTime = tEnd
98simulationSettings.solutionSettings.solutionWritePeriod = 0.005
99#simulationSettings.timeIntegration.simulateInRealtime = True
100simulationSettings.timeIntegration.realtimeFactor = 0.5
101simulationSettings.timeIntegration.verboseMode = 1
102
103simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8
104simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
105simulationSettings.timeIntegration.newton.useModifiedNewton = True
106
107SC.visualizationSettings.nodes.show = True
108SC.visualizationSettings.nodes.drawNodesAsPoint = False
109SC.visualizationSettings.nodes.showBasis = True
110SC.visualizationSettings.nodes.basisSize = 0.015
111SC.visualizationSettings.connectors.showJointAxes = True
112
113#for snapshot:
114SC.visualizationSettings.openGL.multiSampling=4
115SC.visualizationSettings.openGL.lineWidth=2
116
117SC.visualizationSettings.general.autoFitScene = False #use loaded render state
118useGraphics = True
119if useGraphics:
120 simulationSettings.displayComputationTime = True
121 simulationSettings.displayStatistics = True
122 exu.StartRenderer()
123 ## reload previous render configuration
124 if 'renderState' in exu.sys:
125 SC.SetRenderState(exu.sys[ 'renderState' ])
126else:
127 simulationSettings.solutionSettings.writeSolutionToFile = False
128
129## start solver
130mbs.SolveDynamic(simulationSettings, showHints=True)
131
132## visualization
133mbs.SolutionViewer(runMode=2, runOnStart=True)
134
135## evaluate some results
136u0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Displacement)
137rot0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Rotation)
138exu.Print('u0=',u0,', rot0=', rot0)
139
140result = (abs(u0)+abs(rot0)).sum()
141exu.Print('solution of addPrismaticJoint=',result)