coordinateVectorConstraint.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Example of double pendulum with Mass points and CoordinateVectorConstraint;
  5#
  6# Author:   Johannes Gerstmayr
  7# Date:     2022-03-17
  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 #only import if it does not conflict
 16import numpy as np
 17
 18useGraphics = True #without test
 19#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 20#you can erase the following lines and all exudynTestGlobals related operations if this is not intended to be used as TestModel:
 21try: #only if called from test suite
 22    from modelUnitTests import exudynTestGlobals #for globally storing test results
 23    useGraphics = exudynTestGlobals.useGraphics
 24except:
 25    class ExudynTestGlobals:
 26        pass
 27    exudynTestGlobals = ExudynTestGlobals()
 28#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 29
 30SC = exu.SystemContainer()
 31mbs = SC.AddSystem()
 32
 33doublePendulum = True
 34withUserFunction = True
 35
 36L = 0.8 #length of arm
 37mass = 2.5
 38g = 9.81
 39
 40r = 0.05 #just for graphics
 41d = r/2
 42
 43#add ground object and mass point:
 44sizeRect = 1.2*L*(1+int(doublePendulum))
 45#graphicsBackground = GraphicsDataRectangle(-sizeRect,-sizeRect, sizeRect, 0.2*L, [1,1,1,1]) #for appropriate zoom
 46graphicsBackground = graphics.CheckerBoard(point=[0,-0.5*sizeRect,-2*r],size=sizeRect*1.8)
 47
 48oGround = mbs.AddObject(ObjectGround(referencePosition = [0,0,0],
 49                           visualization = VObjectGround(graphicsData = [graphicsBackground])))
 50
 51
 52graphicsSphere = graphics.Sphere(point=[0,0,0], radius=r, color=graphics.color.steelblue, nTiles = 16)
 53
 54nR0 = mbs.AddNode(Point2D(referenceCoordinates=[L,0]))
 55oR0 = mbs.AddObject(MassPoint2D(nodeNumber=nR0, physicsMass=mass, visualization=VMassPoint2D(graphicsData=[graphicsSphere])))
 56
 57mGround0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition = [0,0,0]))
 58mTip0 = mbs.AddMarker(MarkerNodePosition(nodeNumber=nR0))
 59
 60if not withUserFunction: #with internal terms:
 61    oCD0 = mbs.AddObject(DistanceConstraint(markerNumbers=[mGround0, mTip0], distance=L))
 62else:
 63    #just for drawing, with inactive connector:
 64    mbs.AddObject(DistanceConstraint(markerNumbers=[mGround0, mTip0], distance=L, activeConnector=False))
 65
 66    nGround = mbs.AddNode(NodePointGround())
 67    mCoordsGround = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nGround))
 68    mCoords0 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nR0))
 69
 70    #constraint user function:
 71    def UFconstraint(mbs, t, itemNumber, q, q_t,velocityLevel):
 72        #print("q=", q, ", q_t=", q_t)
 73        return [np.sqrt(q[0]**2 + q[1]**2) - L]
 74
 75    #constraint jacobian user function:
 76    def UFjacobian(mbs, t, itemNumber, q, q_t,velocityLevel):
 77        #print("q=", q, ", q_t=", q_t)
 78        jac  = np.zeros((1,2))
 79
 80        f = np.sqrt(q[0]**2 + q[1]**2)
 81        jac[0,0] = q[0]/f
 82        jac[0,1] = q[1]/f
 83        return jac
 84
 85    mbs.AddObject(CoordinateVectorConstraint(markerNumbers=[mCoordsGround, mCoords0],
 86                                             scalingMarker0=np.zeros((1,2)), #needed to define number of algebraic equations; rows=nAE, cols=len(q) of mCoordsGround + mCoords0
 87                                             constraintUserFunction=UFconstraint,
 88                                             jacobianUserFunction=UFjacobian,
 89                                             visualization=VCoordinateVectorConstraint(show=False)))
 90
 91#
 92mbs.AddLoad(Force(markerNumber = mTip0, loadVector = [0, -mass*g, 0]))
 93
 94fileNameDouble = 'solution/coordVecConstraintRefDouble.txt'
 95fileNameSingle = 'solution/coordVecConstraintRefSingle.txt'
 96
 97sPos0 = mbs.AddSensor(SensorNode(nodeNumber = nR0, storeInternal = True,
 98                                 #fileName=fileNameSingle, #single pendulum
 99                                 outputVariableType=exu.OutputVariableType.Position))
