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    [nUIP, bUIP]=AddRigidBody(mainSys = mbs,
108                                inertia = inertiaSphere,
109                                nodeType = str(exu.NodeType.RotationEulerParameters),
110                                position = [origin[0], origin[1], origin[2]],
111                                rotationMatrix = np.eye(3),
112                                angularVelocity = np.zeros(3),
113                                velocity= [0,0,0],
114                                gravity = [0, 0, 0],
115                                graphicsDataList = [graphicsSphere])
116
117    # create markers:
118    mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bGround, localPosition=[0, 0, 0.]))
119    mUIP = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUIP))
120    mUIPLoad = mbs.AddLoad(LoadForceVector(markerNumber = mUIP,loadVector =[0,0,0]))
121
122    k = 100
123    oSpringDamper = mbs.AddObject(RigidBodySpringDamper(markerNumbers=[mGround, mUIP],
124                                                        stiffness=np.eye(6)*k,
125                                                        damping=np.eye(6)*k*5e-2,
126                                                        visualization={'show': False, 'drawSize': -1, 'color': [-1]*4}))
127
128
129    # sensor for position of endpoint of pendulum
130    sensorPos = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
131                                            outputVariableType=exu.OutputVariableType.Position))
132    sensorOri = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
133                                            outputVariableType=exu.OutputVariableType.Rotation))
134    sensorVelt = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
135                                            outputVariableType=exu.OutputVariableType.Velocity))
136    sensorVelr = mbs.AddSensor(SensorBody(bodyNumber=bUIP,
137                                            outputVariableType=exu.OutputVariableType.AngularVelocity))
138
139    # store sensor value of each step in mbs variable, so that is accessible from user function
140    mbs.variables['pos'] = sensorPos
141    mbs.variables['ori'] = sensorOri
142    mbs.variables['velt'] = sensorVelt
143    mbs.variables['velr'] = sensorVelr
144
145    # initialize ROS interface from own subclass
146    myROSInterface = MyExudynROSInterface()
147
148    print('rosversion: ' + str(myROSInterface.myROSversionEnvInt))
149    rospy.logdebug('node running and publishing')
150
151    # exudyn PreStepUserFunction
152    def PreStepUserFunction(mbs, t):
153        # send position data to ROS
154        myROSInterface.PublishPoseUpdate(mbs,t)
155        # send velocity data to ROS
156        myROSInterface.PublishTwistUpdate(mbs,t)
157        # send system state data to ROS
158        myROSInterface.PublishSystemStateUpdate(mbs,t)
159
160        # get string data from ROS /my_string topic, please use: rostopic pub -r 100 /my_string geometry_msgs/WrenchStamped "..."
161        someMessage = myROSInterface.my_string.data
162        if someMessage != '' :
163            print('mystringdata',someMessage)
164
165        # get wrench data from ROS /cmd_wrench topic, please use: rostopic pub -r 100 /cmd_wrench geometry_msgs/WrenchStamped "..."
166        rosForces = myROSInterface.cmd_wrench.wrench.force
167        rosTorques = myROSInterface.cmd_wrench.wrench.torque
168        print('forces from ROS:', rosForces)
169        print('torques from ROS : ', rosTorques)
170
171        # demo set force to certain value received from ROS /cmd_wrench
172        newForce = [rosForces.x, rosForces.y, rosForces.z]
173        mbs.SetLoadParameter(mUIPLoad,'loadVector',newForce)
174
175        return True
176
177    mbs.SetPreStepUserFunction(PreStepUserFunction)
178    # assemble multi body system with all previous specified properties and components
179    mbs.Assemble()
180    # set simulation settings
181    simulationSettings = exu.SimulationSettings() #takes currently set values or default values
182    simulationSettings.timeIntegration.endTime = tEnd
183    simulationSettings.timeIntegration.numberOfSteps = int(tEnd/tRes)
184    simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100
185    simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10
186    simulationSettings.timeIntegration.verboseMode = 1 # if 0 no output; higher --> more output information about solver
187    simulationSettings.timeIntegration.newton.useModifiedNewton = False
188    simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1
189    simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5
190    simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False
191    simulationSettings.timeIntegration.simulateInRealtime = True    # crucial for operating with robot
192    simulationSettings.displayStatistics = True
193    simulationSettings.solutionSettings.solutionInformation = "3D Spring Damper"
194    simulationSettings.solutionSettings.writeSolutionToFile = False
195    viewMatrix = np.eye(3)  @ RotationMatrixZ(np.pi/2)@ RotationMatrixX(np.pi/2)
196    SC.visualizationSettings.general.autoFitScene = False
197    # set up parameter for usage in WSL2 (Ubuntu 20.04) on Windows 10
198    SC.visualizationSettings.window.startupTimeout = 5000
199    SC.visualizationSettings.interactive.selectionLeftMouse=False
200    SC.visualizationSettings.interactive.selectionRightMouse=False
201
202    exu.StartRenderer(True)
203    exu.SolveDynamic(mbs, simulationSettings)
204
205    return True
206
207# __main__ function for testing the interface
208if __name__ == "__main__":
209    try:
210        main()
211    except rospy.ROSInterruptException:
212        pass