relativeRotationTranslationMechanism.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test models for mainSystemExtensions; tests for functions except PlotSensor or SolutionViewer, 
  5#           which are tested already in other functions or cannot be tested with test suite;
  6#           all tests are self-contained and are included as examples for docu
  7#
  8# Author:   Johannes Gerstmayr
  9# Date:     2023-05-19
 10#
 11# 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.
 12#
 13#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 14
 15import exudyn as exu
 16from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 17from math import cos
 18import exudyn.graphics as graphics
 19import numpy as np
 20
 21testIsActive = exu.sys.get('testIsActive', False)
 22
 23## set up MainSystem mbs
 24SC = exu.SystemContainer()
 25mbs = SC.AddSystem()
 26
 27## overall parameters
 28L = 0.4 #length of bodies
 29d = 0.1 #diameter of bodies
 30p0 = [0.,0.,0] #reference position
 31vLoc = np.array([L,0,0]) #last to next joint
 32#g = [0,0,-9.81]
 33g = [0,-9.81,0]
 34
 35## create ground and ground marker
 36oGround=mbs.AddObject(ObjectGround(referencePosition= [-0.5*L,0,0])) 
 37mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround, localPosition=[0,0,0]))
 38A0 = np.eye(3)
 39Alast = A0 #previous marker
 40bodyLast = oGround
 41
 42## set up rotation matrices for relative rotation of joints
 43A0 = RotationMatrixX(0)
 44A1 = RotationMatrixY(0.5*pi)
 45A2 = RotationMatrixZ(0.5*pi*0)
 46A3 = RotationMatrixX(-0.5*pi*0)
 47Alist=[A0,A1,A2,A3,A1]
 48
 49## set up list of vectors defining global axes
 50v0=[0,0,1]
 51v1=[1,1,1]
 52v2=[0,1,0]
 53v3=[0,0,1]
 54v4=[1,1,0]
 55
 56# v1=[0,0,1]
 57# v2=[0,1,0]
 58
 59axisList=[v0,v1,v2,v3,v4]
 60
 61markerList=[]
 62sensorList=[]
 63refValuesList=[]
 64rotJoints = [0,1,1,0,1]
 65
 66## for loop to create a chain of 4 bodies under gravity connected with revolute joints
 67for i in range(4):
 68    ### create inertia for block with dimensions [L,d,d] and graphics for block
 69    inertia = InertiaCuboid(density=1000, sideLengths=[L,d,d])
 70    graphicsBody = graphics.Brick([0,0,0], [0.96*L,d,d], graphics.color.steelblue)
 71
 72    #axis = [0,0,1]
 73    axis = np.array(axisList[i]) #global axis!
 74    axis = axis/np.linalg.norm(axis)
 75    #print('axis=',axis)
 76
 77    ### create and add rigid body to mbs
 78    p0 += Alist[i] @ (0.5*vLoc)
 79    angVel = -2 * pi * (Alast @ np.array(axis))
 80    oRB = mbs.CreateRigidBody(inertia=inertia, 
 81                              referencePosition=p0,
 82                              # initialAngularVelocity=angVel,
 83                              referenceRotationMatrix=Alist[i],
 84                              gravity=g,
 85                              graphicsDataList=[graphicsBody])
 86    nRB= mbs.GetObject(oRB)['nodeNumber']
 87
 88    body0 = bodyLast
 89    body1 = oRB
 90    ### retrieve reference position for simpler definition of global joint position
 91    point = mbs.GetObjectOutputBody(oRB,exu.OutputVariableType.Position,
 92                                    localPosition=[-0.5*L,0,0],
 93                                    configuration=exu.ConfigurationType.Reference)
 94
 95    if rotJoints[i]:
 96        ### set up revolute joint between two bodies, at global position and with global axis
 97        oJoint = mbs.CreateRevoluteJoint(itemNumbers=[body0, body1], 
 98                                position=point, axis=axis, useGlobalFrame=True, 
 99                                axisRadius=0.6*d, axisLength=1.2*d)