100
101
102#for double pendulum, we add a second link
103if doublePendulum:
104    graphicsSphere = graphics.Sphere(point=[0,0,0], radius=r, color=graphics.color.red, nTiles = 16)
105    nR1 = mbs.AddNode(Point2D(referenceCoordinates=[L*2,0]))
106    oR1 = mbs.AddObject(MassPoint2D(nodeNumber=nR1, physicsMass=mass, visualization=VMassPoint2D(graphicsData=[graphicsSphere])))
107
108    mTip1 = mbs.AddMarker(MarkerNodePosition(nodeNumber=nR1))
109
110    if not withUserFunction: #with internal terms:
111        oCD1 = mbs.AddObject(DistanceConstraint(markerNumbers=[mTip0, mTip1], distance=L))
112    else:
113        #just for drawing, with inactive connector:
114        mbs.AddObject(DistanceConstraint(markerNumbers=[mTip0, mTip1], distance=L, activeConnector=False))
115
116        mCoords0 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nR0))
117        mCoords1 = mbs.AddMarker(MarkerNodeCoordinates(nodeNumber=nR1))
118
119        #constraint user function:
120        def UFconstraint2(mbs, t, itemNumber, q, q_t,velocityLevel):
121            #print("q=", q, ", q_t=", q_t)
122            return [np.sqrt((q[2]-q[0])**2 + (q[3]-q[1])**2) - L]
123
124        #constraint jacobian user function:
125        def UFjacobian2(mbs, t, itemNumber, q, q_t,velocityLevel):
126            #print("q=", q, ", q_t=", q_t)
127            jac  = np.zeros((1,4))
128            f = np.sqrt((q[2]-q[0])**2 + (q[3]-q[1])**2)
129            jac[0,0] =-(q[2]-q[0])/f
130            jac[0,1] =-(q[3]-q[1])/f
131            jac[0,2] = (q[2]-q[0])/f
132            jac[0,3] = (q[3]-q[1])/f
133            return jac
134
135        mbs.AddObject(CoordinateVectorConstraint(markerNumbers=[mCoords0, mCoords1],
136                                                 scalingMarker0=np.zeros((1,2+2)), #needed to define number of algebraic equations; rows=nAE, cols=len(q) of mCoordsGround + mCoords0
137                                                 constraintUserFunction=UFconstraint2,
138                                                 jacobianUserFunction=UFjacobian2,
139                                                 visualization=VCoordinateVectorConstraint(show=False)))
140
141
142    #
143    mbs.AddLoad(Force(markerNumber = mTip1, loadVector = [0, -mass*g, 0]))
144
145    sPos1 = mbs.AddSensor(SensorNode(nodeNumber = nR1, storeInternal = True,
146                                     #fileName=fileNameDouble,
147                                     outputVariableType=exu.OutputVariableType.Position))
148
149
150
151mbs.Assemble()
152
153simulationSettings = exu.SimulationSettings()
154
155# useGraphics=False
156tEnd = 1
157h = 1e-3
158if useGraphics:
159    tEnd = 1
160    simulationSettings.timeIntegration.simulateInRealtime = True
161    simulationSettings.timeIntegration.realtimeFactor = 3
162
163simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
164simulationSettings.timeIntegration.endTime = tEnd
165
166#simulationSettings.solutionSettings.solutionWritePeriod = h
167simulationSettings.timeIntegration.verboseMode = 1
168#simulationSettings.solutionSettings.solutionWritePeriod = tEnd/steps
169
170simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.8 #SHOULD work with 0.9 as well
171
172SC.visualizationSettings.nodes.showBasis=True
173
174if useGraphics:
175    exu.StartRenderer()
176    mbs.WaitForUserToContinue()
177
178mbs.SolveDynamic(simulationSettings)
179
180p0=mbs.GetObjectOutputBody(oR0, exu.OutputVariableType.Position, localPosition=[0,0,0])
181exu.Print("p0=", list(p0))
182u=sum(p0)
183
184exu.Print('solution of coordinateVectorConstraint=',u)
185
186exudynTestGlobals.testError = u - (-1.0825265797698322)
187exudynTestGlobals.testResult = u
188
189
190if useGraphics:
191    SC.WaitForRenderEngineStopFlag()
192    exu.StopRenderer() #safely close rendering window!
193
194    if doublePendulum:
195        mbs.PlotSensor([sPos0,sPos0,sPos1,sPos1], components=[0,1,0,1], closeAll=True)
196    else:
197        mbs.PlotSensor([sPos0,sPos0], components=[0,1], closeAll=True)