jointArgsTest.py

You can view and download this file on Github: jointArgsTest.py

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test for creation of joints with special GetJointArgs function
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2025-05-09
  8#
  9# 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.
 10#
 11#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 12
 13import exudyn as exu
 14from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 15import exudyn.graphics as graphics #only import if it does not conflict
 16import numpy as np
 17
 18useGraphics = True #without test
 19#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 20#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 21try: #only if called from test suite
 22    from modelUnitTests import exudynTestGlobals #for globally storing test results
 23    useGraphics = exudynTestGlobals.useGraphics
 24except:
 25    class ExudynTestGlobals:
 26        pass
 27    exudynTestGlobals = ExudynTestGlobals()
 28#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 29
 30
 31SC = exu.SystemContainer()
 32mbs = SC.AddSystem()
 33
 34
 35#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 36g =     [0,-9.81,0] #gravity
 37L = 1               #length
 38w = 0.1             #width
 39bodyDim=[L,w,w] #body dimensions
 40p0 =    [0,0,0]     #origin of pendulum
 41pMid0 = np.array([L*0.5,0,0]) #center of mass, body0
 42pMid1 = np.array([L*0.5,-L,0]) #center of mass, body0
 43
 44#ground body, located at specific position (there could be several ground objects)
 45oGround = mbs.CreateGround(referencePosition=[0,0,0])
 46
 47#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 48#first link:
 49iCube0 = InertiaCuboid(density=5000, sideLengths=bodyDim)
 50
 51#graphics for body
 52graphicsBody0 = graphics.Brick(centerPoint=[0,0,0],size=[L,w,w],color=graphics.color.red)
 53graphicsBody1 = graphics.Brick(centerPoint=[0,0,0],size=[L*1.01,w*0.99,w*1.01],color=graphics.color.dodgerblue)
 54graphicsBody2 = graphics.Brick(centerPoint=[0,0,0],size=[L,w,w],color=graphics.color.lawngreen)
 55graphicsBody3 = graphics.Brick(centerPoint=[0,0,0],size=[L*1.01,w*0.99,w*1.01],color=graphics.color.orange)
 56graphicsCOM0 = graphics.Basis(origin=iCube0.com, length=2*w) #COM frame
 57
 58#body rotated, rotation axis = z-axis on ground
 59if True:
 60    #reference case with CreateRevoluteJoint
 61    #create rigid node and body
 62    b0=mbs.CreateRigidBody(inertia = iCube0, #includes COM
 63                           referencePosition = pMid0,
 64                           referenceRotationMatrix=RotationVector2RotationMatrix([0.2,0.3,0.4]),
 65                           gravity = g,
 66                           graphicsDataList = [graphicsCOM0, graphicsBody0])
 67    r0=mbs.CreateRevoluteJoint(bodyNumbers=[oGround, b0],
 68                               position=[0.25,-0.1,0.1], #global position on ground
 69                               axis=[0,0,1],
 70                               #axisRadius=0.2*w, axisLength=1.4*w
 71                               )
 72
 73    #comparison with new method:
 74    b1=mbs.CreateRigidBody(inertia = iCube0, #includes COM
 75                           referencePosition = pMid0,
 76                           referenceRotationMatrix=RotationVector2RotationMatrix([0.2,0.3,0.4]),
 77                           gravity = g,
 78                           graphicsDataList = [graphicsCOM0, graphicsBody1])
 79    n1 = mbs.GetObject(b1)['nodeNumber']
 80    mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround,
 81                                            localPosition=[0.25,-0.1,0.1], #global position
 82                                            ) )
 83
 84    #just build joint from one marker:
 85    r1=mbs.AddObject(RevoluteJointZ(**GetJointArgs(mbs, markerNumber0=mGround, bodyNumber1=b1) ))
 86    # exu.Print('joint r0=',mbs.GetObject(r0) )
 87    # exu.Print('joint r1=',GetJointArgs(mbs, markerNumber0=mGround, bodyNumber1=b1) )
 88
 89
 90#body rotated, rotation axis = z-axis of rotated body
 91if True:
 92    #reference case with CreateRevoluteJoint
 93    #create rigid node and body
 94    b0B=mbs.CreateRigidBody(inertia = iCube0, #includes COM
 95                           referencePosition = pMid1,
 96                           referenceRotationMatrix=RotationVector2RotationMatrix([0.2,0.3,0.4]),
 97                           gravity = g,
 98                           graphicsDataList = [graphicsCOM0, graphicsBody2])
 99    r0B=mbs.CreateRevoluteJoint(bodyNumbers=[b0B, oGround],
100                               position=[0.1,-0.05,0.05], #global position on ground
101                               axis=[0,1,0],
102                               # axis=[0,0,1],
103                               useGlobalFrame=False,
104                               axisRadius=0.06, axisLength=0.4,
105                               )
106
107    #comparison with new method:
108    b1B=mbs.CreateRigidBody(inertia = iCube0, #includes COM
109                           referencePosition = pMid1,
110                           referenceRotationMatrix=RotationVector2RotationMatrix([0.2,0.3,0.4]),
111                           gravity = g,
112                           graphicsDataList = [graphicsCOM0, graphicsBody3])
113    n1B = mbs.GetObject(b1)['nodeNumber']
114    mb1B = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1B,
115                                            localPosition=[0.1,-0.05,0.05], #global position
116                                            ) )
117
118    #just build joint from one marker:
119    jargs = GetJointArgs(mbs, markerNumber0=mb1B,
120                         # rotationMarker0=RotationMatrixX(-0.5*pi),
121                         rotationMarker0=mbs.GetObject(r0B)['rotationMarker0'],
122                         bodyNumber1=oGround)
123    r1B=mbs.AddObject(RevoluteJointZ(**jargs ))
124    # exu.Print('joint r0=',mbs.GetObject(r0B) )
125    # exu.Print('joint r1=',jargs )
126
127#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
128#assemble system before solving
129mbs.Assemble()
130
131simulationSettings = exu.SimulationSettings() #takes currently set values or default values
132
133tEnd = 0.5 #simulation time
134h = 2e-3 #step size
135simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
136simulationSettings.timeIntegration.endTime = tEnd
137simulationSettings.timeIntegration.verboseMode = 1
138simulationSettings.solutionSettings.solutionWritePeriod = 0.01 #store every 10 ms
139# simulationSettings.timeIntegration.simulateInRealtime = True
140simulationSettings.timeIntegration.newton.useModifiedNewton = True
141
142SC.visualizationSettings.window.renderWindowSize=[1600,1200]
143SC.visualizationSettings.openGL.multiSampling = 4
144
145SC.visualizationSettings.nodes.showBasis=True
146
147if useGraphics:
148    SC.renderer.Start()
149    SC.renderer.DoIdleTasks()
150
151mbs.SolveDynamic(simulationSettings)
152
153if useGraphics:
154    SC.renderer.DoIdleTasks()
155    SC.renderer.Stop()
156
157# if useGraphics:
158#     mbs.SolutionViewer()
159
160#+++++++++++++++++++++++++++++++++++++++++++++
161uTotal = 0.1*sum(mbs.GetNodeOutput(n1, exu.OutputVariableType.CoordinatesTotal) )
162uTotal+= 0.1*sum(mbs.GetNodeOutput(n1B, exu.OutputVariableType.CoordinatesTotal) )
163exu.Print('uTotal=',uTotal)
164
165exudynTestGlobals.testResult = uTotal
166#+++++++++++++++++++++++++++++++++++++++++++++