TCPIPexudynMatlab.py

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

  1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  2# This is an EXUDYN example
  3#
  4# Details:  Example for connecting MATLAB with Exudyn/Python via TCP/IP
  5#           See file TCPIPmatlab.slx for the according Simulink model
  6#
  7# Author:   Johannes Gerstmayr
  8# Date:     2021-11-06
  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.itemInterface import *
 16from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
 17import exudyn.graphics as graphics #only import if it does not conflict
 18from exudyn.graphicsDataUtilities import *
 19
 20import numpy as np
 21
 22
 23#the following way works between Python and MATLAB-Simulink (client),
 24#and gives stable results(with only delay of one step):
 25#
 26# TCP/IP Client Send:
 27#   priority = 2 (in properties)
 28#   blocking = false
 29#   Transfer Delay on (but off also works)
 30# TCP/IP Client Receive:
 31#   priority = 1 (in properties)
 32#   blocking = true
 33#   Sourec Data type = double
 34#   data size = number of double in packer
 35#   Byte order = BigEndian
 36#   timeout = 10
 37
 38
 39
 40
 41#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 42#set up double pendulum:
 43
 44SC = exu.SystemContainer()
 45mbs = SC.AddSystem()
 46
 47#create an environment for mini example
 48
 49
 50L = 1
 51fL = 2.5
 52background = graphics.Quad([[-fL*L, -fL*L, 0],[fL*L, -fL*L, 0],[fL*L, 1*L, 0],[-fL*L, 1*L, 0]],
 53                              graphics.color.darkgrey, nTiles=8,
 54                              alternatingColor=graphics.color.lightgrey)
 55oGround=mbs.AddObject(ObjectGround(referencePosition= [0,0,0],
 56                                   visualization=VObjectGround(graphicsData= [background])))
 57a = L     #x-dim of pendulum
 58b = 0.05    #y-dim of pendulum
 59massRigid = 12
 60inertiaRigid = massRigid/12*(2*a)**2
 61g = 9.81    # gravity
 62
 63graphicsCube = graphics.Brick(centerPoint=[0,0,0], size=[L,b,b], color=graphics.color.steelblue)
 64nRigid0 = mbs.AddNode(Rigid2D(referenceCoordinates=[0.5*L,0,0], initialVelocities=[0,0,0]));
 65oRigid0 = mbs.AddObject(RigidBody2D(physicsMass=massRigid, physicsInertia=inertiaRigid,nodeNumber=nRigid0,
 66                                   visualization=VObjectRigidBody2D(graphicsData= [graphicsCube])))
 67
 68mR0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid0, localPosition=[-0.5*L,0.,0.])) #support point
 69mR0com = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oRigid0, localPosition=[ 0.,0.,0.])) #mid point
 70mR0end = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid0, localPosition=[ 0.5*L,0.,0.])) #end point
 71
 72mG0 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oGround, localPosition=[0.,0.,0.]))
 73mbs.AddObject(RevoluteJoint2D(markerNumbers=[mG0,mR0]))
 74
 75mbs.AddLoad(Force(markerNumber = mR0com, loadVector = [0, -massRigid*g, 0]))
 76
 77nRigid1 = mbs.AddNode(Rigid2D(referenceCoordinates=[1.5*L,0,0], initialVelocities=[0,0,0]));
 78oRigid1 = mbs.AddObject(RigidBody2D(physicsMass=massRigid, physicsInertia=inertiaRigid,nodeNumber=nRigid1,
 79                                   visualization=VObjectRigidBody2D(graphicsData= [graphicsCube])))
 80
 81mR1 = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid1, localPosition=[-0.5*L,0.,0.])) #support point
 82mR1com = mbs.AddMarker(MarkerBodyPosition(bodyNumber=oRigid1, localPosition=[ 0.,0.,0.])) #mid point
 83
 84mbs.AddObject(RevoluteJoint2D(markerNumbers=[mR0end,mR1]))
 85
 86mbs.AddLoad(Force(markerNumber = mR1com, loadVector = [0, -massRigid*g, 0]))
 87
 88#++++++++++++++++++++++++++++++++++++++++++++++++++
 89#damper:
 90mR0C2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRigid0, coordinate=2)) #phi
 91mR1C2 = mbs.AddMarker(MarkerNodeCoordinate(nodeNumber=nRigid1, coordinate=2)) #phi
 92mbs.AddObject(CoordinateSpringDamper(markerNumbers=[mR0C2,mR1C2],
 93                                     stiffness=0, damping=10,
 94                                     visualization=VCoordinateSpringDamper(show=False)))
 95
 96#%%++++++++++++++++++++++++++++++++++++++++++++++++++
 97#connect to MATLAB:
 98loadTorque = mbs.AddLoad(Torque(markerNumber = mR0com, loadVector = [0, 0, 0]))
 99sensorAngle = mbs.AddSensor(SensorBody(bodyNumber=oRigid0, outputVariableType=exu.OutputVariableType.Rotation,
100                                       fileName='solution/test.txt',
101                                       writeToFile=False))
102sensorAngle_t = mbs.AddSensor(SensorBody(bodyNumber=oRigid0, outputVariableType=exu.OutputVariableType.AngularVelocity,
103                                       fileName='solution/test_t.txt',
104                                       writeToFile=False))
105
106
107
108mbs.sys['TCPIPobject'] = CreateTCPIPconnection(sendSize=3, receiveSize=2,
109                                               bigEndian=True, verbose=True)
110sampleTime = 0.01 #sample time in MATLAB! must be same!
111mbs.variables['tLast'] = 0
112
113def PreStepUserFunction(mbs, t):
114    if t >= mbs.variables['tLast'] + sampleTime:
115        mbs.variables['tLast'] += sampleTime
116
117        tcp = mbs.sys['TCPIPobject']
118        phi0 = mbs.GetSensorValues(sensorAngle)
119        #print(phi0)
120        phi0_t = mbs.GetSensorValues(sensorAngle_t)[2]
121
122        y = TCPIPsendReceive(tcp, np.array([t, phi0, phi0_t])) #time, torque
123        tau = y[1]
124        mbs.SetLoadParameter(loadTorque, 'loadVector',[0,0,tau])
125    return True
126
127
128try:
129    mbs.SetPreStepUserFunction(PreStepUserFunction)
130
131    #%%++++++++++++++++++++++++++++++++++++++++++++++++++
132    mbs.Assemble()
133    print(mbs)
134
135    simulationSettings = exu.SimulationSettings() #takes currently set values or default values
136
137    h = 0.002
138    tEnd = 10
139    simulationSettings.timeIntegration.numberOfSteps = int(tEnd/h)
140    simulationSettings.timeIntegration.endTime = tEnd
141    simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100 #10000
142    simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10
143    simulationSettings.timeIntegration.verboseMode = 1
144    # simulationSettings.timeIntegration.simulateInRealtime = True
145
146    simulationSettings.timeIntegration.newton.useModifiedNewton = False
147    simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
148    simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
149    simulationSettings.displayStatistics = True
150
151    #SC.visualizationSettings.nodes.defaultSize = 0.05
152
153    simulationSettings.solutionSettings.solutionInformation = "Rigid pendulum"
154
155    exu.StartRenderer()
156
157
158    mbs.SolveDynamic(simulationSettings)
159
160    SC.WaitForRenderEngineStopFlag()
161    exu.StopRenderer() #safely close rendering window!
162
163finally:
164    CloseTCPIPconnection(mbs.sys['TCPIPobject'])