ROSMassPoint.py
You can view and download this file on Github: ROSMassPoint.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN python example how to use ROS and EXUDYN
3#
4# Details: This example shows how to communicate between an exudyn simulation
5# and ROS publisher and subscriber from bash
6# To make use of this example, you need to
7# install ROS (ROS1 noetic) including rospy (see rosInterface.py)
8# prerequisits to use:
9# use a bash terminal to start the roscore with:
10# roscore
11# send force command to add load to the mass point form bash file with:
12# rostopic pub -r 100 /cmd_wrench geometry_msgs/WrenchStamped "..."
13# Author: Martin Sereinig, Peter Manzl
14# Date: 2023-05-31 (created)
15#
16# Copyright:This file is part of Exudyn. Exudyn is free software.
17# You can redistribute it and/or modify it under the terms of the Exudyn license.
18# See 'LICENSE.txt' for more details.
19#
20#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21import numpy as np
22import exudyn as exu
23from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
24import exudyn.graphics as graphics #only import if it does not conflict
25
26# import needed ROS modules and messages
27import rospy
28from geometry_msgs.msg import WrenchStamped, Twist
29from std_msgs.msg import String
30
31# import new exudyn ROS interface class
32import rosInterface as exuROS
33
34# here build inherited class and using within a simple exudyn simulation of one mass spring-damper
35class MyExudynROSInterface(exuROS.ROSInterface):
36 def __init__(self):
37 # use initialisation of super class
38 # this initialize a rosnode with name
39 super().__init__(name='exuROSexample3Dmass')
40 # initialization of all standard publisher done by super class
41 # self.exuPosePublisher
42 # self.exuStringPublisher
43 # self.exuSystemstatePublisher
44 # self.exuTimePublisher
45 # self.exuTwistPublisher
46
47 # use standard publisher functions form super class!
48 # self.PublishPoseUpdate
49 # self.PublishTwistUpdate
50 # self.PublishSystemStateUpdate
51
52 # initialize all subscriber
53 # suitable callback function is auto generated by superclass (using lambda function)
54 # twist subscriber: cmd_vel
55 twistSubsrciberBase = ''
56 twistSubsrciberTopic = 'cmd_vel' # topic to subscribe
57 self.cmd_vel = Twist() # data type of topic, must be named: self.twistSubscriberTopic
58 self.myTwistSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,Twist)
59
60 # wrench subscriber: cmd_wrench
61 twistSubsrciberBase = ''
62 twistSubsrciberTopic = 'cmd_wrench' # topic to subscribe
63 self.cmd_wrench = WrenchStamped() # data type of topic, must be named: self.twistSubscriberTopic
64 self.myWrenchSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,WrenchStamped)
65
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 # string subsriber: /exudyn/SimpleString
73 stringSubsrciberBase2 = 'exudyn/' # namespace of topic
74 stringSubsrciberTopic2 = 'SimpleString' # topic to subscribe
75 self.SimpleString = String() # data type of topic, must be named: self.twistSubscriberTopic
76 self.myStringSubscriber = self.InitSubscriber(stringSubsrciberBase2,stringSubsrciberTopic2,String)
77
78
79# test main function
80def main():
81
82 # build exudyn model
83 SC = exu.SystemContainer()
84 mbs = SC.AddSystem()
85 tRes = 0.001 # step size in s
86 tEnd = 1e5 # simulation time in s
87 # mass and dimension of sphere
88 mass = 6
89 r = 0.03
90 background = graphics.CheckerBoard(point=[-0.5,0,0],
91 normal=[1, 0, 0],
92 color=[0.7]*3+[1],
93 alternatingColor=[0.8]*3+[1])
94
95 graphicsSphere = graphics.Sphere(point=[0,0,0],
96 radius=r,
97 color=(1,0,0,1),
98 nTiles=64)
99
100 origin = [0, 0, 0]
101 bGround = mbs.AddObject(ObjectGround(referencePosition=origin,
102 visualization=VObjectGround(graphicsData=[background])))
103
104 inertiaSphere = InertiaSphere(mass=mass,radius=r)
105
106 # user interaction point
107 # old rigid body call:
108 # [nUIP, bUIP]=AddRigidBody (mainSys = mbs,
109 # inertia = inertiaSphere,
110 # nodeType = str(exu.NodeType.RotationEulerParameters),
111 # position = [origin[0], origin[1], origin[2]],
112 # rotationMatrix = np.eye(3),
113 # angularVelocity = np.zeros(3),
114 # velocity= [0,0,0],
115 # gravity = [0, 0, 0],
116 # graphicsDataList = [graphicsSphere])
117 dictUIP = mbs.CreateRigidBody(
118 inertia=inertiaSphere,
119 referencePosition=[origin[0], origin[1], origin[2]],
120 referenceRotationMatrix=np.eye(3),
121 gravity=[0, 0, 0],
122 graphicsDataList=[graphicsSphere],
123 returnDict=True)
124 [nUIP, bUIP] = [dictUIP['nodeNumber'], dictUIP['bodyNumber']]
125
126 # create markers:
127 mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bGround, localPosition=[0, 0, 0.]))
128 mUIP = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUIP))
129 mUIPLoad = mbs.AddLoad(LoadForceVector(markerNumber = mUIP,loadVector =[0,0,0]))
130
131 k = 100
132 oSpringDamper = mbs.AddObject(RigidBodySpringDamper(markerNumbers=[mGround, mUIP],
133 stiffness=np.eye(6)*k,
134 damping=np.eye(6)*k*5e-2,
135 visualization={'show': False, 'drawSize': -1, 'color': [-1]*4}))
136
137
138 # sensor for position of endpoint of pendulum
139 sensorPos = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
140 outputVariableType=exu.OutputVariableType.Position))
141 sensorOri = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
142 outputVariableType=exu.OutputVariableType.Rotation))
143 sensorVelt = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
144 outputVariableType=exu.OutputVariableType.Velocity))
145 sensorVelr = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
146 outputVariableType=exu.OutputVariableType.AngularVelocity))
147
148 # store sensor value of each step in mbs variable, so that is accessible from user function
149 mbs.variables['pos'] = sensorPos
150 mbs.variables['ori'] = sensorOri
151 mbs.variables['velt'] = sensorVelt
152 mbs.variables['velr'] = sensorVelr
153
154 # initialize ROS interface from own subclass
155 myROSInterface = MyExudynROSInterface()
156
157 print('rosversion: ' + str(myROSInterface.myROSversionEnvInt))
158 rospy.logdebug('node running and publishing')
159
160 # exudyn PreStepUserFunction
161 def PreStepUserFunction(mbs, t):
162 # send position data to ROS
163 myROSInterface.PublishPoseUpdate(mbs,t)
164 # send velocity data to ROS
165 myROSInterface.PublishTwistUpdate(mbs,t)
166 # send system state data to ROS
167 myROSInterface.PublishSystemStateUpdate(mbs,t)
168
169 # get string data from ROS /my_string topic, please use: rostopic pub -r 100 /my_string geometry_msgs/WrenchStamped "..."
170 someMessage = myROSInterface.my_string.data
171 if someMessage != '' :
172 print('mystringdata',someMessage)
173
174 # get wrench data from ROS /cmd_wrench topic, please use: rostopic pub -r 100 /cmd_wrench geometry_msgs/WrenchStamped "..."
175 rosForces = myROSInterface.cmd_wrench.wrench.force
176 rosTorques = myROSInterface.cmd_wrench.wrench.torque
177 print('forces from ROS:', rosForces)
178 print('torques from ROS : ', rosTorques)
179
180 # demo set force to certain value received from ROS /cmd_wrench
181 newForce = [rosForces.x, rosForces.y, rosForces.z]
182 mbs.SetLoadParameter(mUIPLoad,'loadVector',newForce)
183
184 return True
185
186 mbs.SetPreStepUserFunction(PreStepUserFunction)
187 # assemble multi body system with all previous specified properties and components
188 mbs.Assemble()
189 # set simulation settings
190 simulationSettings = exu.SimulationSettings() #takes currently set values or default values
191 simulationSettings.timeIntegration.endTime = tEnd
192 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/tRes)
193 simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100
194 simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10
195 simulationSettings.timeIntegration.verboseMode = 1 # if 0 no output; higher --> more output information about solver
196 simulationSettings.timeIntegration.newton.useModifiedNewton = False
197 simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
198 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
199 simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
200 simulationSettings.timeIntegration.simulateInRealtime = True # crucial for operating with robot
201 simulationSettings.displayStatistics = True
202 simulationSettings.solutionSettings.solutionInformation = "3D Spring Damper"
203 simulationSettings.solutionSettings.writeSolutionToFile = False
204 viewMatrix = np.eye(3) @ RotationMatrixZ(np.pi/2)@ RotationMatrixX(np.pi/2)
205 SC.visualizationSettings.general.autoFitScene = False
206 # set up parameter for usage in WSL2 (Ubuntu 20.04) on Windows 10
207 SC.visualizationSettings.window.startupTimeout = 5000
208 SC.visualizationSettings.interactive.selectionLeftMouse=False
209 SC.visualizationSettings.interactive.selectionRightMouse=False
210
211 exu.StartRenderer(True)
212 exu.SolveDynamic(mbs, simulationSettings)
213
214 return True
215
216# __main__ function for testing the interface
217if __name__ == "__main__":
218 try:
219 main()
220 except rospy.ROSInterruptException:
221 pass