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