kinematicTreeTest.py
You can view and download this file on Github: kinematicTreeTest.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN example
3#
4# Details: Test model for KinematicTree, using simple 3D chain;
5# results have been compared to redundant links in Examples/kinematicTreeAndMBS.py
6#
7# Author: Johannes Gerstmayr
8# Date: 2022-05-05
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 *
16
17import numpy as np
18
19useGraphics = True #without test
20#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
22try: #only if called from test suite
23 from modelUnitTests import exudynTestGlobals #for globally storing test results
24 useGraphics = exudynTestGlobals.useGraphics
25except:
26 class ExudynTestGlobals:
27 pass
28 exudynTestGlobals = ExudynTestGlobals()
29#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
31SC = exu.SystemContainer()
32mbs = SC.AddSystem()
33
34
35L = 2 #length of links
36w = 0.1 #width of links
37J = InertiaCuboid(density=1000, sideLengths=[L,w,w]) #w.r.t. reference center of mass
38J = J.Translated([0.5*L,0,0])
39com = J.com
40
41gravity3D = [0,-10,0]
42
43n=5 #5#number of coordinates
44
45linkMasses = []
46linkCOMs = exu.Vector3DList()
47linkInertiasCOM=exu.Matrix3DList()
48
49jointTransformations=exu.Matrix3DList()
50jointOffsets = exu.Vector3DList()
51for i in range(n):
52 #create some rotated axis and offsets...
53 A=np.eye(3)
54 if i%2 != 0:
55 A=RotXYZ2RotationMatrix([0*0.5*pi,0.25*pi,0])
56 if i%3 >= 1:
57 A=RotXYZ2RotationMatrix([0.5*pi,0.25*pi,0])
58
59 v = np.array([L,0,0])
60 if i==0:
61 v = np.array([0,0,0])
62
63 #now add joint/link to lists:
64 jointTransformations.Append(A)
65 jointOffsets.Append(v)
66
67 linkMasses += [J.Mass()]
68 linkCOMs.Append(J.COM())
69 linkInertiasCOM.Append(J.InertiaCOM())
70
71
72# linkForces = exu.Vector3DList([[0.,0.,0.]]*n)
73# linkTorques = exu.Vector3DList([[0.,0.,0.]]*n)
74
75#create per-link graphics:
76gLink = GraphicsDataOrthoCubePoint(centerPoint= [0.5*L,0,0], size= [L,w,w], color= color4dodgerblue)
77gJoint = GraphicsDataCylinder([0,0,-1.25*w], [0,0,2.5*w], 0.4*w, color=color4grey)
78gList = [[gJoint,gLink]]*n #one list per link; add joint first, then it will be visible with transparency setting
79
80#create node for unknowns of KinematicTree
81nGeneric = mbs.AddNode(NodeGenericODE2(referenceCoordinates=[0.]*n,
82 initialCoordinates=[0.]*n,
83 initialCoordinates_t=[0.]*n,
84 numberOfODE2Coordinates=n))
85
86#create KinematicTree
87mbs.AddObject(ObjectKinematicTree(nodeNumber=nGeneric, jointTypes=[exu.JointType.RevoluteZ]*n, linkParents=np.arange(n)-1,
88 jointTransformations=jointTransformations, jointOffsets=jointOffsets,
89 linkInertiasCOM=linkInertiasCOM, linkCOMs=linkCOMs, linkMasses=linkMasses,
90 baseOffset = [0.,0.,0.], gravity=gravity3D,
91 #jointForceVector=[0.]*n,
92 visualization=VObjectKinematicTree(graphicsDataList = gList)))
93
94
95mbs.Assemble()
96
97tEnd = 1 #end time of simulation
98h = 0.005 #step size; leads to 1000 steps
99
100simulationSettings = exu.SimulationSettings()
101simulationSettings.solutionSettings.writeSolutionToFile=False
102simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h) #must be integer
103simulationSettings.timeIntegration.endTime = tEnd
104simulationSettings.timeIntegration.verboseMode = 1
105
106SC.visualizationSettings.bodies.kinematicTree.frameSize = 1
107SC.visualizationSettings.bodies.kinematicTree.showJointFrames = True
108SC.visualizationSettings.general.drawWorldBasis = True
109SC.visualizationSettings.general.worldBasisSize = 2
110SC.visualizationSettings.openGL.multiSampling = 4
111
112if useGraphics:
113 exu.StartRenderer() #start graphics visualization
114 mbs.WaitForUserToContinue() #wait for pressing SPACE bar to continue
115
116mbs.SolveDynamic(simulationSettings, solverType = exu.DynamicSolverType.RK44)
117
118if useGraphics:
119 SC.WaitForRenderEngineStopFlag()#wait for pressing 'Q' to quit
120 exu.StopRenderer() #safely close rendering window!
121
122#evaluate final (=current) output values
123q = mbs.GetNodeOutput(nGeneric, exu.OutputVariableType.Coordinates)
124exu.Print('coordinates=',q)
125
126u=sum(q)
127exu.Print('solution of genericODE2test=',u)
128#solution converged to 14 digits (h=5e-5): -1.3093839514061
129
130exudynTestGlobals.testError = u - (-1.309383960216414 ) #2022-05-05: -1.309383960216414 (accurate to 8 digits)
131exudynTestGlobals.testResult = u