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