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