revoluteJointPrismaticJointTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  A chain of 3D rigid bodies connected with revolute and prismatic joints
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2021-07-01
  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
 16
 17from math import sin, cos, pi
 18import numpy as np
 19
 20testIsActive = exu.sys.get('testIsActive', False)
 21
 22SC = exu.SystemContainer()
 23mbs = SC.AddSystem()
 24
 25
 26#background
 27color = [0.1,0.1,0.8,1]
 28L = 0.4 #length of bodies
 29d = 0.1 #diameter of bodies
 30
 31#create background, in order to have according zoom all
 32zz=2*L
 33#background0 = GraphicsDataRectangle(-zz,-zz,zz,zz,graphics.color.white)
 34oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0])) #,visualization=VObjectGround(graphicsData= [background0])))
 35mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround, localPosition=[0,0,0]))
 36A0 = RotationMatrixX(0) 
 37Alast = A0 #previous marker
 38
 39A0 = RotationMatrixX(0)
 40A1 = RotationMatrixY(0.5*pi)
 41A2 = RotationMatrixZ(0.5*pi)
 42A3 = RotationMatrixX(-0.5*pi)
 43A4 = RotationMatrixZ(0.5*pi)
 44
 45Alist=[A0,A1,A2,A3,A4]
 46
 47p0 = [0.,0.,0] #reference position
 48vLoc = np.array([L,0,0]) #last to next joint
 49#g = [0,0,-9.81]
 50g = [0,-9.81,0]
 51
 52def RotatedMarker(marker, rotation):
 53    """a marker on the same body, its frame turned by rotation: the frame of a joint"""
 54    data = mbs.GetMarker(marker)
 55    return mbs.AddMarker(MarkerBodyRigid(bodyNumber=data['bodyNumber'],
 56                                         localHT=exu.HT(data['localHT']) * exu.HT(rotation=rotation)))
 57
 58#create a chain of bodies:
 59for i in range(5):
 60    #print("Build Object", i)
 61    inertia = InertiaCuboid(density=1000, sideLengths=[L,d,d])
 62    #p0 += Alist[i] @ (0.5*vLoc)
 63    p0 += (0.5*vLoc)
 64
 65    ep0 = eulerParameters0 #no rotation
 66    graphicsBody = graphics.Brick([0,0,0], [L,d,d], graphics.color.steelblue)
 67    oRB = mbs.CreateRigidBody(inertia=inertia, 
 68                              referencePosition=p0,
 69                              referenceRotationMatrix=A0,
 70                              gravity=g,
 71                              graphicsDataList=[graphicsBody])
 72    nRB= mbs.GetObject(oRB)['nodeNumber']
 73
 74    mPos0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [-0.5*L,0,0]))
 75    mPos1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [ 0.5*L,0,0]))
 76    useGenericJoint = False #for comparison
 77    if True:
 78        if i != 2:
 79            if not useGenericJoint:
 80                mbs.AddObject(ObjectJointRevoluteZ(markerNumbers = [RotatedMarker(mPosLast, Alist[i]), RotatedMarker(mPos0, Alist[i])], #the joint frames
 81                                                  visualization=VObjectJointRevoluteZ(axisRadius=0.5*d, axisLength=1.2*d)
 82                                                  )) 
 83            else: #compare to GenericJoint
 84                mbs.AddObject(ObjectJointGeneric(markerNumbers = [RotatedMarker(mPosLast, Alist[i]), RotatedMarker(mPos0, Alist[i])], #the joint frames
 85                                                 constrainedAxes=[1,1,1,1,1,0],
 86                                                 visualization=VGenericJoint(axesRadius=0.5*d, axesLength=1.2*d)
 87                                                 )) 
 88        else:
 89            if not useGenericJoint:
 90                mbs.AddObject(ObjectJointPrismaticX(markerNumbers = [RotatedMarker(mPosLast, Alist[i]), RotatedMarker(mPos0, Alist[i])], #the joint frames
 91                                                  visualization=VObjectJointPrismaticX(axisRadius=0.5*d, axisLength=1.2*d)
 92                                                  )) 
 93            else: #compare to GenericJoint
 94                mbs.AddObject(ObjectJointGeneric(markerNumbers = [RotatedMarker(mPosLast, Alist[i]), RotatedMarker(mPos0, Alist[i])], #the joint frames
 95                                                 constrainedAxes=[0,1,1,1,1,1],
 96                                                 visualization=VGenericJoint(axesRadius=0.5*d, axesLength=1.2*d)
 97                                                 )) 
 98            #add spring, to limit motion in prismatic joint:
 99            k=500
100            damp=k*0.02
101            mbs.AddObject(CartesianSpringDamper(markerNumbers = [mPosLast, mPos0], 
102                                       stiffness = [k]*3, damping = [damp]*3,
103                                       visualization=VCartesianSpringDamper(show=False, drawSize=0.5*d)))
104        
105    mPosLast = mPos1
106    
107    # p0 += Alist[i] @ (0.5*vLoc)
108    p0 += (0.5*vLoc)
109    Alast = Alist[i]
110
111mbs.AddLoad(LoadForceVector(markerNumber=mPosLast, loadVector=[0,0,20]))
112
113mbs.Assemble()
114
115simulationSettings = exu.SimulationSettings() #takes currently set values or default values
116
117tEnd = 0.25
118h=0.001  #use small step size to detext contact switching
119
120simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
121simulationSettings.timeIntegration.endTime = tEnd
122simulationSettings.solution.file.writePeriod = 0.001
123simulationSettings.solution.sensors.writePeriod = 0.01
124#simulationSettings.timeIntegration.realtime.active = True
125#simulationSettings.timeIntegration.realtime.factor = 0.1
126simulationSettings.timeIntegration.verboseMode = 1
127
128simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8
129simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
130simulationSettings.timeIntegration.newton.useModifiedNewton = True
131#simulationSettings.timeIntegration.newton.modifiedNewtonJacUpdatePerStep = True 
132
133SC.visualizationSettings.nodes.show = True
134SC.visualizationSettings.nodes.drawNodesAsPoint  = False
135SC.visualizationSettings.nodes.showBasis = True
136SC.visualizationSettings.nodes.basisSize = 0.015
137SC.visualizationSettings.connectors.showJointAxes = True
138 
139SC.visualizationSettings.general.autoFitScene = False #use loaded render state
140#testIsActive = True
141if not testIsActive:
142    simulationSettings.show.computationTime = True
143    simulationSettings.show.statistics = True
144    SC.renderer.Start()
145    SC.renderer.RestoreSavedState()
146    #SC.renderer.DoIdleTasks()
147else:
148    simulationSettings.solution.file.write = False
149
150#mbs.SolveDynamic(simulationSettings, solverType=exu.DynamicSolverType.TrapezoidalIndex2)
151mbs.SolveDynamic(simulationSettings, showHints=True)
152
153if False: #use this to reload the solution and use SolutionViewer
154    sol = LoadSolutionFile('solution/coordinatesSolution.txt')
155    
156    mbs.SolutionViewer(sol)
157
158
159u0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Displacement)
160rot0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Rotation)
161exu.Print('u0=',u0,', rot0=', rot0)
162
163result = (abs(u0)+abs(rot0)).sum()
164exu.Print('solution of revoluteJointprismaticJointTest=',result)
165
166exu.sys['testResult'] = result
167
168
169
170#%%+++++++++++++++++++++++++++++
171if not testIsActive:
172    SC.renderer.DoIdleTasks()
173    SC.renderer.Stop() #safely close rendering window!