How to stop robot motion
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?
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?
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
how do you generate the movement in the first place? Please show us a minimalistic code sequence of what you are doing.
Cheers
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:
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"?
Thank you for your support
Cheers
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)
sim.addItemToCollection(BillCollection,sim.handle_tree,Human_Handle,0)
simJoints={}
for i=1,6,1 do
simJoints[i]=sim.getObjectHandle('UR10_joint'..i)
end
-- Following path task
ikEnv=simIK.createEnvironment()
ikGroup=simIK.createIkGroup(ikEnv)
simIK.setIkGroupCalculation(ikEnv,ikGroup,simIK.method_damped_least_squares,0.01,10)
local ikElement=simIK.addIkElementFromScene(ikEnv,ikGroup,simBase,simTip,simTarget,simIK.constraint_pose)
simIK.setIkElementPrecision(ikEnv,ikGroup,ikElement,{0.0005,0.005*math.pi/180})
end
function sysCall_sensing()
local vel=sim.getObjectVelocity(simTip)
-- Proximity sensor read
Safetyresult,Safetydistance,SafetydetectedPoint,SafetydetectedObjectHandle,SafetydetectedSurfaceNormalVector=sim.handleProximitySensor(Safety_Sensor_Handle)
SafetyZoneresult,SafetyZonedistance=sim.readProximitySensor(Safety_Sensor_Handle)
local txt='SafetyZoneresult is '..SafetyZoneresult
print(txt)
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
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
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"?
Thank you for your support
Cheers
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?
Thanks for your support
Cheers
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?
Thanks for your support
Cheers
Re: How to stop robot motion
Hi, I've changed my scene and I've now the following situation:
Hope I point out the problem clearly and that someone can help me.
Thank you
Cheers
- 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.The same code is used for control points 4 and 5Code: 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()
Hope I point out the problem clearly and that someone can help me.
Thank you
Cheers
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
Have a look at the demo scene scenes/trajectoryAndMotion/reflexxesMotionLibraryExamples.ttt
Cheers
Re: How to stop robot motion
Hi, thank you for your reply.
According to the demo, I've modified the functions as follows:
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:
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
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
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
How could I fix this?
Cheers
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
Cheers
Re: How to stop robot motion
Hi, thank you for your responsecoppelia 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
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
Re: How to stop robot motion
I think your current task is too complex, and you should really try to code some simple examples, e.g. control precisely the position of an object in 1 dimension, with the RML function (here again, see the demo scene I referred to). Once you have that, you can introduce an external event that can interrupt the current movement, and start a new movement.
Imagine you want to drive your object from pos A to pos B, with a max. velocity vel and max acceleration acc. Then you would set-up a RML task with sim.rmlPos. During execution of that movement with sim.rmlStep, if you detect a problem, you could stop movement of the object smoothly, by: a) deleting the current RML object with sim.rmlRemove, then creating a new one with sim.rmlVel, which would drive current velocity from v1, to 0 (with a maximum acceleration value acc). Once you completely stopped, you can start a new movement with the sim.rmlPos function again.
Cheers
Imagine you want to drive your object from pos A to pos B, with a max. velocity vel and max acceleration acc. Then you would set-up a RML task with sim.rmlPos. During execution of that movement with sim.rmlStep, if you detect a problem, you could stop movement of the object smoothly, by: a) deleting the current RML object with sim.rmlRemove, then creating a new one with sim.rmlVel, which would drive current velocity from v1, to 0 (with a maximum acceleration value acc). Once you completely stopped, you can start a new movement with the sim.rmlPos function again.
Cheers