multiMbsTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  test with several mbs
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2021-03-22
  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.itemInterface import *
 15from exudyn.utilities import *
 16
 17import numpy as np
 18
 19
 20def CreateSystem(mbs, pOff, color0):
 21    #%%++++++++++++++++++++++++++++++++++++++++++++++++++++
 22    #physical parameters
 23    p0=np.array(pOff)
 24    g =     [0,-9.81,0] #gravity
 25    bodyDim=[1,0.1,0.1] #body dimensions
 26    pMid0 = np.array([bodyDim[0]*0.5,0,0]) + p0 #center of mass, body0
 27
 28    #first link:
 29    #inertia with helper function
 30    iCube0 = InertiaCuboid(density=5000, sideLengths=[1,0.1,0.1])
 31    #print(iCube)
 32
 33    #graphics for body
 34    graphicsBody0 = GraphicsDataRigidLink(p0=[-0.5*bodyDim[0],0,0],p1=[0.5*bodyDim[0],0,0],
 35                                         axis0=[0,0,1], axis1=[0,0,0*1], radius=[0.05,0.05],
 36                                         thickness = 0.1, width = [0.12,0.12], color=color0)
 37
 38    [n0,b0]=AddRigidBody(mainSys = mbs,
 39                         inertia = iCube0,
 40                         nodeType = str(exu.NodeType.RotationEulerParameters),
 41                         position = pMid0,
 42                         rotationMatrix = np.diag([1,1,1]),
 43                         angularVelocity = [0,0,0],
 44                         gravity = g,
 45                         graphicsDataList = [graphicsBody0])
 46
 47    #ground body and marker
 48    oGround = mbs.AddObject(ObjectGround())
 49    markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=p0))
 50
 51    #markers for rigid body:
 52    markerBody0J0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber=b0, localPosition=[-0.5*bodyDim[0],0,0]))
 53
 54    #revolute joint (free z-axis)
 55    mbs.AddObject(GenericJoint(markerNumbers=[markerGround, markerBody0J0],
 56                               constrainedAxes=[1,1,1,1,1,0],
 57                               visualization=VObjectJointGeneric(axesRadius=0.01, axesLength=0.1)))
 58    mbs.Assemble()
 59
 60def Simulate(SC, mbs):
 61    #%%++++++++++++++++++++++++++++++++++++++++++++++++++++++
 62    #assemble system and solve
 63
 64    simulationSettings = exu.SimulationSettings() #takes currently set values or default values
 65
 66    tEnd = 2 #simulation time
 67    h = 1e-3 #step size
 68    simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
 69    simulationSettings.timeIntegration.endTime = tEnd
 70    simulationSettings.timeIntegration.verboseMode = 1
 71    simulationSettings.timeIntegration.simulateInRealtime = True
 72
 73    SC.visualizationSettings.general.autoFitScene = False
 74    SC.visualizationSettings.window.renderWindowSize=[1600,1200]
 75    SC.visualizationSettings.openGL.multiSampling = 4
 76
 77    # exu.StartRenderer()
 78    # if 'renderState' in exu.sys: #reload old view
 79    #     SC.SetRenderState(exu.sys['renderState'])
 80
 81    mbs.WaitForUserToContinue() #stop before simulating
 82
 83    mbs.SolveDynamic(simulationSettings = simulationSettings,
 84                     solverType=exu.DynamicSolverType.TrapezoidalIndex2)
 85
 86    # SC.WaitForRenderEngineStopFlag() #stop before closing
 87    # exu.StopRenderer() #safely close rendering window!
 88
 89
 90
 91SC2 = exu.SystemContainer()
 92SC = exu.SystemContainer()
 93mbs = SC.AddSystem()
 94mbs2 = SC2.AddSystem()
 95
 96CreateSystem(mbs, [0,0,0], color4red)
 97CreateSystem(mbs, [1.2,0,0], color4blue)
 98CreateSystem(mbs2, [0,0,0], color4red)
 99CreateSystem(mbs2, [0.6,-1.2,0], color4green)
100
101SC.AttachToRenderEngine()
102exu.StartRenderer()
103if 'renderState' in exu.sys: #reload old view
104    SC.SetRenderState(exu.sys['renderState'])
105
106Simulate(SC, mbs)
107# mbs.WaitForUserToContinue()
108#SC.DetachFromRenderEngine()
109SC.WaitForRenderEngineStopFlag() #stop before closing
110exu.StopRenderer() #safely close rendering window!
111
112SC2.AttachToRenderEngine()
113exu.StartRenderer()
114if 'renderState' in exu.sys: #reload old view
115    SC2.SetRenderState(exu.sys['renderState'])
116Simulate(SC2, mbs2)
117
118SC2.WaitForRenderEngineStopFlag() #stop before closing
119exu.StopRenderer() #safely close rendering window!
120
121if False:
122
123    mbs.PlotSensor([sens1],[1])