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

Post by Leizah »

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
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: How to stop robot motion

Post by coppelia »

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

Post by Leizah »

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)
    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
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

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

Re: How to stop robot motion

Post by Leizah »

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

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

Re: How to stop robot motion

Post by Leizah »

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
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: How to stop robot motion

Post by coppelia »

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

Post by Leizah »

Hi, thank you for your reply.
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
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: How to stop robot motion

Post by coppelia »

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

Post by Leizah »

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
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: How to stop robot motion

Post by coppelia »

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

Post Reply