.. _examples-rosmasspoint: *************** ROSMassPoint.py *************** You can view and download this file on Github: `ROSMassPoint.py `_ .. code-block:: python :linenos: #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ # This is an EXUDYN python example how to use ROS and EXUDYN # # Details: This example shows how to communicate between an exudyn simulation # and ROS publisher and subscriber from bash # To make use of this example, you need to # install ROS (ROS1 noetic) including rospy (see rosInterface.py) # prerequisits to use: # use a bash terminal to start the roscore with: # roscore # send force command to add load to the mass point form bash file with: # rostopic pub -r 100 /cmd_wrench geometry_msgs/WrenchStamped "..." # Author: Martin Sereinig, Peter Manzl # Date: 2023-05-31 (created) # # Copyright:This file is part of Exudyn. Exudyn is free software. # You can redistribute it and/or modify it under the terms of the Exudyn license. # See 'LICENSE.txt' for more details. # #+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ import numpy as np import exudyn as exu from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities import exudyn.graphics as graphics #only import if it does not conflict # import needed ROS modules and messages import rospy from geometry_msgs.msg import WrenchStamped, Twist from std_msgs.msg import String # import new exudyn ROS interface class import rosInterface as exuROS # here build inherited class and using within a simple exudyn simulation of one mass spring-damper class MyExudynROSInterface(exuROS.ROSInterface): def __init__(self): # use initialisation of super class # this initialize a rosnode with name super().__init__(name='exuROSexample3Dmass') # initialization of all standard publisher done by super class # self.exuPosePublisher # self.exuStringPublisher # self.exuSystemstatePublisher # self.exuTimePublisher # self.exuTwistPublisher # use standard publisher functions form super class! # self.PublishPoseUpdate # self.PublishTwistUpdate # self.PublishSystemStateUpdate # initialize all subscriber # suitable callback function is auto generated by superclass (using lambda function) # twist subscriber: cmd_vel twistSubsrciberBase = '' twistSubsrciberTopic = 'cmd_vel' # topic to subscribe self.cmd_vel = Twist() # data type of topic, must be named: self.twistSubscriberTopic self.myTwistSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,Twist) # wrench subscriber: cmd_wrench twistSubsrciberBase = '' twistSubsrciberTopic = 'cmd_wrench' # topic to subscribe self.cmd_wrench = WrenchStamped() # data type of topic, must be named: self.twistSubscriberTopic self.myWrenchSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,WrenchStamped) # string subsriber: my_string stringSubsrciberBase = '' stringSubsrciberTopic = 'my_string' # topic to subscribe self.my_string = String() # data type of topic, must be named: self.twistSubscriberTopic self.myStringSubscriber = self.InitSubscriber(stringSubsrciberBase,stringSubsrciberTopic,String) # string subsriber: /exudyn/SimpleString stringSubsrciberBase2 = 'exudyn/' # namespace of topic stringSubsrciberTopic2 = 'SimpleString' # topic to subscribe self.SimpleString = String() # data type of topic, must be named: self.twistSubscriberTopic self.myStringSubscriber = self.InitSubscriber(stringSubsrciberBase2,stringSubsrciberTopic2,String) # test main function def main(): # build exudyn model SC = exu.SystemContainer() mbs = SC.AddSystem() tRes = 0.001 # step size in s tEnd = 1e5 # simulation time in s # mass and dimension of sphere mass = 6 r = 0.03 background = graphics.CheckerBoard(point=[-0.5,0,0], normal=[1, 0, 0], color=[0.7]*3+[1], alternatingColor=[0.8]*3+[1]) graphicsSphere = graphics.Sphere(point=[0,0,0], radius=r, color=(1,0,0,1), nTiles=64) origin = [0, 0, 0] bGround = mbs.AddObject(ObjectGround(referencePosition=origin, visualization=VObjectGround(graphicsData=[background]))) inertiaSphere = InertiaSphere(mass=mass,radius=r) # user interaction point # old rigid body call: # [nUIP, bUIP]=AddRigidBody (mainSys = mbs, # inertia = inertiaSphere, # nodeType = str(exu.NodeType.RotationEulerParameters), # position = [origin[0], origin[1], origin[2]], # rotationMatrix = np.eye(3), # angularVelocity = np.zeros(3), # velocity= [0,0,0], # gravity = [0, 0, 0], # graphicsDataList = [graphicsSphere]) dictUIP = mbs.CreateRigidBody( inertia=inertiaSphere, referencePosition=[origin[0], origin[1], origin[2]], referenceRotationMatrix=np.eye(3), gravity=[0, 0, 0], graphicsDataList=[graphicsSphere], returnDict=True) [nUIP, bUIP] = [dictUIP['nodeNumber'], dictUIP['bodyNumber']] # create markers: mGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bGround, localPosition=[0, 0, 0.])) mUIP = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bUIP)) mUIPLoad = mbs.AddLoad(LoadForceVector(markerNumber = mUIP,loadVector =[0,0,0])) k = 100 oSpringDamper = mbs.AddObject(RigidBodySpringDamper(markerNumbers=[mGround, mUIP], stiffness=np.eye(6)*k, damping=np.eye(6)*k*5e-2, visualization={'show': False, 'drawSize': -1, 'color': [-1]*4})) # sensor for position of endpoint of pendulum sensorPos = mbs.AddSensor(SensorBody(bodyNumber=bUIP, outputVariableType=exu.OutputVariableType.Position)) sensorOri = mbs.AddSensor(SensorBody(bodyNumber=bUIP, outputVariableType=exu.OutputVariableType.Rotation)) sensorVelt = mbs.AddSensor(SensorBody(bodyNumber=bUIP, outputVariableType=exu.OutputVariableType.Velocity)) sensorVelr = mbs.AddSensor(SensorBody(bodyNumber=bUIP, outputVariableType=exu.OutputVariableType.AngularVelocity)) # store sensor value of each step in mbs variable, so that is accessible from user function mbs.variables['pos'] = sensorPos mbs.variables['ori'] = sensorOri mbs.variables['velt'] = sensorVelt mbs.variables['velr'] = sensorVelr # initialize ROS interface from own subclass myROSInterface = MyExudynROSInterface() print('rosversion: ' + str(myROSInterface.myROSversionEnvInt)) rospy.logdebug('node running and publishing') # exudyn PreStepUserFunction def PreStepUserFunction(mbs, t): # send position data to ROS myROSInterface.PublishPoseUpdate(mbs,t) # send velocity data to ROS myROSInterface.PublishTwistUpdate(mbs,t) # send system state data to ROS myROSInterface.PublishSystemStateUpdate(mbs,t) # get string data from ROS /my_string topic, please use: rostopic pub -r 100 /my_string geometry_msgs/WrenchStamped "..." someMessage = myROSInterface.my_string.data if someMessage != '' : print('mystringdata',someMessage) # get wrench data from ROS /cmd_wrench topic, please use: rostopic pub -r 100 /cmd_wrench geometry_msgs/WrenchStamped "..." rosForces = myROSInterface.cmd_wrench.wrench.force rosTorques = myROSInterface.cmd_wrench.wrench.torque print('forces from ROS:', rosForces) print('torques from ROS : ', rosTorques) # demo set force to certain value received from ROS /cmd_wrench newForce = [rosForces.x, rosForces.y, rosForces.z] mbs.SetLoadParameter(mUIPLoad,'loadVector',newForce) return True mbs.SetPreStepUserFunction(PreStepUserFunction) # assemble multi body system with all previous specified properties and components mbs.Assemble() # set simulation settings simulationSettings = exu.SimulationSettings() #takes currently set values or default values simulationSettings.timeIntegration.endTime = tEnd simulationSettings.timeIntegration.numberOfSteps = int(tEnd/tRes) simulationSettings.timeIntegration.newton.relativeTolerance = 1e-8*100 simulationSettings.timeIntegration.newton.absoluteTolerance = 1e-10 simulationSettings.timeIntegration.verboseMode = 1 # if 0 no output; higher --> more output information about solver simulationSettings.timeIntegration.newton.useModifiedNewton = False simulationSettings.timeIntegration.newton.numericalDifferentiation.minimumCoordinateSize = 1 simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations = False simulationSettings.timeIntegration.simulateInRealtime = True # crucial for operating with robot simulationSettings.displayStatistics = True simulationSettings.solutionSettings.solutionInformation = "3D Spring Damper" simulationSettings.solutionSettings.writeSolutionToFile = False viewMatrix = np.eye(3) @ RotationMatrixZ(np.pi/2)@ RotationMatrixX(np.pi/2) SC.visualizationSettings.general.autoFitScene = False # set up parameter for usage in WSL2 (Ubuntu 20.04) on Windows 10 SC.visualizationSettings.window.startupTimeout = 5000 SC.visualizationSettings.interactive.selectionLeftMouse=False SC.visualizationSettings.interactive.selectionRightMouse=False exu.StartRenderer(True) exu.SolveDynamic(mbs, simulationSettings) return True # __main__ function for testing the interface if __name__ == "__main__": try: main() except rospy.ROSInterruptException: pass