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