To achieve this goal, I've added two proximity sensors at the edge of the tables and wrote the following code in the robot child script (Starting from the existing code linked to the UR10 model):
Code: Select all
function sysCall_init()
corout=coroutine.create(coroutineMain)
end
function sysCall_actuation()
if coroutine.status(corout)~='dead' then
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
end
end
-- This is a threaded script, and is just an example!
SafetySensorHandle=sim.getObjectHandle('Safety_Zone_Sensor')
result,distance,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=sim.handleProximitySensor(sim.handle_all)
function movCallback(config,vel,accel,handles)
for i=1,#handles,1 do
if sim.getJointMode(handles[i])==sim.jointmode_force and sim.isDynamicallyEnabled(handles[i]) then
sim.setJointTargetPosition(handles[i],config[i])
else
sim.setJointPosition(handles[i],config[i])
end
end
end
function moveToConfig(handles,maxVel,maxAccel,maxJerk,targetConf)
local currentConf={}
for i=1,#handles,1 do
currentConf[i]=sim.getJointPosition(handles[i])
end
sim.moveToConfig(-1,currentConf,nil,nil,maxVel,maxAccel,maxJerk,targetConf,nil,movCallback,handles)
end
function coroutineMain()
local jointHandles={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
jointHandles[i]=sim.getObjectHandle('UR10_joint'..i)
end
if result==0 then
local vel=120
local accel=40
local jerk=80
else if result > 0 then do
local vel=vel/2
local accel=accel/2
local jerk=jerk/2
end
end
local maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel_math.pi/180,vel*math.pi/180,vel*math.pi/180}
local maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
local maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
local targetPos1={90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180}
moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos1)
local targetPos2={-90*math.pi/180,45*math.pi/180,90*math.pi/180,135*math.pi/180,90*math.pi/180,90*math.pi/180}
moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos2)
local targetPos3={0,0,0,0,0,0}
moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos3)
end
end