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 [nUIP, bUIP]=AddRigidBody(mainSys = mbs,
170 inertia = inertiaCube,
171 nodeType = str(exu.NodeType.RotationEulerParameters),
172 position = [origin[0], origin[1], origin[2]],
173 rotationMatrix = np.eye(3),
174 angularVelocity = np.array([0,0,0]),
175 velocity= [0,0,0],
176 gravity = [0, 0, 0],
177 graphicsDataList = graphicsTurtleList)
178
179
180 # create markers:
181 mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bGround, localPosition=[0.0, 0.0, 0.0]))
182 mUIP = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUIP))
183
184 dampingHelper = 1e-4
185 # create userfunction for TorsionalSpringDamper
186 def UFtorque(mbs,t,itemNumber,r,av,k,d,offset):
187 return (av-offset)*d
188 # create TorsionalSpringDamper object
189 oTSD = mbs.AddObject(TorsionalSpringDamper(markerNumbers = [mGround,mUIP],
190 damping = dampingHelper,
191 offset = 0,
192 springTorqueUserFunction = UFtorque))
193 # create userfunction for CartesianSpringDamper
194 def UFforce(mbs, t,itemNumber, u, v, k, d, offset):
195 return [(v[0]-offset[0])*d[0], (v[1]-offset[1])*d[1], (v[2]-offset[2])*d[2]]
196 # create CartesianSpringDamper object
197
198 oCSD = mbs.AddObject(CartesianSpringDamper(markerNumbers = [mGround, mUIP],
199 damping = [dampingHelper,dampingHelper,dampingHelper],
200 offset = [0,0,0],
201 springForceUserFunction = UFforce,
202 visualization=VObjectConnectorCartesianSpringDamper(show=False)))
203
204 sensorPos = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
205 outputVariableType=exu.OutputVariableType.Position,storeInternal=True))
206 sensorOri = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
207 outputVariableType=exu.OutputVariableType.Rotation))
208 sensorVelt = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
209 outputVariableType=exu.OutputVariableType.Velocity))
210 sensorVelr = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
211 outputVariableType=exu.OutputVariableType.AngularVelocity))
212
213 # store sensor value of each step in mbs variable, so that is accessible from user function
214 mbs.variables['pos'] = sensorPos # just needed if sensor is used for sensor information
215 mbs.variables['ori'] = sensorOri # just needed if sensor is used for sensor information
216 mbs.variables['velt'] = sensorVelt # just needed if sensor is used for sensor information
217 mbs.variables['velr'] = sensorVelr # just needed if sensor is used for sensor information
218 mbs.variables['hearToPublisher'] = hearToPublisher # needed to use with and without external publisher
219 mbs.variables['nodeNumber'] = nUIP # just needed if nodeNumber is used for sensor information
220
221 # initialize ROS interface from own subclass
222 myROSInterface = MyExudynROSInterface()
223
224 print('rosversion: ' + str(myROSInterface.myROSversionEnvInt))
225 rospy.logdebug('node running and publishing')
226
227 # exudyn PreStepUserFunction
228 def PreStepUserFunction(mbs, t):
229
230 # get velocity data from ROS /cmd_vel topic, please use: rostopic pub -r 100 /cmd_vel geometry_msgs/Twist "..."
231 rosLinearVelo = myROSInterface.cmd_vel.linear
232 rosAngularVelo = myROSInterface.cmd_vel.angular
233
234 # EXAMPLE to get position and orientation from exudyn turtle via sensor
235 turtlePosition = mbs.GetSensorValues(mbs.variables['pos'])
236 turtleOrientation = mbs.GetSensorValues(mbs.variables['ori'])[2]
237 turtleOrientationMatrix = RotationMatrixZ(turtleOrientation)
238
239
240 # set velocities to exudyn turtle simulation
241 if mbs.variables['hearToPublisher'] ==True:
242 # exudyn turtle hears on publisher
243 desiredLinearVelocity = turtleOrientationMatrix @ [rosLinearVelo.x, rosLinearVelo.y, rosLinearVelo.z]
244 desiredAngularVelocity = [rosAngularVelo.x, rosAngularVelo.y, rosAngularVelo.z]
245 else:
246 # exudyn turtle moves in a circle
247 desiredLinearVelocity = turtleOrientationMatrix @ [1, 0, 0]
248 desiredAngularVelocity = [0, 0, 1]
249
250 mbs.SetObjectParameter(oCSD, 'offset', desiredLinearVelocity)
251 mbs.SetObjectParameter(oTSD, 'offset', desiredAngularVelocity[2])
252
253 # send velocity data to ROS
254 myROSInterface.PublishTwistUpdate(mbs,t)
255 # send position data to ROS
256 myROSInterface.PublishPoseUpdate(mbs,t)
257 # send system state data to ROS
258 myROSInterface.PublishSystemStateUpdate(mbs,t)
259
260 return True
261
262
263
264
265
266 mbs.SetPreStepUserFunction(PreStepUserFunction)
267 # assemble multi body system with all previous specified properties and components
268 mbs.Assemble()
269 # set simulation settings
270 simulationSettings = exu.SimulationSettings() #takes currently set values or default values
271 simulationSettings.timeIntegration.endTime = tEnd
272 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/tRes)
273 simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100
274 simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10
275 simulationSettings.timeIntegration.verboseMode = 1 # if 0 no output; higher --> more output information about solver
276 simulationSettings.timeIntegration.newton.useModifiedNewton = False
277 simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
278 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
279 simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
280
281 simulationSettings.timeIntegration.simulateInRealtime = True # crucial for operating with robot
282 simulationSettings.displayStatistics = True
283 simulationSettings.solutionSettings.solutionInformation = "Exudyn-ROS turtle"
284
285 simulationSettings.solutionSettings.writeSolutionToFile = False
286 SC.visualizationSettings.general.autoFitScene = True
287 # set up parameter for usage in WSL2 (Ubuntu 20.04) on Windows 10
288 SC.visualizationSettings.interactive.trackMarker = mUIP
289 SC.visualizationSettings.window.startupTimeout = 8000
290 SC.visualizationSettings.openGL.initialZoom = 0.2
291 SC.visualizationSettings.openGL.initialMaxSceneSize= 0.7
292 SC.visualizationSettings.interactive.selectionLeftMouse=False
293 SC.visualizationSettings.interactive.selectionRightMouse=False
294
295
296 exu.StartRenderer(True)
297 exu.SolveDynamic(mbs, simulationSettings)
298
299 return True
300
301# create a function
302
303# __main__ function
304if __name__ == "__main__":
305 try:
306 main()
307 except rospy.ROSInterruptException:
308 pass