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