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