How to stop robot motion

Typically: "How do I... ", "How can I... " questions
Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

How to stop robot motion

Hi,

I have a UR10 robot that performs an IK movement, I would like to interrupt this movement when a proximity sensor reads the presence of an other object.
I was not able to find a function to stop the robot, there's a way to achieve that?

coppelia
Posts: 8889
Joined: 14 Dec 2012, 00:25

Re: How to stop robot motion

Hello,

how do you generate the movement in the first place? Please show us a minimalistic code sequence of what you are doing.

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

Hi

This is the code I'm using, it comes from the IK scenes and considers two different logical condition:
• HumanInDanger: A proximity sensors detects the presence of human and then the robot slows down/stops (This is the condition that doesn't work)
• Robot_Start:The robot starts its motion when a signal is sent (This condition works fine)

Code: Select all

function sysCall_init()
corout=coroutine.create(coroutineMain)

-- Take a few handles from the dynamic robot:
simBase=sim.getObjectHandle('UR10')
simTip=sim.getObjectHandle('UR10_tip')
simTarget=sim.getObjectHandle('UR10_target')
Safety_Sensor_Handle=sim.getObjectHandle('Safety_Zone_Sensor')

Human_Handle=sim.getObjectHandle('Bill_Path')
BillCollection=sim.createCollection(0)

simJoints={}
for i=1,6,1 do
simJoints[i]=sim.getObjectHandle('UR10_joint'..i)
end

ikEnv=simIK.createEnvironment()
ikGroup=simIK.createIkGroup(ikEnv)
simIK.setIkGroupCalculation(ikEnv,ikGroup,simIK.method_damped_least_squares,0.01,10)
simIK.setIkElementPrecision(ikEnv,ikGroup,ikElement,{0.0005,0.005*math.pi/180})

end

function sysCall_sensing()
local vel=sim.getObjectVelocity(simTip)

Safetyresult,Safetydistance,SafetydetectedPoint,SafetydetectedObjectHandle,SafetydetectedSurfaceNormalVector=sim.handleProximitySensor(Safety_Sensor_Handle)
local txt='SafetyZoneresult is '..SafetyZoneresult
print(txt)

end

function sysCall_actuation()
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
end
end

function moveToPoseCallback(pose,velocity,accel,auxData)
sim.setObjectPose(auxData.target,-1,pose)
simIK.applyIkEnvironmentToScene(auxData.ikEnv,auxData.ikGroup)
end

function moveToPose_viaIK(maxVelocity,maxAcceleration,maxJerk,targetPose,auxData)
local currentPose=sim.getObjectPose(auxData.tip,-1)
return sim.moveToPose(-1,currentPose,maxVelocity,maxAcceleration,maxJerk,targetPose,moveToPoseCallback,auxData,nil)
end

function moveToConfigCallback(config,velocity,accel,auxData)
for i=1,#auxData.joints,1 do
local jh=auxData.joints[i]
if sim.getJointMode(jh)==sim.jointmode_force and sim.isDynamicallyEnabled(jh) then
sim.setJointTargetPosition(jh,config[i])
else
sim.setJointPosition(jh,config[i])
end
end
end

function moveToConfig_viaFK(maxVelocity,maxAcceleration,maxJerk,goalConfig,auxData)
local startConfig={}
for i=1,#auxData.joints,1 do
startConfig[i]=sim.getJointPosition(auxData.joints[i])
end
sim.moveToConfig(-1,startConfig,nil,nil,maxVelocity,maxAcceleration,maxJerk,goalConfig,nil,moveToConfigCallback,auxData,nil)
end

function coroutineMain()

-- Proximity sensor logic conditions
if SafetyZoneresult==1 then
HumanInDanger=true
else
HumanInDanger=false
end

-- IK movement data:

local maxIkVel={0.15,0.15,0.15,1.5} -- vx,vy,vz in m/s, Vtheta is rad/s
local maxIkAccel={0.03,0.03,0.03,0.24} -- ax,ay,az in m/s^2, Atheta is rad/s^2
local maxIkJerk={0.01,0.01,0.01,0.02} -- is ignored (i.e. infinite) with RML type 2

local minIkVel={0.05,0.05,0.05,0.5} -- vx,vy,vz in m/s, Vtheta is rad/s
local minIkAccel={0.01,0.01,0.01,0.01} -- ax,ay,az in m/s^2, Atheta is rad/s^2
local minIkJerk={0.01,0.01,0.01,0.02} -- is ignored (i.e. infinite) with RML type 2

local Target_1=sim.getObjectPose(sim.getObjectHandle('Object_Target_1'),-1)
local Target_2=sim.getObjectPose(sim.getObjectHandle('Object_Target_2'),-1)
local Target_3=sim.getObjectPose(sim.getObjectHandle('Object_Target_3'),-1)
local Target_4=sim.getObjectPose(sim.getObjectHandle('Object_Target_4'),-1)

-- FK movement data:
local initConf={}
local maxConfVel={}
local maxConfAccel={}
local maxConfJerk={}
for i=1,#simJoints,1 do
initConf[i]=sim.getJointPosition(simJoints[i])
maxConfVel[i]=45*math.pi/180
maxConfAccel[i]=0.15
maxConfJerk[i]=0.4
end

-- Execute the movement here:
local data={}
data.ikEnv=ikEnv
data.ikGroup=ikGroup
data.tip=simTip
data.target=simTarget
data.joints=simJoints

-- Wait until the human reaches the TP
Robot_Start=sim.waitForSignal('Human_TP')

if not HumanInDanger and Robot_Start then
sim.setJointTargetVelocity(simJoints[1],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[2],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[3],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[4],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[5],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[6],maxIkVel[1])
moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_1,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_2,data)
-- Human restart condition
sim.setIntegerSignal('Robot_Finish',2)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_3,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_4,data)
end

