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