ConvexContactTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test with ObjectContactConvexRoll, which models a roll of a mechanum wheel or any other roll
  5#           which is described by a polynomial profile
  6#
  7# Author:   Peter Manzl
  8# Date:     2021-12-21
  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 numpy as np
 17import exudyn.graphics as graphics
 18
 19testIsActive = exu.sys.get('testIsActive', False)
 20
 21SC = exu.SystemContainer()
 22mbs = SC.AddSystem()
 23
 24
 25# create Ground and graphics: 
 26scb, g1, g2 = 0.5, 0.92, 0.72
 27graphGround = graphics.CheckerBoard(point= [0,0,0], normal= [0,0,1], size= scb, color= [g1, g1, g1, 1.0],
 28                                        alternatingColor= [g2, g2, g2, 1.0], nTiles= 12)
 29oGround = mbs.CreateGround(referencePosition = [0,0,0], 
 30                           graphicsDataList=[graphGround])
 31mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround,localPosition = [0,0,0], name='Ground'))
 32nGround0=mbs.AddNode(NodePointGround(referenceCoordinates = [0,0,0]))
 33
 34
 35poly = [-3.6, 0,  1.65e-02] # coefficients of the polynomial creating the rolling body
 36length = 0.1                # length of the roller
 37contour =  [[-length/2, 0]]  
 38x = np.linspace(start = - length/2, stop =length/2, num=51) 
 39for i in range(np.size(x)):
 40    contour+= [[x[i], np.polyval(poly, x[i])]]
 41contour += [ [length/2, 0]] # to create a closed contour
 42graphRoll = [graphics.SolidOfRevolution([0,0,0], [1,0,0], contour, graphics.color.lightred[0:3]+[1],
 43                                          alternatingColor=graphics.color.blue, nTiles = 32)]
 44
 45InertiaRoll = InertiaCylinder(density=7800, length=length, outerRadius=3e-3, axis=0) 
 46bRoll = mbs.CreateRigidBody(inertia = InertiaRoll, 
 47                            referencePosition = [0,0,poly[-1]*1.2],  
 48                            referenceRotationMatrix =RotationMatrixY(np.pi/16),
 49                            initialAngularVelocity = RotationMatrixY(np.pi/16) @ np.array([-1000,0,0]),  # in Global coordinates
 50                            initialVelocity= [0,0,0],
 51                            gravity = [0,0,-9.81], 
 52                            graphicsDataList = graphRoll) 
 53
 54nRoll = mbs.GetObject(bRoll)['nodeNumber']
 55        
 56nData = mbs.AddNode(NodeGenericData(initialCoordinates=[0,0,0], numberOfDataCoordinates=3))
 57mRoll = mbs.AddMarker(MarkerNodeRigid(nodeNumber=nRoll))
 58CConvexRoll = mbs.AddObject(ObjectContactConvexRoll(markerNumbers=[mGround, mRoll], 
 59                        nodeNumber=nData, contactStiffness=1e3, contactDamping=1, dynamicFriction = 0.9,
 60                       staticFrictionOffset = 0, viscousFriction=0, exponentialDecayStatic=1e-3, 
 61                       frictionProportionalZone=1e-4, rollLength=length, coefficientsHull=poly, 
 62                       visualization={'show': True, 'color': graphics.color.lightgreen}))
 63
 64sBody = mbs.AddSensor(SensorBody(bodyNumber=bRoll, #fileName='PosRoller.txt',
 65                                 storeInternal=True,
 66                                 outputVariableType=exu.OutputVariableType.Position, visualization={'show': False})) 
 67
 68mbs.Assemble()
 69
 70h = 5e-4   #test
 71tEnd = 0.1 #test
 72#tEnd = 0.1*20 #for simulation
 73sims=exu.SimulationSettings()
 74sims.timeIntegration.newton.useModifiedNewton = False #Just for the test; modified Newton is usually faster
 75sims.timeIntegration.generalizedAlpha.spectralRadius=0.7
 76sims.timeIntegration.endTime = tEnd
 77sims.timeIntegration.numberOfSteps = int(tEnd/h) #original: 1e-3, fails now in Newton
 78sims.timeIntegration.verboseMode = 0
 79sims.timeIntegration.stepInformation = 3 #do not show step reduction
 80sims.solution.file.name = 'solution/coordinatesSolution.txt'
 81# sims.timeIntegration.newton.absoluteTolerance = 1e-8
 82# sims.timeIntegration.newton.relativeTolerance = 1e-6
 83
 84if not testIsActive: 
 85    sims.timeIntegration.verboseMode = 1
 86    sims.timeIntegration.stepInformation = 3+128+256
 87    SC.renderer.Start()
 88    SC.renderer.DoIdleTasks()
 89mbs.SolveDynamic(sims)
 90if not testIsActive: 
 91    SC.renderer.DoIdleTasks()
 92    SC.renderer.Stop() #safely close rendering window!
 93
 94sol = mbs.systemData.GetODE2Coordinates()
 95exu.sys['testResult'] = np.sum(sol[:2])
 96exu.Print('result of ConvexContactTest=',exu.sys['testResult'])
 97# %% 
 98if not testIsActive: 
 99    #pos = np.loadtxt('PosRoller.txt', delimiter=',', comments='#')
100    pos = mbs.GetSensorStoredData(sBody)
101    exu.Print('End Pos: {}'.format(pos[-1,:]))
102    
103    
104    mbs.PlotSensor(sBody,[0,1,2])
105    
106    
107if not testIsActive and False:
108    SC.visualizationSettings.general.autoFitScene = False
109    SC.visualizationSettings.general.graphicsUpdateInterval=0.02
110    
111    sol = LoadSolutionFile('solution/coordinatesSolution.txt', safeMode=True)#, maxRows=100)
112    exu.Print('start SolutionViewer')
113    mbs.SolutionViewer(sol)