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
 16import numpy as np
 17
 18testIsActive = exu.sys.get('testIsActive', False)
 19
 20
 21SC = exu.SystemContainer()
 22mbs = SC.AddSystem()
 23
 24    
 25#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 26g =     [0,-9.81,0] #gravity
 27L = 1               #length
 28w = 0.1             #width
 29bodyDim=[L,w,w] #body dimensions
 30p0 =    [0,0,0]     #origin of pendulum
 31pMid0 = np.array([L*0.5,0,0]) #center of mass, body0
 32pMid1 = np.array([L*0.5,-L,0]) #center of mass, body0
 33
 34#ground body, located at specific position (there could be several ground objects)
 35oGround = mbs.CreateGround(referencePosition=[0,0,0])
 36
 37#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 38#first link:
 39iCube0 = InertiaCuboid(density=5000, sideLengths=bodyDim)
 40
 41#graphics for body
 42graphicsBody0 = graphics.Brick(centerPoint=[0,0,0],size=[L,w,w],color=graphics.color.red)
 43graphicsBody1 = graphics.Brick(centerPoint=[0,0,0],size=[L*1.01,w*0.99,w*1.01],color=graphics.color.dodgerblue)
 44graphicsBody2 = graphics.Brick(centerPoint=[0,0,0],size=[L,w,w],color=graphics.color.lawngreen)
 45graphicsBody3 = graphics.Brick(centerPoint=[0,0,0],size=[L*1.01,w*0.99,w*1.01],color=graphics.color.orange)
 46graphicsCOM0 = graphics.Basis(origin=iCube0.com, length=2*w) #COM frame
 47
 48#body rotated, rotation axis = z-axis on ground
 49if True:
 50    #reference case with CreateRevoluteJoint
 51    #create rigid node and body
 52    b0=mbs.CreateRigidBody(inertia = iCube0, #includes COM
 53                           referencePosition = pMid0,
 54                           referenceRotationMatrix=RotationVector2RotationMatrix([0.2,0.3,0.4]),
 55                           gravity = g,
 56                           graphicsDataList = [graphicsCOM0, graphicsBody0])
 57    r0=mbs.CreateRevoluteJoint(itemNumbers=[oGround, b0], 
 58                               position=[0.25,-0.1,0.1], #global position on ground
 59                               axis=[0,0,1], 
 60                               #axisRadius=0.2*w, axisLength=1.4*w
 61                               )
 62
 63    #comparison with new method:
 64    b1=mbs.CreateRigidBody(inertia = iCube0, #includes COM
 65                           referencePosition = pMid0,
 66                           referenceRotationMatrix=RotationVector2RotationMatrix([0.2,0.3,0.4]),
 67                           gravity = g,
 68                           graphicsDataList = [graphicsCOM0, graphicsBody1])
 69    n1 = mbs.GetObject(b1)['nodeNumber']
 70    mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, 
 71                                            localPosition=[0.25,-0.1,0.1], #global position
 72                                            ) )
 73
 74    #just build joint from one marker:
 75    r1=mbs.AddObject(RevoluteJointZ(**GetJointArgs(mbs, markerNumber0=mGround, bodyNumber1=b1) ))
 76    # exu.Print('joint r0=',mbs.GetObject(r0) )
 77    # exu.Print('joint r1=',GetJointArgs(mbs, markerNumber0=mGround, bodyNumber1=b1) )
 78
 79
 80#body rotated, rotation axis = z-axis of rotated body
 81if True:
 82    #reference case with CreateRevoluteJoint
 83    #create rigid node and body
 84    b0B=mbs.CreateRigidBody(inertia = iCube0, #includes COM
 85                           referencePosition = pMid1,
 86                           referenceRotationMatrix=RotationVector2RotationMatrix([0.2,0.3,0.4]),
 87                           gravity = g,
 88                           graphicsDataList = [graphicsCOM0, graphicsBody2])
 89    r0B=mbs.CreateRevoluteJoint(itemNumbers=[b0B, oGround], 
 90                               position=[0.1,-0.05,0.05], #global position on ground
 91                               axis=[0,1,0],
 92                               # axis=[0,0,1],
 93                               useGlobalFrame=False,
 94                               axisRadius=0.06, axisLength=0.4,
 95                               )
 96
 97    #comparison with new method:
 98    b1B=mbs.CreateRigidBody(inertia = iCube0, #includes COM
 99                           referencePosition = pMid1,
100                           referenceRotationMatrix=RotationVector2RotationMatrix([0.2,0.3,0.4]),
101                           gravity = g,
102                           graphicsDataList = [graphicsCOM0, graphicsBody3])
103    n1B = mbs.GetObject(b1)['nodeNumber']
104    #the marker with the rotation of the joint r0B, which its marker 0 carries
105    rotationJoint0 = np.array(mbs.GetMarker(mbs.GetObject(r0B)['markerNumbers'][0])['localHT'])[0:3,0:3]
106    mb1B = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b1B, 
107                                            localHT=exu.HT(rotation=rotationJoint0, translation=[0.1,-0.05,0.05]), #global position
108                                            ) )
109
110    #just build joint from one marker:
111    jargs = GetJointArgs(mbs, markerNumber0=mb1B, 
112                         bodyNumber1=oGround)
113    r1B=mbs.AddObject(RevoluteJointZ(**jargs ))
114    # exu.Print('joint r0=',mbs.GetObject(r0B) )
115    # exu.Print('joint r1=',jargs )
116
117#%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
118#assemble system before solving
119mbs.Assemble()
120
121simulationSettings = exu.SimulationSettings() #takes currently set values or default values
122
123tEnd = 0.5 #simulation time
124h = 2e-3 #step size
125simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
126simulationSettings.timeIntegration.endTime = tEnd
127simulationSettings.timeIntegration.verboseMode = 1
128simulationSettings.solution.file.writePeriod = 0.01 #store every 10 ms
129# simulationSettings.timeIntegration.realtime.active = True
130simulationSettings.timeIntegration.newton.useModifiedNewton = True
131
132SC.visualizationSettings.view0.window.renderWindowSize=[1600,1200]
133SC.visualizationSettings.openGL.multiSampling = 4
134
135SC.visualizationSettings.nodes.showBasis=True
136
137if not testIsActive:
138    SC.renderer.Start()
139    SC.renderer.DoIdleTasks()
140
141mbs.SolveDynamic(simulationSettings)
142
143if not testIsActive:
144    SC.renderer.DoIdleTasks()
145    SC.renderer.Stop()
146
147# if not testIsActive:
148#     mbs.SolutionViewer()
149
150#+++++++++++++++++++++++++++++++++++++++++++++
151uTotal = 0.1*sum(mbs.GetNodeOutput(n1, exu.OutputVariableType.CoordinatesTotal) )
152uTotal+= 0.1*sum(mbs.GetNodeOutput(n1B, exu.OutputVariableType.CoordinatesTotal) )
153exu.Print('uTotal=',uTotal)
154
155exu.sys['testResult'] = uTotal
156#+++++++++++++++++++++++++++++++++++++++++++++