You can view and download this file on Github: ROSMobileManipulator.py
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This is an EXUDYN python example
#
# Details: This example shows how to communicate between an Exudyn simulation and ROS
# To make use of this example, you need to
# install ROS (ROS1 noetic) including rospy (see rosInterface.py)
# prerequisite to use:
# use a bash terminal to start the roscore with:
# roscore
# then run the simulation:
# python 3 ROSMobileManipulator.py
# You can use the prepared ROS node, ROSControlMobileManipulator to control the simulation
# use a bash terminal to start the recommended file (see folder Examples/supplementary):
# python3 ROSControlMobileManipulator.py
# For even more ROS functionality create a ROS package (e.q. myExudynInterface) in a catkin workspace,
# copy files ROSMobileManipulator.py, ROSbodykairos.stl and ROSControlMobileManipulator.py in corresponding folders within the package
# For more functionality see also: ROSMassPoint.py, ROSBringupTurtle.launch, ROSControlTurtleVelocity.py from the EXUDYN examples folder
#
# Author: Martin Sereinig, Peter Manzl
# Date: 2023-05-31 (created)
# last Update: 2023-09-11
# 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.
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# general imports
import numpy as np
import roboticstoolbox as rtb
from spatialmath import SE3
# exudyn imports
import exudyn as exu
from exudyn.utilities import *
from exudyn.itemInterface import *
from exudyn.rigidBodyUtilities import *
from exudyn.graphicsDataUtilities import *
from exudyn.robotics import *
from exudyn.robotics.models import ManipulatorUR5, LinkDict2Robot
from exudyn.robotics.motion import Trajectory, ProfileConstantAcceleration, ProfilePTP
import exudyn.robotics.rosInterface as exuROS #exudyn ROS interface class
import exudyn.robotics.mobile as mobile
# ROS imports
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose
from std_msgs.msg import String
# 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='ROSMobileManipulator')
# initialization of all standard publisher done by super class
# use standard publisher functions form super class
# 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)
# 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: my_pose
poseSubsrciberBase = ''
poseSubsrciberTopic = 'my_pose' # topic to subscribe
self.my_pose = Pose() # data type of topic, must be named: self.twistSubscriberTopic
self.myPoseSubscriber = self.InitSubscriber(poseSubsrciberBase,poseSubsrciberTopic,Pose)
debugFlag = False # turn prints on and off
# function to check (exudyn) module version
def checkInstall(moduleName):
import importlib
found = importlib.util.find_spec(moduleName) is not None
if not(found):
print('Error! Please install the module {} Version 1.7 or higher using \npip install {}==1.7'.format(moduleName, moduleName))
return False
else:
return True
# function to control the mobile manipulator behavior
def functionStateMachine(t, posPlatform, ThetaPlatform, PosObj, armStatus, myState, myDict):
# initialize variables
v = [0.0,0.0,0]
# with None no arm movement is performed
TArm = None
grasp = None
# check robotC control string send via ROS
robotControlString = myROSInterface.my_string.data
if debugFlag:
if robotControlString!='':
print('robot control string: ', robotControlString)
else:
print('no robot control string')
if robotControlString == 'ms':
TArm = None
grasp = None
# get velocity data from ROS /cmd_vel topic, please use: rostopic pub -r 100 /cmd_vel geometry_msgs/Twist "..."
rosLinearVelo = myROSInterface.cmd_vel.linear
rosAngularVelo = myROSInterface.cmd_vel.angular
v = [rosLinearVelo.x, rosLinearVelo.y, rosAngularVelo.z]
elif robotControlString == 'a':
# state for arm movement
v = [0.0,0.0,0]
ArmPosition = [myROSInterface.my_pose.position.x, myROSInterface.my_pose.position.y, myROSInterface.my_pose.position.z]
ArmOrientationQ = [myROSInterface.my_pose.orientation.w, myROSInterface.my_pose.orientation.x, myROSInterface.my_pose.orientation.y, myROSInterface.my_pose.orientation.z]
ArmOrientationR = EulerParameters2RotationMatrix(ArmOrientationQ)
# build homogenous transformation from rotation matrix existing rotation matrix
TArmRot = np.eye(4)
TArmRot[0:3,0:3] = ArmOrientationR
TArm = HTtranslate(ArmPosition) @ TArmRot
elif robotControlString == 'mk':
# state for external cmd_vel (keyboard or other node)
# get velocity data from ROS /cmd_vel topic, please use: rostopic pub -r 100 /cmd_vel geometry_msgs/Twist "..." but send via keyboard node
rosLinearVelo = myROSInterface.cmd_vel.linear
rosAngularVelo = myROSInterface.cmd_vel.angular
v = [rosLinearVelo.x, rosLinearVelo.y, rosAngularVelo.z]
else:
# print('no valid control string received')
v = [0.0, 0.0, 0.0]
TArm = None
grasp = None
return v, TArm, grasp, myState, myDict
# set simulation system
SC = exu.SystemContainer()
mbs = SC.AddSystem()
# function to simulate the mobile manipulator
def SimulationMobileRobot(funcStatMachine,myROSInterface, p0=[0,0], theta0=0, flagFixObject=False, flagOnlyGrasp=False, verboseMode = 0, sensorWriteToFile = False):
compensateStaticTorques = False
mobRobSolutionPath = 'solution/'
hstepsize = 5e-3 # step size
tEnd = 100 # simulation time
comShift=[0,0,-0.1]
debugPlatformOffset = 125*9.81 /10**(6)
constrainedAxesSet=[0,0,0,0,0,0]
offsetUserFunctionParametersSet=[0,0,0,0,0,0]
debugOffsetNumber= debugPlatformOffset
#ground body and marker
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])
graphicsGroundList =[gGround]
coordinateSystemGround = False
if coordinateSystemGround:
graphicsGroundList += [GraphicsDataCylinder([0,0,0], [0.5,0,0], 0.0035, color4red)] # base coordinate system x
graphicsGroundList +=[GraphicsDataCylinder([0,0,0], [0,0.5,0], 0.0035, color4green)] # base coordinate system y
graphicsGroundList +=[GraphicsDataCylinder([0,0,0], [0,0,0.5], 0.0035, color4blue)] # base coordinate system z
oGround = mbs.AddObject(ObjectGround(visualization=VObjectGround(graphicsData=graphicsGroundList)))
markerGround = mbs.AddMarker(MarkerBodyRigid(bodyNumber=oGround, localPosition=[0,0,0]))
comShiftPlatform = [0,0,0]
# define mobile manipulator KAIROS
mobileRobot = { 'gravity': [0,0,-9.81], # gravity in m/s^2
'platformDimensions': [0.575, 0.718 , 0.2495], # [width, length, hight] [0.575, 0.718 , 0.495]
'platformMass': 125-18.4, # platform mass- manipulator mass
'platformInitialPose': HTtranslate([p0[0],p0[1],(0.495+(-0.12))]) @ HTrotateZ(theta0), # platform initial pose as HT middle of platform (box representation)
'platformInitialOmega': [0,0,0], # platform initial rotational velocity around x,y,z axis
'platformInitialVelocity': [0,0,0], # platform initial translational velocity in x,y,z direction
'platformCOM': comShift, # center of mass shift to base coordinate system
'comShiftPlatform': comShiftPlatform, # the shift of the platform alone
'platformBaseCoordinate': [0.0 ,0.0 ,0.0], # geometric center in middle of platform
'platformInertia': InertiaCuboid, # platform inertia w.r.t. COM!
'platformRepresentation': 'box', # 'box' or 'stl' graphical representation of the mobile platform
'platformStlFile': 'ROSbodykairos.stl', # path to the used stl file
'friction': [1 ,0.0, 0.0], # [dryFriction1, dryFriction2,rollFriction]= [0.4,0.0075,0.05] for LeoBot (Master Thesis Manzl)
'viscousFrictionWheel': [0.1, 0.1], # orthotropic damping in the rotated roller frame; see also Exudyn documentation of
'frictionAngle': np.pi/4 , # friction angle theta=pi/4 for mecanum wheel, theta=0 for standard wheel
'wheelType': 0, # 0=wheelType wheel o-config, 1=mecanum wheel x-config, 2=standard wheel (always in bottom view)
'wheelBase': 0.430, # distance between center of wheels (wheel axes) between front and back
'wheelTrack': 0.390, # distance between center of wheels between left and right
'wheelRoh': 200*8, # density of wheel in kg/m^3
'wheelRadius': 0.254/2, # radius of wheel in m
'wheelWidth': 0.1, # width of wheel in m, just for graphics
'wheelMass': 8, # Mass of one mecanum wheel, leobot measured
'wheelInertia': InertiaCylinder, # inertia for infinitely small ring:
'wheelNumbers': 4, # number of wheels on platform
'wheelContactStiffness': 10**(6),
'wheelContactDamping': 50*np.sqrt(10**(5)),
'serialRobotMountpoint': HTtranslate([0.178 , 0, 0.12]),
'proportionalZone': 1e-2, # friction regularization
'debugOffset': debugOffsetNumber
}
#################### Build mobile robot and add it to existing mbs
mobileRobotBackDic = mobile.mobileRobot2MBS(mbs, mobileRobot, markerGround)
mbs.variables['mobileRobotBackDic'] = mobileRobotBackDic # to be able to use all variables in all functions (make it global useable)
# add mbs.variable for ROS sensor
mbs.variables['nodeNumber'] = mobileRobotBackDic['nPlatformList'][0] # just needed if nodeNumber is used for sensor information
# for shorter writing:
Lx = mobileRobot['wheelTrack']
Ly = mobileRobot['wheelBase']
R = mobileRobot['wheelRadius']
#initialize mobile platform kinematics
platformKinematics = mobile.MobileKinematics(R,Lx,Ly,wheeltype=0)
def UFoffset(mbs,t,itemIndex,offsetUserFunctionParameters):
return offsetUserFunctionParameters
mbs.SetObjectParameter(mobileRobotBackDic['oPlatformList'][0],'offsetUserFunctionParameters',offsetUserFunctionParametersSet)
mbs.SetObjectParameter(mobileRobotBackDic['oPlatformList'][0],'offsetUserFunction',UFoffset)
mbs.SetObjectParameter(mobileRobotBackDic['oPlatformList'][0],'constrainedAxes',constrainedAxesSet)
######################## Sensor data from mobile platform ###
WheelSpringDamper = [0]*4
MotorDataNode = [0]*4
cWheelBrakes = [0]*4
for i in range(4):
RM0 = mbs.GetObject(mobileRobotBackDic['oAxlesList'][i])['rotationMarker0']
RM1 = mbs.GetObject(mobileRobotBackDic['oAxlesList'][i])['rotationMarker1']
# wheel controller for KAIROS Platform
paramOpt = {'kMotor': 100, 'fact_dMotor': 0.5}
kWheelControl = paramOpt['kMotor']
dWheelControl = kWheelControl * paramOpt['fact_dMotor']
MotorDataNode[i] = mbs.AddNode(NodeGenericData(numberOfDataCoordinates = 1, initialCoordinates=[0]))
WheelSpringDamper[i] = mbs.AddObject(TorsionalSpringDamper(name='Wheel{}Motor'.format(i),
markerNumbers=[mobileRobotBackDic['mAxlesList'][i], mobileRobotBackDic['mWheelsList'][i]],
nodeNumber= MotorDataNode[i], # for continuous Rotation
stiffness = kWheelControl, damping = dWheelControl, offset = 0,
rotationMarker0=RM0,
rotationMarker1=RM1))
cWheelBrakes[i] = mbs.AddObject(GenericJoint(markerNumbers=
[mobileRobotBackDic['mAxlesList'][i], mobileRobotBackDic['mWheelsList'][i]],
constrainedAxes = [0]*6,
rotationMarker0=RM0,
rotationMarker1=RM1,
))
mbs.variables['flagBrakeActive'] = False
# wheel user function
mbs.variables['signWheels'] = [-1,1,1,-1]
mbs.variables['t0'] = 0
mbs.variables['phiWheel'] = [0,0,0,0]
vMax = 3.0
wMax = vMax / mobileRobot['wheelRadius'] # m/s
mbs.variables['wHistory'] = [[],[],[],[]] # for debug
def PreStepUFWheel(mbs, t, w= [0,0,0,0]):
if t == 0:
return True
dt = mbs.sys['dynamicSolver'].it.currentStepSize
dwMax = wMax * dt
if debugFlag: print('dwMax = ', dwMax)
for i in range(4):
wOld = mbs.GetObjectParameter(WheelSpringDamper[i], 'velocityOffset')
if w[i] > wOld + dwMax:
w[i] = wOld + dwMax
elif w[i] < wOld - dwMax:
w[i] = wOld - dwMax
mbs.variables['phiWheel'][i] += (t-mbs.variables['t0'])*w[i] #* mbs.variables['signWheels'][i]
mbs.SetObjectParameter(WheelSpringDamper[i], 'offset', mbs.variables['phiWheel'][i])
mbs.SetObjectParameter(WheelSpringDamper[i], 'velocityOffset', w[i])
mbs.variables['wHistory'][i] += [w[i]]
# mbs.SetObjectParameter(WheelSpringDamper[i], 'offset', SmoothStep(t, 0.5 , 2, 0, 1)*t * mbs.variables['signWheels'][i])
mbs.variables['t0'] = t
return True
sPlatformPosition = mbs.AddSensor(SensorMarker(name='platformpos',markerNumber=mobileRobotBackDic['mPlatformList'][-1],
fileName=mobRobSolutionPath + '/rollingDiscCarPos.txt',
outputVariableType = exu.OutputVariableType.Position, writeToFile = sensorWriteToFile,storeInternal=True))
sPlatformVelocity = mbs.AddSensor(SensorMarker(name='platformvelo',markerNumber=mobileRobotBackDic['mPlatformList'][-1],
fileName=mobRobSolutionPath + '/rollingDiscCarVel.txt',
outputVariableType = exu.OutputVariableType.Velocity, writeToFile = sensorWriteToFile,storeInternal=True))
sPlatformOrientation = mbs.AddSensor(SensorBody(name='platformRot',bodyNumber=mobileRobotBackDic['bPlatformList'][0],
fileName=mobRobSolutionPath + '/rollingDiscCarOrientation.txt',
outputVariableType = exu.OutputVariableType.Rotation , writeToFile = sensorWriteToFile))
mbs.variables['sensorList'] = [sPlatformPosition, sPlatformOrientation]
# ad manipulator to model
if 1:
mode='newDH'
qOffset = [-np.pi * 1/4, 0,0,0,0,0]
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
tx = 0.03
zOff = -0.2
toolSize= [tx*0.5, 0.06,0.12]
r6 = 0.04
graphicsToolList = []
graphicsToolList += [GraphicsDataCylinder(pAxis=[0,0,zOff], vAxis= [0,0,np.abs(zOff)*0.8], radius=r6, color=color4red)]
graphicsToolList+= [GraphicsDataOrthoCubePoint([ tx,0, 0], toolSize, color4grey)]
graphicsToolList+= [GraphicsDataOrthoCubePoint([-tx,0, 0], toolSize, color4grey)]
graphicsToolList+= [GraphicsDataOrthoCubePoint([0,0, -0.05], [tx*5,0.09,0.04], color4grey)]
graphicsToolList += [GraphicsDataBasis(length=0.2)]
basePoseHT=mobileRobot['platformInitialPose'] @ mobileRobot['serialRobotMountpoint'] @ HTrotateZ(qOffset[0]) #robot base position and orientation
# manipulator input with included function from exudyn robotics models
myRobotList = ManipulatorUR5()
robot = Robot(gravity=[0,0,-9.81],
base = RobotBase(HT=basePoseHT), #visualization=VRobotBase(graphicsData=graphicsBaseList)),
tool = RobotTool(HT=HTtranslate([0,0,0.155]), # @ HTrotateZ(np.pi/2),
visualization=VRobotTool(graphicsData=graphicsToolList)),
referenceConfiguration = q0) #referenceConfiguration created with 0s automatically
robot = LinkDict2Robot(myRobotList, robotClass=robot)
#control parameters, per joint:
fc=0.5
Pcontrol = np.array([4000, 4000, 4000, 100, 100, 100])
Dcontrol = np.array([60, 60, 60, 6, 6, 0.6])
Pcontrol = fc*Pcontrol
Dcontrol = fc*Dcontrol
# change predefined control parameters
for i in range(robot.NumberOfLinks()):
robot.links[i].PDcontrol = (Pcontrol[i], Dcontrol[i])
#trajectory generated with optimal acceleration profiles:
trajectory = Trajectory(initialCoordinates=q0, initialTime=0)
trajectory.Add(ProfileConstantAcceleration(q0,0.5))
jointList = [0]*robot.NumberOfLinks() #this list must be filled afterwards with the joint numbers in the mbs!
def ComputeMBSstaticRobotTorques(robot):
q=[]
for joint in jointList:
q += [mbs.GetObjectOutput(joint, exu.OutputVariableType.Rotation)[2]] #z-rotation
HT=robot.JointHT(np.array(q)+qOffset)
return robot.StaticTorques(HT)
#++++++++++++++++++++++++++++++++++++++++++++++++
#base, graphics, object and marker:
#baseMarker; could also be a moving base according to doc but found no examples!
baseMarker = mobileRobotBackDic['mPlatformList'][-1] # mbs.AddMarker(MarkerBodyRigid(bodyNumber=objectGround, localPosition=[0,0,0]))
sArmTorque = [0,0,0,0,0,0]
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#build mbs robot model:
robotDict = robot.CreateRedundantCoordinateMBS(mbs, baseMarker=baseMarker)
jointList = robotDict['jointList'] #must be stored there for the load user function
torsionalSDlist = robotDict['springDamperList']
for i in range(len(robotDict['springDamperList'])):
sArmTorque[i] = mbs.AddSensor(SensorObject(objectNumber= robotDict['springDamperList'][i],
fileName=mobRobSolutionPath+ '/ArmMotorTorque'+str(i) + '.txt',
outputVariableType = exu.OutputVariableType.TorqueLocal, writeToFile=sensorWriteToFile))
# mbs.SetObjectParameter(torsionalSDlist[i], 'offset', q0[i] + qOffset[i])
mEndeffektor = mbs.AddMarker(MarkerBodyRigid(name='Endeffektor', bodyNumber=robotDict['bodyList'][-1], localPosition=[0,0,0.157]))
sEndeffektor = mbs.AddSensor(SensorMarker(markerNumber=mEndeffektor, writeToFile=False, outputVariableType=exu.OutputVariableType.Position))
# robotics toolbox by Peter Corke
UR5_rtb = rtb.models.DH.UR5()
UR5_rtb.tool = SE3([0,0,0.2])
def getNewTraj(TArm, qLast):
vm = [np.pi * 1.1/2]*6
am = [5 * 0.6]*6
nAttempts = 10
qNew = UR5_rtb.ikine_LM(TArm, q0 = qLast) # , q0=qLast-qOffset)
iAttempt = 1
if not(qNew.success):
for iAttempt in range(1, nAttempts):
# randomize initial angles to try to get a solution for the inverse kinematics
qNew = UR5_rtb.ikine_LM(TArm, q0 = (np.random.random(6)-1)*np.pi*2) # , q0=qLast-qOffset)
if qNew.success:
break
if not(qNew.success):
print('Inverse Kinematics could not be solved after {} attempts. \nPlease check if the given Pose \n{}\nis in the workspace.'.format(iAttempt, TArm))
return None, None
teMax = 0
qNew = qNew.q # - qOffset
for i in range(len(qNew)):
te = abs(qNew[i] - qLast[i])/vm[i] + vm[i]/am[i]
if te > teMax:
teMax = te
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)))
# qNew[1] += np.pi*2
return qNew, teMax
def activateBrakes(cJoints, oTSD, flag):
for i in range(len(cJoints)):
# mbs.SetObjectParameter(cJoints[i], 'constrainedAxes', [0,0,0,0,0,1*bool(flag)])
if flag: # deactivate motors
mbs.SetObjectParameter(oTSD[i], 'stiffness', 100)
mbs.SetObjectParameter(oTSD[i], 'damping', 5)
else:
mbs.SetObjectParameter(oTSD[i], 'stiffness', kWheelControl)
mbs.SetObjectParameter(oTSD[i], 'damping', dWheelControl)
print('un'*bool(not(flag)) + 'locking wheels')
return True
hObj = 0.08
xTable, yTable, hTable = 0.2, 0.4, 0.645
if flagFixObject:
PosObj = [3.0,1.0, hTable + hObj/2] # for testing of grasp
else:
PosObj = (np.random.rand(3)-0.5) * [0.4,0.8,0] + [1.5 ,0, hTable + hObj/2]
graphicsTarget = GraphicsDataCylinder(pAxis = [0,0,-hObj/2], vAxis = [0,0,hObj ], radius = 0.02, color=color4lightgreen)
inertiaTarget = InertiaCylinder(500, hObj , 0.02, 2)
nObj, bObj = AddRigidBody(mainSys = mbs,
inertia = inertiaTarget,
nodeType = str(exu.NodeType.RotationEulerParameters),
position = PosObj,
rotationMatrix = np.eye(3),
angularVelocity = [0,0,0],
velocity= [0,0,0],
gravity = [0,0,0],
graphicsDataList = [graphicsTarget])
mObj = mbs.AddMarker(MarkerBodyRigid(bodyNumber=bObj))
cGrasp = mbs.AddObject(RigidBodySpringDamper(markerNumbers=[mEndeffektor, mObj], stiffness = np.eye(6)*1e3, damping = np.eye(6)*1e2,
visualization={'show': False, 'drawSize': -1, 'color': [0]*4}, activeConnector=False))
graphicsTable = GraphicsDataOrthoCubePoint(centerPoint = [0,0,0], size = [xTable, yTable, hTable], color=color4darkgrey2)
nTable, bTable = AddRigidBody(mainSys = mbs,
inertia = inertiaTarget,
nodeType = str(exu.NodeType.RotationEulerParameters),
position = list(PosObj[0:2]) + [hTable/2],
rotationMatrix = np.eye(3),
angularVelocity = [0,0,0],
velocity= [0,0,0],
gravity = [0,0,0],
graphicsDataList = [graphicsTable])
mbs.variables['myDict'] = {}
#prestep user functions
for i in range(6):
mbs.variables['qDebug{}'.format(i)] = []
mbs.variables['state'] = 0
mbs.variables['trajectory'] = trajectory
#user function which is called only once per step, speeds up simulation drastically
def PreStepUF(mbs, t):
if compensateStaticTorques:
staticTorques = ComputeMBSstaticRobotTorques(robot)
else:
staticTorques = np.zeros(len(jointList))
PosPlatform = mbs.GetSensorValues(mbs.variables['sensorList'][0]) - [0.178, 0 , 0 ]#mbs.variables['sensorRecord{}'.format(mbs.variables['sensorRecorders'][0])]
ThetaPlatform = mbs.GetSensorValues(mbs.variables['sensorList'][1])[-1]
phi = np.zeros(len(robot.links))
for i in range(len(robot.links)):
phi[i] = mbs.GetObjectOutput(jointList[i], exu.OutputVariableType.Rotation)[2] #z-rotation
if t > mbs.variables['trajectory'][-1]['finalTime']:
armStatus = 1 # current trajectory finished
else:
armStatus = 0
# here functionstatemachine in preStepUserFunction call
vel, TArm, grasp, mbs.variables['state'], mbs.variables['myDict'] = funcStatMachine(t, PosPlatform, ThetaPlatform,
PosObj, armStatus, mbs.variables['state'], mbs.variables['myDict'])
if type(TArm) == np.ndarray:
TArm = SE3(TArm)
if mbs.variables['state'] == -1:
mbs.SetRenderEngineStopFlag(True)
print('finished Statemachine. ')
# platform kinematics calculation
w = platformKinematics.getWheelVelocities(vel)
if TArm != None:
lastTraj = mbs.variables['trajectory'][-1]
qLast = lastTraj['coordinateSets'][-1] # the last desired joint angles
tLast = lastTraj['finalTime'] #
qNew, Ttraj = getNewTraj(TArm, qLast)
if type(qNew) != type(None):
mbs.variables['trajectory'].Add(ProfileConstantAcceleration(qLast,t-tLast+0.1))
mbs.variables['trajectory'].Add(ProfileConstantAcceleration(qNew,Ttraj))
# print('\n\nTTraj is: \n{}\n\n'.format(np.round(Tj, 3)))
if grasp:
pEE = mbs.GetSensorValues(sEndeffektor)
distanceGrasp = pEE - PosObj - [0,0,0]
# print('distance grasp = ', distanceGrasp)
if np.linalg.norm(distanceGrasp) < 0.1: # distance of grasp to
print('grasp successful!')
# activate constraint for grasp
RotEE = mbs.GetNodeOutput(robotDict['nodeList'][-1], exu.OutputVariableType.RotationMatrix).reshape([3,3])
mbs.SetObjectParameter(cGrasp, 'rotationMarker1', RotEE)
# mbs.SetObjectParameter(cGrasp, '', RotEE) # position
# mbs.SetObjectParameter(cGrasp, 'constrainedAxes', [1]*6)
# mbs.SetObjectParameter(cGrasp, '', [1]*6)
mbs.SetObjectParameter(cGrasp, 'activeConnector', True)
offset_local = list(RotEE @ distanceGrasp)
offset_local[1] = 0
mbs.SetObjectParameter(cGrasp, 'offset', offset_local + [0,0,0]) # list(RotEE @ distanceGrasp) + [0,0,-hObj*0])
else:
print('grasping the object was not successful. Calculated distacne = {}'.format(np.round(distanceGrasp, 3)))
[u,v,a] = mbs.variables['trajectory'].Evaluate(t)
for i in range(len(robot.links)):
tsd = torsionalSDlist[i]
mbs.SetObjectParameter(tsd, 'offset', u[i] + qOffset[i])
mbs.SetObjectParameter(tsd, 'velocityOffset', v[i])
mbs.SetObjectParameter(tsd, 'torque', staticTorques[i]) #additional torque from given velocity
# send velocity data to ROS
myROSInterface.PublishTwistUpdate(mbs,t)
# send position data to ROS
myROSInterface.PublishPoseUpdate(mbs,t)
PreStepUFWheel(mbs, t, w)
if np.linalg.norm(w) < 1e-5 and t > 0.5:
if not(mbs.variables['flagBrakeActive']):
activateBrakes(cWheelBrakes, WheelSpringDamper, True)
mbs.variables['flagBrakeActive'] = True
else:
if mbs.variables['flagBrakeActive']:
activateBrakes(cWheelBrakes, WheelSpringDamper, False)
mbs.variables['flagBrakeActive'] = False
return True
mbs.SetPreStepUserFunction(PreStepUF)
SC.visualizationSettings.interactive.trackMarker = mobileRobotBackDic['mPlatformList'][0]
# start simulation:
mbs.Assemble()
PreStepUF(mbs, 0)
SC.visualizationSettings.connectors.showJointAxes = True
SC.visualizationSettings.connectors.jointAxesLength = 0.02
SC.visualizationSettings.connectors.jointAxesRadius = 0.002
SC.visualizationSettings.nodes.showBasis = True
SC.visualizationSettings.nodes.basisSize = 0.1
SC.visualizationSettings.loads.show = False
SC.visualizationSettings.openGL.multiSampling=4
SC.visualizationSettings.openGL.shadow = 0.5
SC.visualizationSettings.openGL.light0position = [0, -2, 10, 0]
SC.visualizationSettings.openGL.shadowPolygonOffset = 0.1
#mbs.WaitForUserToContinue()
simulationSettings = exu.SimulationSettings() #takes currently set values or default values
simulationSettings.timeIntegration.numberOfSteps = int(tEnd/hstepsize)
simulationSettings.timeIntegration.endTime = tEnd
simulationSettings.solutionSettings.solutionWritePeriod = hstepsize #0.005
simulationSettings.solutionSettings.sensorsWritePeriod = hstepsize # 0.005
simulationSettings.solutionSettings.binarySolutionFile = False
simulationSettings.solutionSettings.writeSolutionToFile = False
simulationSettings.timeIntegration.simulateInRealtime = True
#simulationSettings.timeIntegration.realtimeFactor = 0.25
simulationSettings.timeIntegration.verboseMode = verboseMode
#simulationSettings.timeIntegration.newton.useModifiedNewton = True
#simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints = True
#simulationSettings.timeIntegration.generalizedAlpha.useNewmark = simulationSettings.timeIntegration.generalizedAlpha.useIndex2Constraints
simulationSettings.timeIntegration.newton.useModifiedNewton = True
#simulationSettings.timeIntegration.generalizedAlpha.spectralRadius = 0.5 # 0.25
simulationSettings.timeIntegration.discontinuous.maxIterations = 3
simulationSettings.timeIntegration.adaptiveStepRecoveryIterations = 10
simulationSettings.timeIntegration.generalizedAlpha.computeInitialAccelerations= True # False
simulationSettings.displayComputationTime = True
simulationSettings.displayStatistics = True
#simulationSettings.linearSolverType = exu.LinearSolverType.EigenSpars
SC.visualizationSettings.general.autoFitScene=False
SC.visualizationSettings.general.renderWindowString = 'Mobile Robot Simulation'
SC.visualizationSettings.window.renderWindowSize=[1920,1200]
SC.visualizationSettings.window.startupTimeout=5000
SC.visualizationSettings.interactive.selectionLeftMouse = False
SC.visualizationSettings.interactive.selectionRightMouse = False
SC.visualizationSettings.openGL.initialModelRotation =RotationMatrixZ(-0.2) @ RotationMatrixX(np.pi/2.7) #
SC.visualizationSettings.openGL.initialZoom = 1.5
SC.visualizationSettings.openGL.initialCenterPoint = [0, 2, 0] # -1.7, -2, -2]
# SC.visualizationSettings.contour.outputVariable = exu.OutputVariableType.Displacement
# SC.visualizationSettings.contour.outputVariableComponent = 2 #0=x, 1=y, 2=z
exu.StartRenderer()
mbs.WaitForUserToContinue()
exu.SolveDynamic(mbs, simulationSettings, showHints=True, storeSolver = True)
#mbs.WaitForRenderEngineStopFlag()
exu.StopRenderer()
# for debug
if debugFlag:
import matplotlib.pyplot as plt
for i in range(4):
plt.plot(mbs.variables['wHistory'][i], label='wheel ' + str(i+1))
plt.legend()
plt.show()
mbs.PlotSensor(sensorNumbers=[sPlatformPosition], components=[0,1,2], labels=['x(m); ','y','z'], colorCodeOffset=2, closeAll=False)
mbs.PlotSensor(sensorNumbers=[sPlatformVelocity], components=[0,1,2], labels=['vx(m); ','vy','vz'], colorCodeOffset=2, closeAll=False)
return mbs
# main function
if __name__ == '__main__':
if not(checkInstall('exudyn')):
print('Error! Simulation can not be started!')
import sys
sys.exit()
print('Starting Simulation...')
# initialize ROS interface from own subclass
myROSInterface = MyExudynROSInterface()
# start simulation
SimulationMobileRobot(functionStateMachine,myROSInterface,p0=[0,0],flagFixObject=True)