if HumanInDanger then
sim.setJointTargetVelocity(simJoints[1],minIkVel[1])
sim.setJointTargetVelocity(simJoints[2],minIkVel[1])
sim.setJointTargetVelocity(simJoints[3],minIkVel[1])
sim.setJointTargetVelocity(simJoints[4],minIkVel[1])
sim.setJointTargetVelocity(simJoints[5],minIkVel[1])
sim.setJointTargetVelocity(simJoints[6],minIkVel[1])
moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
moveToPose_viaIK(minIkVel,minIkAccel,minIkJerk,Target_1,data)
moveToPose_viaIK(minIkVel,minIkAccel,minIkJerk,Target_2,data)
moveToPose_viaIK(minIkVel,minIkAccel,minIkJerk,Target_3,data)
moveToPose_viaIK(minIkVel,minIkAccel,minIkJerk,Target_4,data)
end
end

function sysCall_cleanup()
simIK.eraseEnvironment(ikEnv)
end

I know that this code doesn't work because I should stop the robot motion and the restart it with different values of speed/acceleration, but I don't know how to do that.
It would also be better to just slow down the robot without stopping it, but I don't know how to do that since this is the only IK code that I found as reference.

There's a way to stop/slow down the robot when the condition Humanindanger is set to "true"?

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

Hi

I've tried to solve the issue, but I'm still stucked on how slow down the robot.
I've tried using the sim.checkDistance function, but I was not able to find out how to reduce the speed.

Should I prefer the distance calculation method or the proximity sensor approach to set the logical condition of slow down?

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

Hi, I've changed my scene and I've now the following situation:
• The robot begins its movement (5 control points are used to create the path via IK) when the human operator sends a signal "Robot_Start" (The human stops)

Code: Select all

if Robot_Start then
sim.setJointTargetVelocity(simJoints[1],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[2],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[3],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[4],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[5],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[6],maxIkVel[1])
moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_1,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_2,data)
• When the robot arrives at the second control point, a signal is sent to start the human movement

Code: Select all

-- Human restart condition
sim.setIntegerSignal('Robot_Working',2)
sim.switchThread()
• The human approaches the robot. At the beginning he is outside the proximity sensor range (not 'HumanInDanger' condition), but it arrives before the robot reaches the third control point of the IK trajectory.
Here I have the problem, because I need the robot to slow down when the HumanInDanger condition triggers.

Code: Select all

if not HumanInDanger then
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_3,data)
else
sim.setJointTargetVelocity(simJoints[1],minIkVel[1])
sim.setJointTargetVelocity(simJoints[2],minIkVel[1])
sim.setJointTargetVelocity(simJoints[3],minIkVel[1])
sim.setJointTargetVelocity(simJoints[4],minIkVel[1])
sim.setJointTargetVelocity(simJoints[5],minIkVel[1])
sim.setJointTargetVelocity(simJoints[6],minIkVel[1])
moveToConfig_viaFK_min(minConfVel,minConfAccel,minConfJerk,initConf,data)
moveToPose_viaIK_min(minIkVel,minIkAccel,minIkJerk,Target_3,data)
end
sim.switchThread()
The same code is used for control points 4 and 5
moveToConfig_viaFK_min and moveToPose_viaIK_min are function defined as the one used into the IK examples, with the minimum parameters instead of maximum one.

Hope I point out the problem clearly and that someone can help me.
Thank you
Cheers

coppelia
Posts: 8889
Joined: 14 Dec 2012, 00:25

