genericJointUserFunctionTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test for GenericJoint with userFunction
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2020-04-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.utilities import * #includes itemInterface and rigidBodyUtilities
 15import exudyn.graphics as graphics
 16from exudyn.FEM import *
 17
 18import numpy as np
 19
 20testIsActive = exu.sys.get('testIsActive', False)
 21
 22SC = exu.SystemContainer()
 23mbs = SC.AddSystem()
 24
 25nBodies = 5
 26color = [0.1,0.1,0.8,1]
 27s = 0.1 #width of cube
 28sx = 3*s #length of cube/body
 29cPosZ = 0. #offset of constraint in z-direction
 30zz = sx * (nBodies+1)*2 #max size of background
 31
 32background0 = graphics.Lines([[-zz,-zz,0], [zz,-zz,0], [zz,2.5*sx,0], [-zz,2.5*sx,0], [-zz,-zz,0]], color=color)
 33oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], 
 34                                   visualization=VObjectGround(graphicsData= [background0])))
 35mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround, 
 36                                            localPosition=[-sx,0,0]))
 37oRB = -1 #for output value
 38
 39def UFgenericJoint(mbs, t, itemIndex, param):
 40    phiZ = -2*pi*0.5*(1.-np.cos(t*2*pi / 4)) / nBodies
 41    phiX = -0.5*pi*0.5*(1.-np.cos(t*2*pi / 4)) / nBodies
 42    #exu.Print("phi=", phi)
 43    return [0,0,0, phiX,0,phiZ]
 44
 45#create a chain of bodies:
 46for i in range(nBodies):
 47    f = 0 #factor for initial velocities
 48    omega0 = [0,50.*f,20*f] #arbitrary initial angular velocity
 49    ep0 = eulerParameters0 #no rotation
 50    ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
 51
 52    p0 = [-sx*0+i*2*sx,0.,0] #reference position
 53    v0 = [0.2*f,0.,0.] #initial translational velocity
 54
 55    nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0, 
 56                                      initialVelocities=v0+list(ep_t0)))
 57    #nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=[0,0,0,1,0,0,0], initialVelocities=[0,0,0,0,0,0,0]))
 58    oGraphics = graphics.Brick(centerPoint=[0, 0, 0], size=[2*sx, 2*s, 2*s], addFaces=False, addEdges=True, edgeColor=[0.8,0.1,0.1,1])
 59    oRB = mbs.AddObject(ObjectRigidBody(mass=2, 
 60                                        inertia=[6,1,6,0,0,0], 
 61                                        nodeNumber=nRB, 
 62                                        centerOfMass=[0,0,0],
 63                                        visualization=VObjectRigidBody(graphicsData=[oGraphics])))
 64
 65    mMassRB = mbs.AddMarker(MarkerBodyMass(bodyNumber = oRB))
 66    mbs.AddLoad(Gravity(markerNumber = mMassRB, loadVector=[0.,-9.81,0.])) #gravity in negative z-direction
 67
 68    if i==0:
 69        mPos = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [-sx,0.,cPosZ*0]))
 70    else:
 71        mPos = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [-sx,0.,cPosZ]))
 72
 73    val=1
 74    if i==0: val=0
 75    if i < 2:
 76        mbs.AddObject(GenericJoint(markerNumbers = [mPos, mPosLast], constrainedAxes=[val,1,1, 1,1,1],
 77                                    offsetUserFunctionParameters=[0,0,0, 0,0,0],
 78                                    offsetUserFunction=UFgenericJoint))
 79    else:
 80        if i==3:
 81            mbs.AddObject(GenericJoint(markerNumbers = [mPosLast, mPos], constrainedAxes=[1,1,1, 1,0,1]))
 82        elif i==4:
 83            mbs.AddObject(GenericJoint(markerNumbers = [mPosLast, mPos], constrainedAxes=[1,1,1, 1,1,0]))
 84        else:
 85            mbs.AddObject(SphericalJoint(markerNumbers = [mPosLast, mPos], constrainedAxes=[1,1,1]))
 86
 87    #marker for next chain body
 88    mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [sx,0.,cPosZ]))
 89
 90
 91mbs.Assemble()
 92#exu.Print(mbs)
 93
 94simulationSettings = exu.SimulationSettings() #takes currently set values or default values
 95
 96fact = 500
 97simulationSettings.timeIntegration.numberOfSteps = 1*fact
 98simulationSettings.timeIntegration.endTime = 0.005*fact
 99simulationSettings.solution.file.writePeriod = simulationSettings.timeIntegration.endTime/fact*10
100simulationSettings.timeIntegration.verboseMode = 1
101
102simulationSettings.timeIntegration.newton.useModifiedNewton = True
103simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = False
104simulationSettings.timeIntegration.generalizedAlpha.useNewmark = False
105simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well 
106
107simulationSettings.solution.file.information = "rigid body tests"
108SC.visualizationSettings.nodes.defaultSize = 0.05
109simulationSettings.show.computationTime = False
110#simulationSettings.show.statistics = True
111
112
113
114if not testIsActive:
115    SC.renderer.Start()
116    SC.renderer.DoIdleTasks()
117
118mbs.SolveDynamic(simulationSettings)#, experimentalNewSolver=True)
119
120
121#+++++++++++++++++++++++++++++++++++++++++++++
122#compute TestModel error for EulerParameters and index2 solver
123pos = mbs.GetObjectOutputBody(oRB,exu.OutputVariableType.Position, localPosition=[0,0,0])
124exu.Print('pos=',pos)
125u = 0
126for i in range(3): #take sum of all coordinates
127    u += abs(pos[i])
128
129exu.Print('solution of GenericJointTest=',u)
130
131exu.sys['testResult'] = u
132
133
134if not testIsActive:
135    #SC.renderer.DoIdleTasks()
136    SC.renderer.Stop() #safely close rendering window!