addPrismaticJoint.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Create a chain of bodies connected with prismatic joints; Example for CreatePrismaticJoint utility function
  5#
  6# Model:    N-link chain of rigid bodies connected by prismatic joints
  7#
  8# Author:   Johannes Gerstmayr
  9# Date:     2021-07-02
 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# *clean example*
 14#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 15
 16## import libaries
 17import exudyn as exu
 18from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 19import exudyn.graphics as graphics #only import if it does not conflict
 20
 21from math import sin, cos, pi
 22import numpy as np
 23
 24## set up mbs
 25SC = exu.SystemContainer()
 26mbs = SC.AddSystem()
 27
 28## define overall parameters
 29L = 0.4 #length of bodies
 30d = 0.1 #diameter of bodies
 31p0 = [0.,0.,0] #reference position
 32vLoc = np.array([L,0,0]) #last to next joint
 33g = [0,-9.81,0]
 34
 35## create ground with marker
 36oGround=mbs.AddObject(ObjectGround(referencePosition= [-0.5*L,0,0]))
 37mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround, localPosition=[0,0,0]))
 38bodyLast = oGround
 39
 40## set up rotation matrices for relative rotation of joints
 41A0 = np.eye(3)
 42Alast = A0 #previous marker
 43
 44A0 = RotationMatrixX(0)
 45A1 = RotationMatrixY(0.5*pi)
 46A2 = RotationMatrixZ(0.5*pi)
 47A3 = RotationMatrixX(-0.5*pi)
 48Alist=[A0,A1,A2,A3]
 49
 50## set up list of vectors defining axes
 51v0=[0,0,1]
 52v1=[1,1,1]
 53v2=[1,0,0]
 54v3=[0,0,1]
 55axisList=[v0,v1,v2,v3]
 56
 57## loop to create a chain of 4 bodies under gravity connected with prismatic joints
 58for i in range(4):
 59    ### create inertia for block with dimensions [L,d,d]
 60    inertia = InertiaCuboid(density=1000, sideLengths=[L,d,d])
 61    ### create graphics for body as block with (body-fixed) centerPoint, size and color
 62    graphicsBody = graphics.Brick(centerPoint=[0,0,0], size=[0.96*L,d,d], color=graphics.color.steelblue)
 63
 64    ### create and add rigid body to mbs
 65    p0 += Alist[i] @ (0.5*vLoc)
 66    oRB = mbs.CreateRigidBody(inertia=inertia,
 67                              referencePosition=p0,
 68                              referenceRotationMatrix=Alist[i],
 69                              gravity=g,
 70                              graphicsDataList=[graphicsBody])
 71    nRB= mbs.GetObject(oRB)['nodeNumber']
 72
 73    body0 = bodyLast
 74    body1 = oRB
 75    ### retrieve reference position for simpler definition of global joint position
 76    point = mbs.GetObjectOutputBody(oRB,exu.OutputVariableType.Position,
 77                                    localPosition=[-0.5*L,0,0],
 78                                    configuration=exu.ConfigurationType.Reference)
 79    axis = axisList[i]
 80    ### set up prismatic joint between two bodies, at global position and with global axis
 81    mbs.CreatePrismaticJoint(bodyNumbers=[body0, body1], position=point, axis=axis,
 82                             useGlobalFrame=True, axisRadius=0.6*d, axisLength=1.2*d)
 83
 84    bodyLast = oRB
 85
 86    p0 += Alist[i] @ (0.5*vLoc)
 87    Alast = Alist[i]
 88
 89## assemble and set up simulation settings
 90mbs.Assemble()
 91
 92simulationSettings = exu.SimulationSettings() #takes currently set values or default values
 93
 94tEnd = 2
 95h=0.001  #use small step size to detext contact switching
 96
 97simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
 98simulationSettings.timeIntegration.endTime = tEnd
 99simulationSettings.solutionSettings.solutionWritePeriod = 0.005
100#simulationSettings.timeIntegration.simulateInRealtime = True
101simulationSettings.timeIntegration.realtimeFactor = 0.5
102simulationSettings.timeIntegration.verboseMode = 1
103
104simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8
105simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
106simulationSettings.timeIntegration.newton.useModifiedNewton = True
107
108SC.visualizationSettings.nodes.show = True
109SC.visualizationSettings.nodes.drawNodesAsPoint  = False
110SC.visualizationSettings.nodes.showBasis = True
111SC.visualizationSettings.nodes.basisSize = 0.015
112SC.visualizationSettings.connectors.showJointAxes = True
113
114#for snapshot:
115SC.visualizationSettings.openGL.multiSampling=4
116SC.visualizationSettings.openGL.lineWidth=2
117
118SC.visualizationSettings.general.autoFitScene = False #use loaded render state
119useGraphics = True
120if useGraphics:
121    simulationSettings.displayComputationTime = True
122    simulationSettings.displayStatistics = True
123    exu.StartRenderer()
124    ## reload previous render configuration
125    if 'renderState' in exu.sys:
126        SC.SetRenderState(exu.sys[ 'renderState' ])
127else:
128    simulationSettings.solutionSettings.writeSolutionToFile = False
129
130## start solver
131mbs.SolveDynamic(simulationSettings, showHints=True)
132
133## visualization
134mbs.SolutionViewer(runMode=2, runOnStart=True)
135
136## evaluate some results
137u0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Displacement)
138rot0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Rotation)
139exu.Print('u0=',u0,', rot0=', rot0)
140
141result = (abs(u0)+abs(rot0)).sum()
142exu.Print('solution of addPrismaticJoint=',result)