Re: How to stop robot motion

You will have to make your routines moveToConfig_viaFK_min and moveToPose_viaIK_min interruptible or able to react to an external event, and re-plan the movement. Those routines are using sim.rml-type functions internally.
Have a look at the demo scene scenes/trajectoryAndMotion/reflexxesMotionLibraryExamples.ttt

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

According to the demo, I've modified the functions as follows:

Code: Select all

function moveToPose_viaIK_min(minVelocity,minAcceleration,minJerk,targetPose,auxData)
local currentPose=sim.getObjectPose(auxData.tip,-1)
local currentPosVelAccel={currentPose,maxIkVel,maxIkAccel}
local maxAccelJerk={maxIkVel,maxIkAccel,maxIkJerk} -- vx,vy,vz in m/s, ax,ay,az in m/s^2, jx,jy,jz is ignored (i.e. infinite) with RML type 2
local targetVel={minIkVel} -- x,y,z in m, vx,vy,vz in m/s
local rmlObject=sim.rmlVel(3,0.001,-1,currentPosVelAccel,maxAccelJerk,{1,1,1},targetVel)
local result,newPosVelAccel=sim.rmlStep(rmlObject,sim.getSimulationTimeStep())
end

Code: Select all

function moveToConfig_viaFK_min(minVelocity,minAcceleration,minJerk,goalConfig,auxData)
local startConfig={}
for i=1,#auxData.joints,1 do
startConfig[i]=sim.getJointPosition(auxData.joints[i])
end
sim.moveToConfig(-1,startConfig,nil,nil,minVelocity,minAcceleration,minJerk,goalConfig,nil,moveToConfigCallback,auxData,nil)
end
The I modified the IK path (I removed the if-conditions and I just consider 3 points to create the IK path to test the code) as follows:

Code: Select all

if Robot_Start then
sim.setJointTargetVelocity(simJoints[1],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[2],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[3],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[4],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[5],maxIkVel[1])
sim.setJointTargetVelocity(simJoints[6],maxIkVel[1])
moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_1,data)
moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_2,data)
-- Human restart condition
sim.setIntegerSignal('Robot_Working',2)
moveToConfig_viaFK_min(minConfVel,minConfAccel,minConfJerk,initConf,data)
result,newPosVelAccel=sim.rmlStep(rmlObject,sim.getSimulationTimeStep())
if result~=-1 then
sim.setJointTargetVelocity(simJoints[1],minIkVel[1])
sim.setJointTargetVelocity(simJoints[2],minIkVel[1])
sim.setJointTargetVelocity(simJoints[3],minIkVel[1])
sim.setJointTargetVelocity(simJoints[4],minIkVel[1])
sim.setJointTargetVelocity(simJoints[5],minIkVel[1])
sim.setJointTargetVelocity(simJoints[6],minIkVel[1])
moveToPose_viaIK_min(minIkVel,minIkAccel,minIkJerk,Target_3,data)
moveToConfig_viaFK_min(minConfVel,minConfAccel,minConfJerk,initConf,data)
end
Unfortunately, it doesn't work. When the robot reaches the target point 2 he "stops" and then becames to move very slowly (Not at the speed I set) and it doesn't follow the path anymore.
How could I fix this?

Cheers

coppelia
Posts: 8889
Joined: 14 Dec 2012, 00:25

Re: How to stop robot motion

You are not using the RML functions correctly. You should initialize a RML object with sim.rmlPos or sim.rmlVel, then call sim.rmlStep in a loop until you get a certain return value indicating that the movement has finished, or until a certain external event has happened. In that loop, you should also call sim.switchThread. And finally, you should clean up the RML object sim sim.rmlRemove. Have a look at the examples, and modify the example, and see the effect of your modifications.

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: How to stop robot motion

coppelia wrote:
06 Oct 2021, 14:29
You are not using the RML functions correctly. You should initialize a RML object with sim.rmlPos or sim.rmlVel, then call sim.rmlStep in a loop until you get a certain return value indicating that the movement has finished, or until a certain external event has happened. In that loop, you should also call sim.switchThread. And finally, you should clean up the RML object sim sim.rmlRemove. Have a look at the examples, and modify the example, and see the effect of your modifications.

Cheers
Hi, thank you for your response

I'm still missing the point. I've analysed the example and I tried to recreate the same situation using sim.rmlVel instead of sim.rmlPos.
The initialization of the object is set in moveToPose_viaIK_min. The sim.rmlStep function is also there. I've also problem to set the input values, since I'm not sure that the correntPos value I set is actually the one of the robot tip, since it is automatically generates through IK.

Should I erase the moveToPose_viaIK_min function and structure the code differently?
Cheers

coppelia