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