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