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