100    else:
101        ### set up prismatic joint between two bodies, at global position and with global axis
102        oJoint = mbs.CreatePrismaticJoint(itemNumbers=[body0, body1], position=point, axis=axis, 
103                                          useGlobalFrame=True, axisRadius=0.6*d, axisLength=1.2*d)
104
105
106    markers = mbs.GetObject(oJoint)['markerNumbers']
107    localPosition0 = mbs.GetMarker(markers[0])['localPosition']
108    localPosition1 = mbs.GetMarker(markers[1])['localPosition']
109    
110    if rotJoints[i]:
111        nGeneric = mbs.AddNode(NodeGenericData(initialCoordinates=[0], numberOfDataCoordinates=1))
112        mRelative = mbs.AddMarker(MarkerBodiesRelativeRotationCoordinate(bodyNumbers=[body0, body1], 
113                                                                         nodeNumber=nGeneric,
114                                                                         localPosition0=localPosition0, localPosition1=localPosition1,
115                                                                         axis0 = Alast.T@axis))
116    else:
117        mRelative = mbs.AddMarker(MarkerBodiesRelativeTranslationCoordinate(bodyNumbers=[body0, body1], 
118                                                                localPosition0=localPosition0, localPosition1=localPosition1,
119                                                                axis0 = Alast.T@axis))
120    markerList.append(mRelative)
121
122    bodyLast = oRB
123
124    p0 += Alist[i] @ (0.5*vLoc)
125    Alast = Alist[i]
126
127    sMarker = mbs.AddSensor(SensorMarker(markerNumber=mRelative, storeInternal=True, 
128                                   outputVariableType=exu.OutputVariableType.Coordinates))
129    sensorList.append(sMarker)
130
131#++++++++++++++++++++++++++++++++++++
132mbs.Assemble() #needed to compute marker output for current configuration
133
134#compute values in reference/initial configuration in order to avoid jumps in constraints
135for mRelative in markerList:
136    refValue = mbs.GetMarkerOutput(mRelative, exu.OutputVariableType.Coordinates)[0]
137    print('refValue=',refValue)
138    refValuesList.append(refValue)
139    mbs.SetMarkerParameter(mRelative, 'offset', refValue)
140
141
142#++++++++++++++++++++++++++++++++++++
143#constraints to connect rotations/translations
144
145def UFoffset0(mbs, t, itemNumber, lOffset):
146    return 0.3*(1-cos(2*pi*t))
147    # return 1.2*(sin(0.5*pi*t))
148
149nGround = mbs.AddNode(NodePointGround())
150mcGround = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nGround, coordinate=0))
151
152cc0 = mbs.AddObject(CoordinateConstraint(markerNumbers=[mcGround, markerList[0]], 
153                                          offset = 0.,
154                                          offsetUserFunction=UFoffset0))
155
156cc1 = mbs.AddObject(CoordinateConstraint(markerNumbers=[markerList[0], markerList[1]], 
157                                          factor1=1./(2*pi),
158                                          # velocityLevel=True
159                                          ))
160
161cc2 = mbs.AddObject(CoordinateConstraint(markerNumbers=[markerList[1], markerList[2]], 
162# cc2 = mbs.AddObject(CoordinateConstraint(markerNumbers=[mcGround, markerList[2]], 
163                                          factor1=-1,
164                                          # velocityLevel=True
165                                          offsetUserFunction=UFoffset0
166                                          ))
167cc3 = mbs.AddObject(CoordinateConstraint(markerNumbers=[markerList[2], markerList[3]], 
168                                          factor1=3*(2*pi),
169                                          # velocityLevel=True
170                                          ))
171
172
173#++++++++++++++++++++++++++++++++++++
174
175
176## assemble and set up simulation settings
177mbs.Assemble()
178
179# print('cc2=',mbs.GetObjectOutput(cc2, exu.OutputVariableType.ConstraintEquation))
180
181simulationSettings = exu.SimulationSettings() #takes currently set values or default values
182
183tEnd = 0.2
184h=0.001  #use small step size to detext contact switching
185
186simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
187simulationSettings.timeIntegration.endTime = tEnd
188simulationSettings.solution.file.writePeriod = 0.01
189simulationSettings.solution.sensors.writePeriod = 0.01
190simulationSettings.timeIntegration.verboseMode = 1 #print some progress
191
192simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8
193simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
194simulationSettings.timeIntegration.newton.useModifiedNewton = True
195
196SC.visualizationSettings.markers.defaultSize = 0.1
197SC.visualizationSettings.markers.drawSimplified = False
198SC.visualizationSettings.nodes.show = True
199SC.visualizationSettings.nodes.drawNodesAsPoint  = False
200SC.visualizationSettings.nodes.showBasis = True
201SC.visualizationSettings.nodes.basisSize = 0.015
202SC.visualizationSettings.connectors.showJointAxes = True
203
204SC.visualizationSettings.openGL.multiSampling=2
205SC.visualizationSettings.openGL.lineWidth=2
206SC.visualizationSettings.view0.window.renderWindowSize = [1600,1200]
207SC.visualizationSettings.view0.scene.drawCoordinateSystem=True
208
209SC.visualizationSettings.general.autoFitScene = False #use loaded render state
210simulationSettings.show.computationTime = True
211simulationSettings.show.statistics = True
212
213
214if not testIsActive:
215    SC.renderer.Start()
216    SC.renderer.RestoreSavedState()
217    SC.renderer.DoIdleTasks()
218
219mbs.SolveDynamic(simulationSettings = simulationSettings)
220
221if not testIsActive:
222    SC.renderer.Stop()
223
224
225if not testIsActive:
226    mbs.SolutionViewer()
227
228    if len(sensorList):
229        mbs.PlotSensor(sensorList)
230
231#%%++++
232testError = np.linalg.norm(mbs.systemData.GetODE2Coordinates())
233exu.Print('solution of relativeRotationTranslationMechanism=',testError)
234
235#%%++++++++++++++++++++++++++++++++++++++++++++++++++++
236
237exu.sys['testResult'] = testError