ROSMobileManipulator.py
You can view and download this file on Github: ROSMobileManipulator.py
1#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2# This is an EXUDYN python example
3#
4# Details: This example shows how to communicate between an Exudyn simulation and ROS
5# To make use of this example, you need to
6# install ROS (ROS1 noetic) including rospy (see rosInterface.py)
7# prerequisite to use:
8# use a bash terminal to start the roscore with:
9# roscore
10# then run the simulation:
11# python 3 ROSMobileManipulator.py
12# You can use the prepared ROS node, ROSControlMobileManipulator to control the simulation
13# use a bash terminal to start the recommended file
14# (it is in python/Examples/testData/ROS/):
15# python3 ROSControlMobileManipulator.py
16# For even more ROS functionality create a ROS package (e.q. myExudynInterface) in a catkin workspace,
17# copy files ROSMobileManipulator.py, ROSbodykairos.stl and ROSControlMobileManipulator.py
18# (the latter two are in python/Examples/testData/ROS/) into the corresponding folders of the package
19# For more functionality see also: ROSMassPoint.py, ROSBringupTurtle.launch, ROSControlTurtleVelocity.py from the EXUDYN examples folder
20#
21#
22# STATUS (2026-09-18): this example was written and TESTED IN 2023 against ROS1 noetic and has
23# not been run since - the maintainer has no ROS installation to test it with, and it
24# is excluded from the automated example run for that reason. Take it as a WORKING
25# ILLUSTRATION of how the exudyn/ROS coupling is put together, not as something that
26# runs unchanged: topic names, message types and the node layout have to be adapted
27# to your own installation and ROS version. Re-checking it against a current ROS is
28# issue #2511.
29# The supplementary files - the ROS nodes, the launch file and the STL geometry -
30# are in python/Examples/testData/ROS/.
31# Author: Martin Sereinig, Peter Manzl
32# Date: 2023-05-31 (created)
33# last Update: 2023-09-11
34# Copyright:This file is part of Exudyn. Exudyn is free software.
35# You can redistribute it and/or modify it under the terms of the Exudyn license.
36# See 'LICENSE.txt' for more details.
37#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
38
39# general imports
40import numpy as np
41import roboticstoolbox as rtb
42from spatialmath import SE3
43
44# exudyn imports
45import exudyn as exu
46#a local run must not open a plot window when windows are suppressed (#2477)
47if exu.special.userInterface.suppressPlots: import matplotlib; matplotlib.use('Agg')
48from exudyn.utilities import * #includes itemInterface and rigidBodyUtilities
49import exudyn.graphics as graphics
50from exudyn.itemInterface import *
51from exudyn.rigidBodyUtilities import *
52from exudyn.robotics import *
53from exudyn.robotics.models import ManipulatorUR5, LinkDict2Robot
54from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP
55import exudyn.robotics.rosInterface as exuROS #exudyn ROS interface class
56import exudyn.robotics.mobile as mobile
57
58# ROS imports
59from geometry_msgs.msg import Twist
60from geometry_msgs.msg import Pose
61from std_msgs.msg import String
62
63
64# here build inherited class and using within a simple exudyn simulation of one mass spring-damper
65class MyExudynROSInterface(exuROS.ROSInterface):
66 def __init__(self):
67 # use initialisation of super class
68 # this initialize a rosnode with name
69 super().__init__(name='ROSMobileManipulator')
70 # initialization of all standard publisher done by super class
71 # use standard publisher functions form super class
72 # initialize all subscriber
73 # suitable callback function is auto generated by superclass (using lambda function)
74 # twist subscriber: cmd_vel
75 twistSubsrciberBase = ''
76 twistSubsrciberTopic = 'cmd_vel' # topic to subscribe
77 self.cmd_vel = Twist() # data type of topic, must be named: self.twistSubscriberTopic
78 self.myTwistSubscriber = self.InitSubscriber(twistSubsrciberBase,twistSubsrciberTopic,Twist)
79 # string subsriber: my_string
80 stringSubsrciberBase = ''
81 stringSubsrciberTopic = 'my_string' # topic to subscribe
82 self.my_string = String() # data type of topic, must be named: self.twistSubscriberTopic
83 self.myStringSubscriber = self.InitSubscriber(stringSubsrciberBase,stringSubsrciberTopic,String)
84 # string subsriber: my_pose
85 poseSubsrciberBase = ''
86 poseSubsrciberTopic = 'my_pose' # topic to subscribe
87 self.my_pose = Pose() # data type of topic, must be named: self.twistSubscriberTopic
88 self.myPoseSubscriber = self.InitSubscriber(poseSubsrciberBase,poseSubsrciberTopic,Pose)
89
90
91debugFlag = False # turn prints on and off
92
93# function to check (exudyn) module version
94def checkInstall(moduleName):
95 import importlib
96 found = importlib.util.find_spec(moduleName) is not None
97 if not(found):
98 print('Error! Please install the module {} Version 1.7 or higher using \npip install {}==1.7'.format(moduleName, moduleName))
99 return False
100 else:
101 return True
102
103# function to control the mobile manipulator behavior
104def functionStateMachine(t, posPlatform, ThetaPlatform, PosObj, armStatus, myState, myDict):
105 # initialize variables
106 v = [0.0,0.0,0]
107 # with None no arm movement is performed
108 TArm = None
109 grasp = None
110 # check robotC control string send via ROS
111 robotControlString = myROSInterface.my_string.data
112 if debugFlag:
113 if robotControlString!='':
114 print('robot control string: ', robotControlString)
115 else:
116 print('no robot control string')
117 if robotControlString == 'ms':
118 TArm = None
119 grasp = None
120 # get velocity data from ROS /cmd_vel topic, please use: rostopic pub -r 100 /cmd_vel geometry_msgs/Twist "..."
121 rosLinearVelo = myROSInterface.cmd_vel.linear
122 rosAngularVelo = myROSInterface.cmd_vel.angular
123 v = [rosLinearVelo.x, rosLinearVelo.y, rosAngularVelo.z]
124 elif robotControlString == 'a':
125 # state for arm movement
126 v = [0.0,0.0,0]
127 ArmPosition = [myROSInterface.my_pose.position.x, myROSInterface.my_pose.position.y, myROSInterface.my_pose.position.z]
128 ArmOrientationQ = [myROSInterface.my_pose.orientation.w, myROSInterface.my_pose.orientation.x, myROSInterface.my_pose.orientation.y, myROSInterface.my_pose.orientation.z]
129 ArmOrientationR = EulerParameters2RotationMatrix(ArmOrientationQ)
130 # build homogenous transformation from rotation matrix existing rotation matrix
131 TArmRot = np.eye(4)
132 TArmRot[0:3,0:3] = ArmOrientationR
133 TArm = exu.HT(translation=ArmPosition) @ TArmRot
134 elif robotControlString == 'mk':
135 # state for external cmd_vel (keyboard or other node)
136 # get velocity data from ROS /cmd_vel topic, please use: rostopic pub -r 100 /cmd_vel geometry_msgs/Twist "..." but send via keyboard node
137 rosLinearVelo = myROSInterface.cmd_vel.linear
138 rosAngularVelo = myROSInterface.cmd_vel.angular
139 v = [rosLinearVelo.x, rosLinearVelo.y, rosAngularVelo.z]
140 else:
141 # print('no valid control string received')
142 v = [0.0, 0.0, 0.0]
143 TArm = None
144 grasp = None
145 return v, TArm, grasp, myState, myDict
146
147# set simulation system
148SC = exu.SystemContainer()
149mbs = SC.AddSystem()
150# function to simulate the mobile manipulator
151def SimulationMobileRobot(funcStatMachine,myROSInterface, p0=[0,0], theta0=0, flagFixObject=False, flagOnlyGrasp=False, verboseMode = 0, sensorWriteToFile = False):
152 compensateStaticTorques = False
153 mobRobSolutionPath = 'solution/'
154 hstepsize = 5e-3 # step size
155 tEnd = 100 # simulation time
156 comShift=[0,0,-0.1]
157 debugPlatformOffset = 125*9.81 /10**(6)
158 constrainedAxesSet=[0,0,0,0,0,0]
159 offsetUserFunctionParametersSet=[0,0,0,0,0,0]
160 debugOffsetNumber= debugPlatformOffset
161
162 #ground body and marker
163 gGround = graphics.CheckerBoard(point = [0,0,0], size=8, nTiles = 12) # (centerPoint=[4,4,-0.001],size=[12,12,0.002], color=graphics.color.lightgrey[0:3]+[0.5])
164 graphicsGroundList =[gGround]
165 coordinateSystemGround = False
166 if coordinateSystemGround:
167 graphicsGroundList += [graphics.Cylinder([0,0,0], [0.5,0,0], 0.0035, graphics.color.red)] # base coordinate system x
168 graphicsGroundList +=[graphics.Cylinder([0,0,0], [0,0.5,0], 0.0035, graphics.color.green)] # base coordinate system y
169 graphicsGroundList +=[graphics.Cylinder([0,0,0], [0,0,0.5], 0.0035, graphics.color.blue)] # base coordinate system z
170 oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=graphicsGroundList)))
171 markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
172 comShiftPlatform = [0,0,0]
173
174 # define mobile manipulator KAIROS
175 mobileRobot = { 'gravity': [0,0,-9.81], # gravity in m/s^2
176 'platformDimensions': [0.575, 0.718 , 0.2495], # [width, length, hight] [0.575, 0.718 , 0.495]
177 'platformMass': 125-18.4, # platform mass- manipulator mass
178 'platformInitialPose': exu.HT(translation=[p0[0],p0[1],(0.495+(-0.12))]) @ exu.HT().SetRotationZ(theta0), # platform initial pose as HT middle of platform (box representation)
179 'platformInitialOmega': [0,0,0], # platform initial rotational velocity around x,y,z axis
180 'platformInitialVelocity': [0,0,0], # platform initial translational velocity in x,y,z direction
181 'platformCOM': comShift, # center of mass shift to base coordinate system
182 'comShiftPlatform': comShiftPlatform, # the shift of the platform alone
183 'platformBaseCoordinate': [0.0 ,0.0 ,0.0], # geometric center in middle of platform
184 'platformInertia': InertiaCuboid, # platform inertia w.r.t. COM!
185 'platformRepresentation': 'box', # 'box' or 'stl' graphical representation of the mobile platform
186 'platformStlFile': 'ROSbodykairos.stl', # path to the used stl file
187 'friction': [1 ,0.0, 0.0], # [dryFriction1, dryFriction2,rollFriction]= [0.4,0.0075,0.05] for LeoBot (Master Thesis Manzl)
188 'viscousFrictionWheel': [0.1, 0.1], # orthotropic damping in the rotated roller frame; see also Exudyn documentation of
189 'frictionAngle': np.pi/4 , # friction angle theta=pi/4 for mecanum wheel, theta=0 for standard wheel
190 'wheelType': 0, # 0=wheelType wheel o-config, 1=mecanum wheel x-config, 2=standard wheel (always in bottom view)
191 'wheelBase': 0.430, # distance between center of wheels (wheel axes) between front and back
192 'wheelTrack': 0.390, # distance between center of wheels between left and right
193 'wheelRoh': 200*8, # density of wheel in kg/m^3
194 'wheelRadius': 0.254/2, # radius of wheel in m
195 'wheelWidth': 0.1, # width of wheel in m, just for graphics
196 'wheelMass': 8, # Mass of one mecanum wheel, leobot measured
197 'wheelInertia': InertiaCylinder, # inertia for infinitely small ring:
198 'wheelNumbers': 4, # number of wheels on platform
199 'wheelContactStiffness': 10**(6),
200 'wheelContactDamping': 50*np.sqrt(10**(5)),
201 'serialRobotMountpoint': exu.HT(translation=[0.178 , 0, 0.12]),
202 'proportionalZone': 1e-2, # friction regularization
203 'debugOffset': debugOffsetNumber
204 }
205
206 #################### Build mobile robot and add it to existing mbs
207 mobileRobotBackDic = mobile.MobileRobot2MBS(mbs, mobileRobot, markerGround)
208 mbs.variables['mobileRobotBackDic'] = mobileRobotBackDic # to be able to use all variables in all functions (make it global useable)
209 # add mbs.variable for ROS sensor
210 mbs.variables['nodeNumber'] = mobileRobotBackDic['nPlatformList'][0] # just needed if nodeNumber is used for sensor information
211 # for shorter writing:
212 Lx = mobileRobot['wheelTrack']
213 Ly = mobileRobot['wheelBase']
214 R = mobileRobot['wheelRadius']
215 #initialize mobile platform kinematics
216 platformKinematics = mobile.MobileKinematics(R,Lx,Ly,wheeltype=0)
217
218 def UFoffset(mbs,t,itemIndex,offsetUserFunctionParameters):
219 return offsetUserFunctionParameters
220 mbs.SetObjectParameter(mobileRobotBackDic['oPlatformList'][0],'offsetUserFunctionParameters',offsetUserFunctionParametersSet)
221 mbs.SetObjectParameter(mobileRobotBackDic['oPlatformList'][0],'offsetUserFunction',UFoffset)
222 mbs.SetObjectParameter(mobileRobotBackDic['oPlatformList'][0],'constrainedAxes',constrainedAxesSet)
223 ######################## Sensor data from mobile platform ###
224 WheelSpringDamper = [0]*4
225 MotorDataNode = [0]*4
226 cWheelBrakes = [0]*4
227 for i in range(4):
228 #the markers of the revolute joint carry the frame of the axle (localHT), the motor and brake use them as well
229 mAxleWheel = mbs.GetObject(mobileRobotBackDic['oAxlesList'][i])['markerNumbers']
230
231 # wheel controller for KAIROS Platform
232 paramOpt = {'kMotor': 100, 'fact_dMotor': 0.5}
233 kWheelControl = paramOpt['kMotor']
234 dWheelControl = kWheelControl * paramOpt['fact_dMotor']
235 MotorDataNode[i] = mbs.AddNode(NodeGenericData(numberOfDataCoordinates = 1, initialCoordinates=[0]))
236 WheelSpringDamper[i] = mbs.AddObject(TorsionalSpringDamper(name='Wheel{}Motor'.format(i),
237 markerNumbers=mAxleWheel,
238 nodeNumber= MotorDataNode[i], # for continuous Rotation
239 stiffness = kWheelControl, damping = dWheelControl, offset = 0))
240 cWheelBrakes[i] = mbs.AddObject(GenericJoint(markerNumbers=mAxleWheel,
241 constrainedAxes = [0]*6,
242 ))
243 mbs.variables['flagBrakeActive'] = False
244
245 # wheel user function
246 mbs.variables['signWheels'] = [-1,1,1,-1]
247 mbs.variables['t0'] = 0
248 mbs.variables['phiWheel'] = [0,0,0,0]
249 vMax = 3.0
250 wMax = vMax / mobileRobot['wheelRadius'] # m/s
251 mbs.variables['wHistory'] = [[],[],[],[]] # for debug
252 def PreStepUFWheel(mbs, t, w= [0,0,0,0]):
253 if t == 0:
254 return True
255 dt = mbs.sys['dynamicSolver'].it.currentStepSize
256 dwMax = wMax * dt
257
258 if debugFlag: print('dwMax = ', dwMax)
259
260 for i in range(4):
261 wOld = mbs.GetObjectParameter(WheelSpringDamper[i], 'velocityOffset')
262 if w[i] > wOld + dwMax:
263 w[i] = wOld + dwMax
264 elif w[i] < wOld - dwMax:
265 w[i] = wOld - dwMax
266 mbs.variables['phiWheel'][i] += (t-mbs.variables['t0'])*w[i] #* mbs.variables['signWheels'][i]
267 mbs.SetObjectParameter(WheelSpringDamper[i], 'offset', mbs.variables['phiWheel'][i])
268 mbs.SetObjectParameter(WheelSpringDamper[i], 'velocityOffset', w[i])
269 mbs.variables['wHistory'][i] += [w[i]]
270 # mbs.SetObjectParameter(WheelSpringDamper[i], 'offset', SmoothStep(t, 0.5 , 2, 0, 1)*t * mbs.variables['signWheels'][i])
271 mbs.variables['t0'] = t
272 return True
273 sPlatformPosition = mbs.AddSensor(SensorMarker(name='platformpos',markerNumber=mobileRobotBackDic['mPlatformList'][-1],
274 fileName=mobRobSolutionPath + '/rollingDiscCarPos.txt',
275 outputVariableType = exu.OutputVariableType.Position, writeToFile = sensorWriteToFile,storeInternal=True))
276 sPlatformVelocity = mbs.AddSensor(SensorMarker(name='platformvelo',markerNumber=mobileRobotBackDic['mPlatformList'][-1],
277 fileName=mobRobSolutionPath + '/rollingDiscCarVel.txt',
278 outputVariableType = exu.OutputVariableType.Velocity, writeToFile = sensorWriteToFile,storeInternal=True))
279 sPlatformOrientation = mbs.AddSensor(SensorBody(name='platformRot',bodyNumber=mobileRobotBackDic['bPlatformList'][0],
280 fileName=mobRobSolutionPath + '/rollingDiscCarOrientation.txt',
281 outputVariableType = exu.OutputVariableType.Rotation , writeToFile = sensorWriteToFile))
282 mbs.variables['sensorList'] = [sPlatformPosition, sPlatformOrientation]
283 # ad manipulator to model
284 if 1:
285 mode='newDH'
286 qOffset = [-np.pi * 1/4, 0,0,0,0,0]
287 q0 = [-3*np.pi/4, np.pi - 1e-15 , np.pi*1.5/2 ,0- 1e-15 ,0- 1e-15 ,0- 1e-15 ] #zero angle configuration
288 tx = 0.03
289 zOff = -0.2
290 toolSize= [tx*0.5, 0.06,0.12]
291 r6 = 0.04
292 graphicsToolList = []
293 graphicsToolList += [graphics.Cylinder(pAxis=[0,0,zOff], vAxis= [0,0,np.abs(zOff)*0.8], radius=r6, color=graphics.color.red)]
294 graphicsToolList+= [graphics.Brick([ tx,0, 0], toolSize, graphics.color.grey)]
295 graphicsToolList+= [graphics.Brick([-tx,0, 0], toolSize, graphics.color.grey)]
296 graphicsToolList+= [graphics.Brick([0,0, -0.05], [tx*5,0.09,0.04], graphics.color.grey)]
297 graphicsToolList += [graphics.Basis(length=0.2)]
298 basePoseHT=mobileRobot['platformInitialPose'] @ mobileRobot['serialRobotMountpoint'] @ exu.HT().SetRotationZ(qOffset[0]) #robot base position and orientation
299
300 # manipulator input with included function from exudyn robotics models
301 myRobotList = ManipulatorUR5()
302 robot = Robot(gravity=[0,0,-9.81],
303 base = RobotBase(HT=basePoseHT), #visualization=VRobotBase(graphicsData=graphicsBaseList)),
304 tool = RobotTool(HT=exu.HT(translation=[0,0,0.155]), # @ HTrotateZ(np.pi/2),
305 visualization=VRobotTool(graphicsData=graphicsToolList)),
306 referenceConfiguration = q0) #referenceConfiguration created with 0s automatically
307 robot = LinkDict2Robot(myRobotList, robotClass=robot)
308 #control parameters, per joint:
309 fc=0.5
310 Pcontrol = np.array([4000, 4000, 4000, 100, 100, 100])
311 Dcontrol = np.array([60, 60, 60, 6, 6, 0.6])
312 Pcontrol = fc*Pcontrol
313 Dcontrol = fc*Dcontrol
314
315 # change predefined control parameters
316 for i in range(robot.NumberOfLinks()):
317 robot.links[i].PDcontrol = (Pcontrol[i], Dcontrol[i])
318 #trajectory generated with optimal acceleration profiles:
319 trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
320 trajectory.Add(ProfileConstantAcceleration(q0,0.5))
321 jointList = [0]*robot.NumberOfLinks() #this list must be filled afterwards with the joint numbers in the mbs!
322 def ComputeMBSstaticRobotTorques(robot):
323 q=[]
324 for joint in jointList:
325 q += [mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2]] #z-rotation
326 HT=robot.JointHT(np.array(q)+qOffset)
327 return robot.StaticTorques(HT)
328
329 #++++++++++++++++++++++++++++++++++++++++++++++++
330 #base, graphics, object and marker:
331 #baseMarker; could also be a moving base according to doc but found no examples!
332 baseMarker = mobileRobotBackDic['mPlatformList'][-1] # mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
333 sArmTorque = [0,0,0,0,0,0]
334
335 #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
336 #build mbs robot model:
337
338 robotDict = robot.CreateRedundantCoordinateMBS(mbs, baseMarker=baseMarker)
339 jointList = robotDict['jointList'] #must be stored there for the load user function
340 torsionalSDlist = robotDict['springDamperList']
341
342 for i in range(len(robotDict['springDamperList'])):
343 sArmTorque[i] = mbs.AddSensor(SensorObject(objectNumber= robotDict['springDamperList'][i],
344 fileName=mobRobSolutionPath+ '/ArmMotorTorque'+str(i) + '.txt',
345 outputVariableType = exu.OutputVariableType.TorqueLocal, writeToFile=sensorWriteToFile))
346 # mbs.SetObjectParameter(torsionalSDlist[i], 'offset', q0[i] + qOffset[i])
347 mEndeffektor = mbs.AddMarker(MarkerBodyRigid(name='Endeffektor', bodyNumber=robotDict['bodyList'][-1], localPosition=[0,0,0.157]))
348 sEndeffektor = mbs.AddSensor(SensorMarker(markerNumber=mEndeffektor, writeToFile=False, outputVariableType=exu.OutputVariableType.Position))
349 # robotics toolbox by Peter Corke
350 UR5_rtb = rtb.models.DH.UR5()
351 UR5_rtb.tool = SE3([0,0,0.2])
352 def getNewTraj(TArm, qLast):
353 vm = [np.pi * 1.1/2]*6
354 am = [5 * 0.6]*6
355 nAttempts = 10
356 qNew = UR5_rtb.ikine_LM(TArm, q0 = qLast) # , q0=qLast-qOffset)
357 iAttempt = 1
358 if not(qNew.success):
359 for iAttempt in range(1, nAttempts):
360 # randomize initial angles to try to get a solution for the inverse kinematics
361 qNew = UR5_rtb.ikine_LM(TArm, q0 = (np.random.random(6)-1)*np.pi*2) # , q0=qLast-qOffset)
362 if qNew.success:
363 break
364 if not(qNew.success):
365 print('Inverse Kinematics could not be solved after {} attempts. \nPlease check if the given Pose \n{}\nis in the workspace.'.format(iAttempt, TArm))
366 return None, None
367 teMax = 0
368 qNew = qNew.q # - qOffset
369 for i in range(len(qNew)):
370 te = abs(qNew[i] - qLast[i])/vm[i] + vm[i]/am[i]
371 if te > teMax:
372 teMax = te
373 print('Planned new PTP motion after {} attempts from:\n{}\nto\n{}\nin {}s. '.format(iAttempt, np.round(qLast, 2), np.round(qNew, 2), np.round(teMax, 2)))
374 # qNew[1] += np.pi*2
375 return qNew, teMax
376
377 def activateBrakes(cJoints, oTSD, flag):
378 for i in range(len(cJoints)):
379 # mbs.SetObjectParameter(cJoints[i], 'constrainedAxes', [0,0,0,0,0,1*bool(flag)])
380 if flag: # deactivate motors
381 mbs.SetObjectParameter(oTSD[i], 'stiffness', 100)
382 mbs.SetObjectParameter(oTSD[i], 'damping', 5)
383 else:
384 mbs.SetObjectParameter(oTSD[i], 'stiffness', kWheelControl)
385 mbs.SetObjectParameter(oTSD[i], 'damping', dWheelControl)
386
387 print('un'*bool(not(flag)) + 'locking wheels')
388 return True
389
390 hObj = 0.08
391 xTable, yTable, hTable = 0.2, 0.4, 0.645
392 if flagFixObject:
393 PosObj = [3.0,1.0, hTable + hObj/2] # for testing of grasp
394 else:
395 PosObj = (np.random.rand(3)-0.5) * [0.4,0.8,0] + [1.5 ,0, hTable + hObj/2]
396
397 graphicsTarget = graphics.Cylinder(pAxis = [0,0,-hObj/2], vAxis = [0,0,hObj ], radius = 0.02, color=graphics.color.lightgreen)
398 inertiaTarget = InertiaCylinder(500, hObj , 0.02, 2)
399 # old interface to rigid body:
400 # nObj, bObj = AddRigidBody (mainSys = mbs,
401 # inertia = inertiaTarget,
402 # nodeType = str(exu.NodeType.RotationEulerParameters),
403 # position = PosObj,
404 # rotationMatrix = np.eye(3),
405 # angularVelocity = [0,0,0],
406 # velocity= [0,0,0],
407 # gravity = [0,0,0],
408 # graphicsDataList = [graphicsTarget])
409 dictObj = mbs.CreateRigidBody(
410 inertia=inertiaTarget,
411 referencePosition=PosObj,
412 referenceRotationMatrix=np.eye(3),
413 gravity=[0, 0, 0],
414 graphicsDataList=[graphicsTarget],
415 returnDict=True)
416 [nObj, bObj] = [dictObj['nodeNumber'], dictObj['bodyNumber']]
417
418 mObj = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bObj))
419 cGrasp = mbs.AddObject(RigidBodySpringDamper(markerNumbers=[mEndeffektor, mObj], stiffness = np.eye(6)*1e3, damping = np.eye(6)*1e2,
420 visualization={'show': False, 'drawSize': -1, 'color': [0]*4}, activeConnector=False))
421 graphicsTable = graphics.Brick(centerPoint = [0,0,0], size = [xTable, yTable, hTable], color=graphics.color.darkgrey2)
422
423 # nTable, bTable = AddRigidBody (mainSys = mbs,
424 # inertia = inertiaTarget,
425 # nodeType = str(exu.NodeType.RotationEulerParameters),
426 # position = list(PosObj[0:2]) + [hTable/2],
427 # rotationMatrix = np.eye(3),
428 # angularVelocity = [0,0,0],
429 # velocity= [0,0,0],
430 # gravity = [0,0,0],
431 # graphicsDataList = [graphicsTable])
432 dictTable = mbs.CreateRigidBody(
433 inertia=inertiaTarget,
434 referencePosition=list(PosObj[0:2]) + [hTable/2],
435 referenceRotationMatrix=np.eye(3),
436 gravity=[0, 0, 0],
437 graphicsDataList=[graphicsTable],
438 returnDict=True)
439 [nTable, bTable] = [dictTable['nodeNumber'], dictTable['bodyNumber']]
440
441
442 mbs.variables['myDict'] = {}
443
444 #prestep user functions
445 for i in range(6):
446 mbs.variables['qDebug{}'.format(i)] = []
447 mbs.variables['state'] = 0
448 mbs.variables['trajectory'] = trajectory
449 #user function which is called only once per step, speeds up simulation drastically
450
451 def PreStepUF(mbs, t):
452 if compensateStaticTorques:
453 staticTorques = ComputeMBSstaticRobotTorques(robot)
454 else:
455 staticTorques = np.zeros(len(jointList))
456
457 PosPlatform = mbs.GetSensorValues(mbs.variables['sensorList'][0]) - [0.178, 0 , 0 ]#mbs.variables['sensorRecord{}'.format(mbs.variables['sensorRecorders'][0])]
458 ThetaPlatform = mbs.GetSensorValues(mbs.variables['sensorList'][1])[-1]
459
460 phi = np.zeros(len(robot.links))
461 for i in range(len(robot.links)):
462 phi[i] = mbs.GetObjectOutput(jointList[i], exu.OutputVariableType.Rotation)[2] #z-rotation
463
464 if t > mbs.variables['trajectory'][-1]['finalTime']:
465 armStatus = 1 # current trajectory finished
466 else:
467 armStatus = 0
468
469 # here functionstatemachine in preStepUserFunction call
470 vel, TArm, grasp, mbs.variables['state'], mbs.variables['myDict'] = funcStatMachine(t, PosPlatform, ThetaPlatform,
471 PosObj, armStatus, mbs.variables['state'], mbs.variables['myDict'])
472 if type(TArm) == np.ndarray:
473 TArm = SE3(TArm)
474 if mbs.variables['state'] == -1:
475 mbs.SetRenderEngineStopFlag(True)
476 print('finished Statemachine. ')
477
478 # platform kinematics calculation
479 w = platformKinematics.GetWheelVelocities(vel)
480
481 if TArm != None:
482 lastTraj = mbs.variables['trajectory'][-1]
483 qLast = lastTraj['coordinateSets'][-1] # the last desired joint angles
484 tLast = lastTraj['finalTime'] #
485 qNew, Ttraj = getNewTraj(TArm, qLast)
486 if type(qNew) != type(None):
487 mbs.variables['trajectory'].Add(ProfileConstantAcceleration(qLast,t-tLast+0.1))
488 mbs.variables['trajectory'].Add(ProfileConstantAcceleration(qNew,Ttraj))
489 # print('\n\nTTraj is: \n{}\n\n'.format(np.round(Tj, 3)))
490
491 if grasp:
492 pEE = mbs.GetSensorValues(sEndeffektor)
493 distanceGrasp = pEE - PosObj - [0,0,0]
494 # print('distance grasp = ', distanceGrasp)
495 if np.linalg.norm(distanceGrasp) < 0.1: # distance of grasp to
496 print('grasp successful!')
497 # activate constraint for grasp
498 RotEE = mbs.GetNodeOutput(robotDict['nodeList'][-1], exu.OutputVariableType.RotationMatrix).reshape([3,3])
499 mbs.SetObjectParameter(cGrasp, 'rotationMarker1', RotEE)
500 # mbs.SetObjectParameter(cGrasp, '', RotEE) # position
501
502 # mbs.SetObjectParameter(cGrasp, 'constrainedAxes', [1]*6)
503 # mbs.SetObjectParameter(cGrasp, '', [1]*6)
504 mbs.SetObjectParameter(cGrasp, 'activeConnector', True)
505 offset_local = list(RotEE @ distanceGrasp)
506 offset_local[1] = 0
507 mbs.SetObjectParameter(cGrasp, 'offset', offset_local + [0,0,0]) # list(RotEE @ distanceGrasp) + [0,0,-hObj*0])
508 else:
509 print('grasping the object was not successful. Calculated distacne = {}'.format(np.round(distanceGrasp, 3)))
510 [u,v,a] = mbs.variables['trajectory'].Evaluate(t)
511 for i in range(len(robot.links)):
512 tsd = torsionalSDlist[i]
513 mbs.SetObjectParameter(tsd, 'offset', u[i] + qOffset[i])
514 mbs.SetObjectParameter(tsd, 'velocityOffset', v[i])
515 mbs.SetObjectParameter(tsd, 'torque', staticTorques[i]) #additional torque from given velocity
516 # send velocity data to ROS
517 myROSInterface.PublishTwistUpdate(mbs,t)
518 # send position data to ROS
519 myROSInterface.PublishPoseUpdate(mbs,t)
520 PreStepUFWheel(mbs, t, w)
521
522 if np.linalg.norm(w) < 1e-5 and t > 0.5:
523 if not(mbs.variables['flagBrakeActive']):
524 activateBrakes(cWheelBrakes, WheelSpringDamper, True)
525 mbs.variables['flagBrakeActive'] = True
526 else:
527 if mbs.variables['flagBrakeActive']:
528 activateBrakes(cWheelBrakes, WheelSpringDamper, False)
529 mbs.variables['flagBrakeActive'] = False
530
531 return True
532
533 mbs.SetPreStepUserFunction(PreStepUF)
534 SC.visualizationSettings.view0.camera.trackMarker = mobileRobotBackDic['mPlatformList'][0]
535 # start simulation:
536 mbs.Assemble()
537 PreStepUF(mbs, 0)
538
539 SC.visualizationSettings.connectors.showJointAxes = True
540 SC.visualizationSettings.connectors.jointAxesLength = 0.02
541 SC.visualizationSettings.connectors.jointAxesRadius = 0.002
542 SC.visualizationSettings.nodes.showBasis = True
543 SC.visualizationSettings.nodes.basisSize = 0.1
544 SC.visualizationSettings.loads.show = False
545 SC.visualizationSettings.openGL.multiSampling=4
546 SC.visualizationSettings.openGL.light0.shadow = 0.5
547 SC.visualizationSettings.openGL.light0.position = [0, -2, 10, 0]
548 SC.visualizationSettings.openGL.light0.shadowPolygonOffset = 0.1
549 #SC.renderer.DoIdleTasks()
550
551 simulationSettings = exu.SimulationSettings() #takes currently set values or default values
552 simulationSettings.timeIntegration.numberOfSteps = int(tEnd/hstepsize)
553 simulationSettings.timeIntegration.endTime = tEnd
554 simulationSettings.solution.file.writePeriod = hstepsize #0.005
555 simulationSettings.solution.sensors.writePeriod = hstepsize # 0.005
556 simulationSettings.solution.file.binary = False
557 simulationSettings.solution.file.write = False
558
559 simulationSettings.timeIntegration.realtime.active = True
560 #simulationSettings.timeIntegration.realtime.factor = 0.25
561 simulationSettings.timeIntegration.verboseMode = verboseMode
562 #simulationSettings.timeIntegration.newton.useModifiedNewton = True
563 #simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
564 #simulationSettings.timeIntegration.generalizedAlpha.useNewmark = simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints
565 simulationSettings.timeIntegration.newton.useModifiedNewton = True
566 #simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 # 0.25
567 simulationSettings.timeIntegration.discontinuous.maxIterations = 3
568 simulationSettings.timeIntegration.adaptiveStepRecoveryIterations = 10
569 simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations= True # False
570
571 simulationSettings.show.computationTime = True
572 simulationSettings.show.statistics = True
573 #simulationSettings.linearSolver.solverType = exu.LinearSolverType.EigenSpars
574
575
576 SC.visualizationSettings.general.autoFitScene=False
577 SC.visualizationSettings.general.renderWindowString = 'Mobile Robot Simulation'
578 SC.visualizationSettings.view0.window.renderWindowSize=[1920,1200]
579 SC.visualizationSettings.general.rendererStartupTimeout=5000
580 SC.visualizationSettings.interactive.advanced.selectionLeftMouse = False
581 SC.visualizationSettings.interactive.advanced.selectionRightMouse = False
582
583 SC.visualizationSettings.openGL.advanced.initialModelRotation =RotationMatrixZ(-0.2) @ RotationMatrixX(np.pi/2.7) #
584 SC.visualizationSettings.openGL.advanced.initialZoom = 1.5
585 SC.visualizationSettings.openGL.advanced.initialCenterPoint = [0, 2, 0] # -1.7, -2, -2]
586 # SC.visualizationSettings.contour.outputVariable = exu.OutputVariableType.Displacement
587 # SC.visualizationSettings.contour.outputVariableComponent = 2 #0=x, 1=y, 2=z
588 SC.renderer.Start()
589 SC.renderer.DoIdleTasks()
590 mbs.SolveDynamic(simulationSettings, showHints=True, storeSolver = True)
591 #mbs.WaitForRenderEngineStopFlag()
592 SC.renderer.Stop()
593
594 # for debug
595 if debugFlag:
596 import matplotlib.pyplot as plt
597 for i in range(4):
598 plt.plot(mbs.variables['wHistory'][i], label='wheel ' + str(i+1))
599 plt.legend()
600 plt.show()
601 mbs.PlotSensor(sensorNumbers=[sPlatformPosition], components=[0,1,2], labels=['x(m); ','y','z'], colorCodeOffset=2, closeAll=False)
602 mbs.PlotSensor(sensorNumbers=[sPlatformVelocity], components=[0,1,2], labels=['vx(m); ','vy','vz'], colorCodeOffset=2, closeAll=False)
603 return mbs
604
605# main function
606if __name__ == '__main__':
607 if not(checkInstall('exudyn')):
608 print('Error! Simulation can not be started!')
609 import sys
610 sys.exit()
611
612 print('Starting Simulation...')
613 # initialize ROS interface from own subclass
614 myROSInterface = MyExudynROSInterface()
615 # start simulation
616 SimulationMobileRobot(functionStateMachine,myROSInterface,p0=[0,0],flagFixObject=True)