ROSTurtle.py
You can view and download this file on Github: ROSTurtle.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN python example
3#
4# Details: This example shows how to communicate between an exudyn simulation and ROS
5# To make use of this example, you need to
6# install ROS (ROS1 noetic) including rospy (see rosInterface.py)
7# prerequisite to use:
8# use a bash terminal to start the roscore with:
9# roscore
10# You can also use the ROS turtlesim_node to subsrcibe the Twist massage from exudyn
11# use a bash terminal to start the turtlesim node with:
12# rosrun turtlesim turtlesim_node turtle1/cmd_vel:=exudyn/Twist
13# Start example with argument -pub, to use it with external publisher:
14# python3 ROSTurtle.py -pub
15# Start example with argument -noTrack, to not track the turtle:
16# python3 ROSTurtle.py -noTrack
17# For even more ROS functionality create a ROS package (e.q. myExudynInterface) in a catkin workspace,
18# copy files ROSTurtle.py, Turtle.stl and ROSBringupTurtle.launch file (see folder Examples/supplementary) in corresponding folders within the package
19# For more functionality see also: ROSMassPoint.py, ROSBringupTurtle.launch, ROSControlTurtleVelocity.py
20# Author: Martin Sereinig, Peter Manzl
21# Date: 2023-05-31 (created)
22#
23# Copyright:This file is part of Exudyn. Exudyn is free software.
24# You can redistribute it and/or modify it under the terms of the Exudyn license.
25# See 'LICENSE.txt' for more details.
26#
27#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28import numpy as np
29import sys
30import exudyn as exu
31from exudyn.utilities import *
32
33# import needed ROS modules and messages
34import rospy
35from geometry_msgs.msg import Twist
36from std_msgs.msg import String
37
38# import new exudyn ROS interface class
39import rosInterface as exuROS
40
41# here build inherited class and using within a simple exudyn simulation of one mass spring-damper
42class MyExudynROSInterface(exuROS.ROSInterface):
43 def __init__(self):
44 # use initialisation of super class
45 # this initialize a rosnode with name
46 super().__init__(name='ROSexampleTurtle')
47 # initialization of all standard publisher done by super class
48 # self.exuPosePublisher
49 # self.exuStringPublisher
50 # self.exuSystemstatePublisher
51 # self.exuTimePublisher
52 # self.exuTwistPublisher
53
54 # use standard publisher functions form super class!
55 # self.PublishPoseUpdate
56 # self.PublishTwistUpdate
57 # self.PublishSystemStateUpdate
58
59 # initialize all subscriber
60 # suitable callback function is auto generated by superclass (using lambda function)
61 # twist subscriber: cmd_vel
62 twistSubsrciberBase = ''
63 twistSubsrciberTopic = 'cmd_vel' # topic to subscribe
64 self.cmd_vel = Twist() # data type of topic, must be named: self.twistSubscriberTopic
65 self.myTwistSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,Twist)
66 # string subsriber: my_string
67 stringSubsrciberBase = ''
68 stringSubsrciberTopic = 'my_string' # topic to subscribe
69 self.my_string = String() # data type of topic, must be named: self.twistSubscriberTopic
70 self.myStringSubscriber = self.InitSubscriber(stringSubsrciberBase,stringSubsrciberTopic,String)
71
72
73def main():
74
75 #function to check if argument is in sys.argv with try/except
76 def argumentInSysArgv(index):
77 try:
78 sys.argv[index]
79 except IndexError:
80 return ''
81 else:
82 return sys.argv[index]
83
84 # turtle moves in circle and is tracked by default (no arguments)
85 hearToPublisher = False
86 saveTrack = True
87 # parse command line arguments for multiple arguments:
88 # -pub: use external publisher
89 # -noTrack: do not track turtle
90 if len(sys.argv) > 1:
91 for arguments in range(len(sys.argv)):
92 if argumentInSysArgv(arguments) == '-pub':
93 hearToPublisher = True
94 print('Wait for external ROS publisher on /cmd_vel for turtle to move')
95 if argumentInSysArgv(arguments) == '-noTrack':
96 saveTrack = False
97 print('Turtle is not tracked')
98
99 # build exudyn model
100 SC = exu.SystemContainer()
101 mbs = SC.AddSystem()
102 tRes = 0.001 # step size in s
103 tEnd = 1e5 # simulation time in s
104 # density and dimension of box
105 boxdensity = 1e-5
106 boxLength = [0.5, 0.25, 0.1]
107
108 background = GraphicsDataCheckerBoard(point=[0,0,0],
109 normal=[0, 0, 1],
110 color=[0.7]*3+[0.5],
111 alternatingColor=[0.8]*3+[1],
112 nTiles=10,
113 size=10)
114
115 graphicsCube = GraphicsDataOrthoCubePoint(centerPoint = [0,0,0],
116 size = boxLength,
117 color = [1,0.,0.,1.],
118 addNormals = False,
119 addEdges = False,
120 edgeColor = color4black,
121 addFaces = True)
122 inertiaCube = InertiaCuboid(density= boxdensity, sideLengths = boxLength)
123
124
125 origin = [0, 0, 0]
126 bGround = mbs.AddObject(ObjectGround(referencePosition=origin,
127 visualization=VObjectGround(graphicsData=[background])))
128
129 # graphics userfunction definieren:
130 if saveTrack:
131 def graphicsTrajUF(mbs, itemNumber):
132 t = mbs.systemData.GetTime(exu.ConfigurationType.Visualization)
133 # position of turtle stored by sensor in mbs.variables['pos']
134 pOld = mbs.GetSensorStoredData(mbs.variables['pos'])
135 try:
136 iCurr = np.min([np.argmin(abs(pOld[:,0] - t)), len(pOld[:,0])-1])
137 pOld = pOld[:iCurr, :]
138 except:
139 pass
140
141 if len(pOld) > 2:
142 trajData = np.matrix.flatten(pOld[:,1:]).tolist()
143
144 for i in range(int(len(trajData)/3)):
145 trajData[2+3*i] += 0.115 # draw it on top of the robot
146 graphicsTraj = {'type':'Line', 'data': trajData, 'color':color4blue}
147 else:
148 graphicsTraj = []
149 return [graphicsTraj]
150 # add object ground with graphics user function to add turtle track
151 oTrack = mbs.AddObject(ObjectGround(visualization =VObjectGround(graphicsData=[], graphicsDataUserFunction = graphicsTrajUF)))
152
153 graphicsTurtleList = []
154 try:
155 try:
156 path2stl = rospy.get_param('/ROSExampleTurtle/stlFilePath') # node_name/argsname
157 except:
158 path2stl = ''
159 print('stl file path: ', path2stl)
160 turtleRot = RotationMatrixZ(-np.pi/2)
161 stlGrafics = GraphicsDataFromSTLfile(path2stl+'ROSTurtle.stl',color=[1,0,0,1],scale=0.25,pOff=[0.35,0,0], Aoff=turtleRot)
162 graphicsTurtleList += [stlGrafics]
163 except:
164 print('stl not found, maybe wrong directory, use box instead')
165 graphicsTurtleList += [graphicsCube]
166
167 # user interaction point
168 [nUIP, bUIP]=AddRigidBody(mainSys = mbs,
169 inertia = inertiaCube,
170 nodeType = str(exu.NodeType.RotationEulerParameters),
171 position = [origin[0], origin[1], origin[2]],
172 rotationMatrix = np.eye(3),
173 angularVelocity = np.array([0,0,0]),
174 velocity= [0,0,0],
175 gravity = [0, 0, 0],
176 graphicsDataList = graphicsTurtleList)
177
178
179 # create markers:
180 mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bGround, localPosition=[0.0, 0.0, 0.0]))
181 mUIP = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUIP))
182
183 dampingHelper = 1e-4
184 # create userfunction for TorsionalSpringDamper
185 def UFtorque(mbs,t,itemNumber,r,av,k,d,offset):
186 return (av-offset)*d
187 # create TorsionalSpringDamper object
188 oTSD = mbs.AddObject(TorsionalSpringDamper(markerNumbers = [mGround,mUIP],
189 damping = dampingHelper,
190 offset = 0,
191 springTorqueUserFunction = UFtorque))
192 # create userfunction for CartesianSpringDamper
193 def UFforce(mbs, t,itemNumber, u, v, k, d, offset):
194 return [(v[0]-offset[0])*d[0], (v[1]-offset[1])*d[1], (v[2]-offset[2])*d[2]]
195 # create CartesianSpringDamper object
196
197 oCSD = mbs.AddObject(CartesianSpringDamper(markerNumbers = [mGround, mUIP],
198 damping = [dampingHelper,dampingHelper,dampingHelper],
199 offset = [0,0,0],
200 springForceUserFunction = UFforce,
201 visualization=VObjectConnectorCartesianSpringDamper(show=False)))
202
203 sensorPos = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
204 outputVariableType=exu.OutputVariableType.Position,storeInternal=True))
205 sensorOri = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
206 outputVariableType=exu.OutputVariableType.Rotation))
207 sensorVelt = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
208 outputVariableType=exu.OutputVariableType.Velocity))
209 sensorVelr = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
210 outputVariableType=exu.OutputVariableType.AngularVelocity))
211
212 # store sensor value of each step in mbs variable, so that is accessible from user function
213 mbs.variables['pos'] = sensorPos # just needed if sensor is used for sensor information
214 mbs.variables['ori'] = sensorOri # just needed if sensor is used for sensor information
215 mbs.variables['velt'] = sensorVelt # just needed if sensor is used for sensor information
216 mbs.variables['velr'] = sensorVelr # just needed if sensor is used for sensor information
217 mbs.variables['hearToPublisher'] = hearToPublisher # needed to use with and without external publisher
218 mbs.variables['nodeNumber'] = nUIP # just needed if nodeNumber is used for sensor information
219
220 # initialize ROS interface from own subclass
221 myROSInterface = MyExudynROSInterface()
222
223 print('rosversion: ' + str(myROSInterface.myROSversionEnvInt))
224 rospy.logdebug('node running and publishing')
225
226 # exudyn PreStepUserFunction
227 def PreStepUserFunction(mbs, t):
228
229 # get velocity data from ROS /cmd_vel topic, please use: rostopic pub -r 100 /cmd_vel geometry_msgs/Twist "..."
230 rosLinearVelo = myROSInterface.cmd_vel.linear
231 rosAngularVelo = myROSInterface.cmd_vel.angular
232
233 # EXAMPLE to get position and orientation from exudyn turtle via sensor
234 turtlePosition = mbs.GetSensorValues(mbs.variables['pos'])
235 turtleOrientation = mbs.GetSensorValues(mbs.variables['ori'])[2]
236 turtleOrientationMatrix = RotationMatrixZ(turtleOrientation)
237
238
239 # set velocities to exudyn turtle simulation
240 if mbs.variables['hearToPublisher'] ==True:
241 # exudyn turtle hears on publisher
242 desiredLinearVelocity = turtleOrientationMatrix @ [rosLinearVelo.x, rosLinearVelo.y, rosLinearVelo.z]
243 desiredAngularVelocity = [rosAngularVelo.x, rosAngularVelo.y, rosAngularVelo.z]
244 else:
245 # exudyn turtle moves in a circle
246 desiredLinearVelocity = turtleOrientationMatrix @ [1, 0, 0]
247 desiredAngularVelocity = [0, 0, 1]
248
249 mbs.SetObjectParameter(oCSD, 'offset', desiredLinearVelocity)
250 mbs.SetObjectParameter(oTSD, 'offset', desiredAngularVelocity[2])
251
252 # send velocity data to ROS
253 myROSInterface.PublishTwistUpdate(mbs,t)
254 # send position data to ROS
255 myROSInterface.PublishPoseUpdate(mbs,t)
256 # send system state data to ROS
257 myROSInterface.PublishSystemStateUpdate(mbs,t)
258
259 return True
260
261
262
263
264
265 mbs.SetPreStepUserFunction(PreStepUserFunction)
266 # assemble multi body system with all previous specified properties and components
267 mbs.Assemble()
268 # set simulation settings
269 simulationSettings = exu.SimulationSettings() #takes currently set values or default values
270 simulationSettings.timeIntegration.endTime = tEnd
271 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/tRes)
272 simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100
273 simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10
274 simulationSettings.timeIntegration.verboseMode = 1 # if 0 no output; higher --> more output information about solver
275 simulationSettings.timeIntegration.newton.useModifiedNewton = False
276 simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
277 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
278 simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
279
280 simulationSettings.timeIntegration.simulateInRealtime = True # crucial for operating with robot
281 simulationSettings.displayStatistics = True
282 simulationSettings.solutionSettings.solutionInformation = "Exudyn-ROS turtle"
283
284 simulationSettings.solutionSettings.writeSolutionToFile = False
285 SC.visualizationSettings.general.autoFitScene = True
286 # set up parameter for usage in WSL2 (Ubuntu 20.04) on Windows 10
287 SC.visualizationSettings.interactive.trackMarker = mUIP
288 SC.visualizationSettings.window.startupTimeout = 8000
289 SC.visualizationSettings.openGL.initialZoom = 0.2
290 SC.visualizationSettings.openGL.initialMaxSceneSize= 0.7
291 SC.visualizationSettings.interactive.selectionLeftMouse=False
292 SC.visualizationSettings.interactive.selectionRightMouse=False
293
294
295 exu.StartRenderer(True)
296 exu.SolveDynamic(mbs, simulationSettings)
297
298 return True
299
300# create a function
301
302# __main__ function
303if __name__ == "__main__":
304 try:
305 main()
306 except rospy.ROSInterruptException:
307 pass