sphericalJointTest.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Simulate Chain with 3D rigid bodies and SphericalJoint;
  5#           Also test MarkerNodePosition
  6#
  7# Author:   Johannes Gerstmayr
  8# Date:     2020-04-09
  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 exudyn.graphics as graphics
 17
 18testIsActive = exu.sys.get('testIsActive', False)
 19
 20SC = exu.SystemContainer()
 21mbs = SC.AddSystem()
 22
 23nBodies = 4
 24color = [0.1,0.1,0.8,1]
 25s = 0.1 #width of cube
 26sx = 3*s #lengt of cube/body
 27cPosZ = 0.1 #offset of constraint in z-direction
 28zz = sx * (nBodies+1)*2 #max size of background
 29
 30background0 = graphics.Lines([[-zz,-zz,0], [zz,-zz,0], [zz,sx,0], [-zz,sx,0], [-zz,-zz,0]], color=color)
 31oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0], 
 32                                   visualization=VObjectGround(graphicsData= [background0])))
 33mPosLast = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oGround, 
 34                                            localPosition=[-sx,0,cPosZ*0]))
 35
 36#create a chain of bodies:
 37for i in range(nBodies):
 38    f = 0 #factor for initial velocities
 39    omega0 = [0,50.*f,20*f] #arbitrary initial angular velocity
 40    ep0 = eulerParameters0 #no rotation
 41    ep_t0 = AngularVelocity2EulerParameters_t(omega0, ep0)
 42
 43    p0 = [-sx+i*2*sx,0.,0] #reference position
 44    v0 = [0.2*f,0.,0.] #initial translational velocity
 45
 46    nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=p0+ep0, 
 47                                      initialVelocities=v0+list(ep_t0)))
 48    #nRB = mbs.AddNode(NodeRigidBodyEP(referenceCoordinates=[0,0,0,1,0,0,0], initialVelocities=[0,0,0,0,0,0,0]))
 49    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])
 50    oRB = mbs.AddObject(ObjectRigidBody(mass=2, 
 51                                        inertia=[6,1,6,0,0,0], 
 52                                        nodeNumber=nRB, 
 53                                        visualization=VObjectRigidBody(graphicsData=[oGraphics])))
 54
 55    mMassRB = mbs.AddMarker(MarkerBodyMass(bodyNumber = oRB))
 56    mbs.AddLoad(Gravity(markerNumber = mMassRB, loadVector=[0.,-9.81,0.])) #gravity in negative z-direction
 57
 58    if i==0:
 59        #mPos = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition = [-sx*0,0.,cPosZ*0]))
 60        mPos = mbs.AddMarker(MarkerNodePosition(nodeNumber=nRB))
 61    else:
 62        mPos = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition = [-sx,0.,cPosZ]))
 63
 64    #alternative with spring-damper:    
 65    #mbs.AddObject(ObjectConnectorCartesianSpringDamper(markerNumbers = [mPosLast, mPos], 
 66    #                                                   stiffness=[k,k,k], damping=[d,d,d])) #gravity in negative z-direction
 67    axes = [1,1,1]
 68    if (i==0):
 69        axes = [0,1,1]
 70
 71    mbs.AddObject(SphericalJoint(markerNumbers = [mPosLast, mPos], constrainedAxes=axes))
 72
 73    #marker for next chain body
 74    mPosLast = mbs.AddMarker(MarkerBodyPosition(bodyNumber = oRB, localPosition = [sx,0.,cPosZ]))
 75
 76
 77mbs.Assemble()
 78#exu.Print(mbs)
 79
 80simulationSettings = exu.SimulationSettings() #takes currently set values or default values
 81
 82fact = 1000
 83simulationSettings.timeIntegration.numberOfSteps = 1*fact
 84simulationSettings.timeIntegration.endTime = 0.001*fact
 85simulationSettings.solution.file.writePeriod = simulationSettings.timeIntegration.endTime/fact*10
 86simulationSettings.timeIntegration.verboseMode = 1
 87
 88simulationSettings.timeIntegration.newton.useModifiedNewton = True
 89simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = False
 90simulationSettings.timeIntegration.generalizedAlpha.useNewmark = False
 91simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.6 #0.6 works well 
 92
 93simulationSettings.solution.file.information = "rigid body tests"
 94SC.visualizationSettings.nodes.defaultSize = 0.05
 95#simulationSettings.show.computationTime = True
 96#simulationSettings.show.statistics = True
 97
 98if not testIsActive:
 99    SC.renderer.Start()
100    SC.renderer.DoIdleTasks()
101
102mbs.SolveDynamic(simulationSettings)
103
104#+++++++++++++++++++++++++++++++++++++++++++++
105sol = mbs.systemData.GetODE2Coordinates(); 
106solref = mbs.systemData.GetODE2Coordinates(configuration=exu.ConfigurationType.Reference); 
107#exu.Print('sol=',sol)
108u = 0
109for i in range(14): #take coordinates of first two bodies
110    u += abs(sol[i]+solref[i])
111
112exu.Print('solution of sphericalJointTest=',u)
113
114exu.sys['testResult'] = u
115
116
117if not testIsActive:
118    #SC.renderer.DoIdleTasks()
119    SC.renderer.Stop() #safely close rendering window!