addRevoluteJoint.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Test for CreateRevoluteJoint utility function
  5#
  6# Model:    N-link chain of rigid bodies connected with revolute joints
  7#
  8# Author:   Johannes Gerstmayr
  9# Date:     2021-07-01
 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.itemInterface import *
 19from exudyn.utilities import *
 20
 21from math import sin, cos, pi
 22import numpy as np
 23
 24## set up MainSystem mbs
 25SC = exu.SystemContainer()
 26mbs = SC.AddSystem()
 27
 28## 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
 33#g = [0,0,-9.81]
 34g = [0,-9.81,0]
 35
 36## create ground and ground marker
 37oGround=mbs.AddObject(ObjectGround(referencePosition= [-0.5*L,0,0]))
 38mPosLast = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oGround, localPosition=[0,0,0]))
 39A0 = np.eye(3)
 40Alast = A0 #previous marker
 41bodyLast = oGround
 42
 43## set up rotation matrices for relative rotation of joints
 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## for loop to create a chain of 4 bodies under gravity connected with revolute joints
 58for i in range(4):
 59    ### create inertia for block with dimensions [L,d,d] and graphics for block
 60    inertia = InertiaCuboid(density=1000, sideLengths=[L,d,d])
 61    graphicsBody = GraphicsDataOrthoCubePoint([0,0,0], [0.96*L,d,d], color4steelblue)
 62
 63    ### create and add rigid body to mbs
 64    p0 += Alist[i] @ (0.5*vLoc)
 65    oRB = mbs.CreateRigidBody(inertia=inertia,
 66                              referencePosition=p0,
 67                              referenceRotationMatrix=Alist[i],
 68                              gravity=g,
 69                              graphicsDataList=[graphicsBody])
 70    nRB= mbs.GetObject(oRB)['nodeNumber']
 71
 72    body0 = bodyLast
 73    body1 = oRB
 74    ### retrieve reference position for simpler definition of global joint position
 75    point = mbs.GetObjectOutputBody(oRB,exu.OutputVariableType.Position,
 76                                    localPosition=[-0.5*L,0,0],
 77                                    configuration=exu.ConfigurationType.Reference)
 78    #axis = [0,0,1]
 79    axis = axisList[i]
 80
 81    ### set up revolute joint between two bodies, at global position and with global axis
 82    mbs.CreateRevoluteJoint(bodyNumbers=[body0, body1],
 83                            position=point, axis=axis, useGlobalFrame=True,
 84                            axisRadius=0.6*d, axisLength=1.2*d)
 85
 86    # alternative: create revolute joint with local frame for axis and position
 87    # mbs.CreateRevoluteJoint(bodyNumbers=[body0, body1],
 88    #                         position=[0.5*L,0,0], axis=Alast.T@axis, useGlobalFrame=False,
 89    #                         axisRadius=0.6*d, axisLength=1.2*d)
 90
 91    # alternative: with markers and ObjectJointRevoluteZ
 92    # mPos0 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [-0.5*L,0,0]))
 93    # mPos1 = mbs.AddMarker(MarkerBodyRigid(bodyNumber = oRB, localPosition = [ 0.5*L,0,0]))
 94    # useGenericJoint = False #for comparison
 95    # mbs.AddObject(ObjectJointRevoluteZ(markerNumbers = [mPosLast, mPos0],
 96    #                                   rotationMarker0=Alist[i],
 97    #                                   rotationMarker1=Alist[i],
 98    #                                   visualization=VObjectJointRevoluteZ(axesRadius=0.5*d, axesLength=1.2*d)
 99    #                                   ))
100    # mPosLast = mPos1
101
102    bodyLast = oRB
103
104    p0 += Alist[i] @ (0.5*vLoc)
105    Alast = Alist[i]
106
107## assemble and set up simulation settings
108mbs.Assemble()
109
110simulationSettings = exu.SimulationSettings() #takes currently set values or default values
111
112tEnd = 2
113h=0.001  #use small step size to detext contact switching
114
115simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
116simulationSettings.timeIntegration.endTime = tEnd
117simulationSettings.solutionSettings.solutionWritePeriod = 0.01
118simulationSettings.solutionSettings.sensorsWritePeriod = 0.01
119simulationSettings.timeIntegration.verboseMode = 1 #print some progress
120
121simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8
122simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations=True
123simulationSettings.timeIntegration.newton.useModifiedNewton = True
124
125SC.visualizationSettings.nodes.show = True
126SC.visualizationSettings.nodes.drawNodesAsPoint  = False
127SC.visualizationSettings.nodes.showBasis = True
128SC.visualizationSettings.nodes.basisSize = 0.015
129SC.visualizationSettings.connectors.showJointAxes = True
130
131SC.visualizationSettings.openGL.multiSampling=4
132SC.visualizationSettings.openGL.lineWidth=2
133SC.visualizationSettings.window.renderWindowSize = [800,600]
134SC.visualizationSettings.general.drawCoordinateSystem=True
135
136SC.visualizationSettings.general.autoFitScene = False #use loaded render state
137simulationSettings.displayComputationTime = True
138simulationSettings.displayStatistics = True
139
140## start solver
141mbs.SolveDynamic(simulationSettings, showHints=True)
142
143## start solution viewer
144mbs.SolutionViewer() #can also be entered in IPython ...
145
146
147u0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Displacement)
148rot0 = mbs.GetNodeOutput(nRB, exu.OutputVariableType.Rotation)
149exu.Print('u0=',u0,', rot0=', rot0)
150
151result = (abs(u0)+abs(rot0)).sum()
152exu.Print('solution of addRevoluteJoint=',result)