Change robot speed using Proximity sensor

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

Change robot speed using Proximity sensor

Post by Leizah »

Hi.

I've a scene in which a Bill model approaches to a UR10 robot.

I would like to change robot speed according to the distance between the robot base and the human model.

To do that, I placed a proximity sensor ('Safety_Zone_Sensor') at a defined distance d in order to trigger the switch of robot speed.
To get that result, I wrote the following code (The 'Explicit handling' box is checked):

Code: Select all

function sysCall_init()
    corout=coroutine.create(coroutineMain)
    SafetyZoneHandle=sim.getObjectHandle('Safety_Zone_Sensor')
    
end

function sysCall_sensing()
    Safetyresult,Safetydistance,SafetydetectedPoint,SafetydetectedObjectHandle,SafetydetectedSurfaceNormalVector=sim.handleProximitySensor(SafetyZoneHandle)
    SafetyZoneresult,SafetyZonedistance=sim.readProximitySensor(SafetyZoneHandle)
    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
-- This is a threaded script, and is just an example!

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 SafetyZoneresult==0 then
        local vel=120
        local accel=40
        local jerk=80
    else if SafetyZoneresult > 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


Unfortunately, I get the following error:

Code: Select all

[UR10@childScript:error] [string "UR10@childScript"]:52: attempt to compare number with nil
    stack traceback:
        [string "UR10@childScript"]:52: in function 'coroutineMain'
    stack traceback:
        [C]: in function 'error'
        [string "UR10@childScript"]:18: in function 'sysCall_actuation'
I've performed the simulation without the if-cycle that returns the error and I get the right values of SafetyZoneresult (0 when Bill is not colliding with the sensor, 1 when he is), so I don't understand why I get this error.

coppelia
Site Admin
Posts: 8828
Joined: 14 Dec 2012, 00:25

Re: Change robot speed using Proximity sensor

Post by coppelia »

Hello,

that simply means that SafetyZoneresult is nil. Which in turn means that sysCall_sensing wasn't called yet, or that sim.readProximitySensor returned nil, which can't be from the documentation.

Cheers

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

Re: Change robot speed using Proximity sensor

Post by Leizah »

coppelia wrote:
22 Jul 2021, 09:37
Hello,

that simply means that SafetyZoneresult is nil. Which in turn means that sysCall_sensing wasn't called yet, or that sim.readProximitySensor returned nil, which can't be from the documentation.

Cheers
Ok, so how can I fix it?
Leizah wrote:
21 Jul 2021, 18:11
I've performed the simulation without the if-cycle that returns the error and I get the right values of SafetyZoneresult (0 when Bill is not colliding with the sensor, 1 when he is), so I don't understand why I get this error.
As I said in my previous answer, setting a value of vel without using the if-cycle related to the result of the sim.readProximitySensor command I get the expected values of 0 when no contact is detected and 1 when there's a contact, so I think that the problem is related to the call of the sysCall_sensing function, but I don't know how to fix this.

EDIT

I solved the nil problem setting the values of vel, accel and jerk as global values into the sysCall_init() function.
Then, I create a if-cycle into the function coroutineMain() to change these three values when the proximity sensor result is 1, but unfortunately nothing happens, the robot does not change its speed (It seems like the SafetyZoneresults, which, according to the documentation, has a value of -1,0 or 1, is a nil value, but I've not understand why).
This is my updated code:

Code: Select all

function sysCall_init()
    corout=coroutine.create(coroutineMain)
    SafetyZoneHandle=sim.getObjectHandle('Safety_Zone_Sensor')
    vel=120
    accel=120
    jerk=120
end

function sysCall_sensing()
    Safetyresult,Safetydistance,SafetydetectedPoint,SafetydetectedObjectHandle,SafetydetectedSurfaceNormalVector=sim.handleProximitySensor(SafetyZoneHandle)
    SafetyZoneresult,SafetyZonedistance=sim.readProximitySensor(SafetyZoneHandle)
    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
-- This is a threaded script, and is just an example!

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 SafetyZoneresult==1 then
         vel=1
         accel=1
         jerk=1
    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

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

Re: Change robot speed using Proximity sensor

Post by Leizah »

coppelia wrote:
22 Jul 2021, 09:37
Hello,

that simply means that SafetyZoneresult is nil. Which in turn means that sysCall_sensing wasn't called yet, or that sim.readProximitySensor returned nil, which can't be from the documentation.

Cheers
Hi,

I'm sorry to bother you again, but I still have no clue on how to fix this.

To avoid the missing call of the sysCall_sensing function, I used again sim.readProximitySensor into the coroutineMain() function.
Using the print command, I can see that the value of SafeZoneResult (The result of the proximity sensor) is 0 when Bill is outside the sensor area, 1 when is inside it.
So I tried using the distance measured by the sensor (I verified that this distance is near 0 when Bill reaches the inner part of the sensor area and near 1.8 when he reaches the outer part of it), but I still get no results.

If I don't set an initial value for vel, accel and jerk, the software returns me the following error:

Code: Select all

[UR10@childScript:error] [string "UR10@childScript"]:71: attempt to perform arithmetic on a nil value (global 'vel')
    stack traceback:
        [string "UR10@childScript"]:71: in function 'coroutineMain'
I was not able to figure out why the global variable 'vel' is considered as a nil value, according to the following if-condition that I set:

Code: Select all

if (SafetyZoneresult < 1) and (SafetyZoneresult ~= nil) then
        local vel=120
        local accel=80
        local jerk=40
else
       local vel=60
       local accel=40
       local jerk=20
end
(I tried to set the initial speed very low and the other one equal to the nominal value)

I hope that there's a way to fix this problem.
Thanks in advance for your support.

Cheers

fferri
Posts: 807
Joined: 09 Sep 2013, 19:28

Re: Change robot speed using Proximity sensor

Post by fferri »

Leizah wrote:
27 Jul 2021, 15:36

Code: Select all

if (SafetyZoneresult < 1) and (SafetyZoneresult ~= nil) then
In such comparison, order of conjuncts is important.

E.g.:

Code: Select all

--lua
if x>0 and x~=nil then print(1) else print(2) end
stdin:1: attempt to compare number with nil
stack traceback:
stdin:1: in main chunk
[C]: in ?


move the nil check before:

Code: Select all

--lua
if x~=nil and x>0 then print(1) else print(2) end
2

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

Re: Change robot speed using Proximity sensor

Post by Leizah »

fferri wrote:
28 Jul 2021, 14:15
Leizah wrote:
27 Jul 2021, 15:36

Code: Select all

if (SafetyZoneresult < 1) and (SafetyZoneresult ~= nil) then
In such comparison, order of conjuncts is important.

E.g.:

Code: Select all

--lua
if x>0 and x~=nil then print(1) else print(2) end
stdin:1: attempt to compare number with nil
stack traceback:
stdin:1: in main chunk
[C]: in ?


move the nil check before:

Code: Select all

--lua
if x~=nil and x>0 then print(1) else print(2) end
2
Thank you for your reply, but unfortunately the problem was somewhere else.

I think that's linked to the actual position of the if-cycle into the code, because I tried with another approach (using the simCheckDistance command and setting a condition "if d > x then ... else...", but it reads just the first value and doesn't change it during the simulation (I tried also with a while statement, nothing happens too).

To be clear, I set the following code:

Code: Select all

function sysCall_init()
    SafetyZoneHandle=sim.getObjectHandle('Safety_Zone_Sensor')
    local Robot_Handle=sim.getObjectHandle('UR10')
    local Human_Handle=sim.getObjectHandle('Bill_Path')
    local Safety_Zone_Handle=sim.getObjectHandle('Safety_Zone')
    HumanCollection=sim.createCollection(0)
    sim.addItemToCollection(HumanCollection,sim.handle_tree,Human_Handle,0)
    RobotCollection=sim.createCollection(0)
    sim.addItemToCollection(RobotCollection,sim.handle_tree,Robot_Handle,0)
    SafetyZoneCollection=sim.createCollection(0)
    sim.addItemToCollection(SafetyZoneCollection,sim.handle_tree,Safety_Zone_Handle,0)
    
    corout=coroutine.create(coroutineMain)
    
end

function sysCall_sensing()
  
    local Safetyresult,Safetydistance,SafetydetectedPoint,SafetydetectedObjectHandle,SafetydetectedSurfaceNormalVector=sim.handleProximitySensor(SafetyZoneHandle)
    local SafetyZoneresult,SafetyZonedistance=sim.readProximitySensor(SafetyZoneHandle)
   
    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 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
    
    Robot_Human,Robot_Human_dist,Robot_Human_pair=sim.checkDistance(RobotCollection,HumanCollection)
    SafetyZone_Human,SafetyZone_Human_dist,SafetyZone_Human_pair=sim.checkDistance(SafetyZoneCollection,HumanCollection)
    
    if Robot_Human_dist[7] < 1.87 then
        vel=120
        accel=80
        jerk=60 
        
        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}
        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}
        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}
    
        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}
        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}
        targetPos3={0,0,0,0,0,0}
    else
        vel=1
        accel=1
        jerk=1 
        
        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}
        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}
        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}
    
        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}
        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}
        targetPos3={0,0,0,0,0,0}
    end
    
    moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos1)
    moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos2)
    moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos3)
I decided to place the maxVel, maxAccel and maxJerk equation into both the if and else condition because, when it is placed outside this condition, the software always returns me a "attempt to perform arithmetic on a nil value ('vel'/'accel'/'jerk')" error.

In my scene, Bill initial position is 4 m far from the robot, so I have the following situations:
  • If I set the condition

    Code: Select all

    Robot_Human_dist[7] < 10
    then the robot, during the simulation, starts with the following parameters:

    vel=120
    accel=80
    jerk=60

    and, so long as the else condition cannot happen (The cell il 7x7, so Bill never reach a distance d>10 m), these parameters never change during the simulation.
  • If I set the condition

    Code: Select all

    Robot_Human_dist[7] < 1.87
    (1.87 is the distance where the proximity sensor has been placed) then the robot, during the simulation, starts with the following parameters:

    vel=1
    accel=1
    jerk=1

    this is correct, because the initial value of distance is 4, but when the condition is reached, the initial parameters don't change during the simulation.

So the if-condition (and seems that the while-condition does the same too) sets just the initial values of the simulations, whom, once set, never change.
Where am I wrong? Should I use a different tipe of condition?

Thanks in advance for your support.

Cheers

fferri
Posts: 807
Joined: 09 Sep 2013, 19:28

Re: Change robot speed using Proximity sensor

Post by fferri »

Try to print the content of the various variables (or maybe use a graph, when it makes sense, e.g. continuous variables).

It is hard to diagnose your problem remotely.

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

Re: Change robot speed using Proximity sensor

Post by Leizah »

fferri wrote:
29 Jul 2021, 15:24
Try to print the content of the various variables (or maybe use a graph, when it makes sense, e.g. continuous variables).

It is hard to diagnose your problem remotely.
Which variables have I to print?
I could also share a semplified version of the scene, with the walking bill, the robot and the proximity sensor, if it can help.

Cheers.

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

Re: Change robot speed using Proximity sensor

Post by Leizah »

fferri wrote:
29 Jul 2021, 15:24
Try to print the content of the various variables (or maybe use a graph, when it makes sense, e.g. continuous variables).

It is hard to diagnose your problem remotely.
Hi fferri, I worked out on the code, implementing IK on the robot.
I tried to find the best values to show in order to clarify my problem, but at the end I belive that's easier if I post my actual scene.

scene

This is the code I used:

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')
    Human_Handle=sim.getObjectHandle('Bill_Path')
    Safety_Sensor_Handle=sim.getObjectHandle('Safety_Zone_Sensor')
    
    simJoints={}
    for i=1,6,1 do
        simJoints[i]=sim.getObjectHandle('UR10_joint'..i)
    end
    
    -- Proximity sensor read
    Safetyresult,Safetydistance,SafetydetectedPoint,SafetydetectedObjectHandle,SafetydetectedSurfaceNormalVector=sim.handleProximitySensor(Safety_Sensor_Handle)
    SafetyZoneresult,SafetyZonedistance=sim.readProximitySensor(Safety_Sensor_Handle)
    
    -- Proximity sensor logic conditions
    if SafetyZoneresult==1 then
       HumanInDanger=true
    else
       HumanInDanger=false
    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})
    
    -- Avoidance tasks:
    ikGroupAvoidance=simIK.createIkGroup(ikEnv)
    simIK.setIkGroupCalculation(ikEnv,ikGroupAvoidance,simIK.method_damped_least_squares,1,1)
    linksAndAvoidanceItems={}
    avoidanceItems={tips={},targets={},ikElements={},ikTips={}}
    for i=1,6,1 do
        local link=sim.getObjectHandle('UR10_link'..i)
        local data={}
        local tip=sim.createDummy(0.05,{0,1,0,0,0,0,0,0,0,0,0,0})
        sim.setObjectSpecialProperty(tip,0)
        local target=sim.createDummy(0.05,{1,0,0,0,0,0,0,0,0,0,0,0})
        sim.setObjectSpecialProperty(target,0)
        data.tip=tip
        sim.setObjectParent(tip,link,true)
        data.target=target
        sim.setObjectParent(target,simBase,true)
        local ikEl,map=simIK.addIkElementFromScene(ikEnv,ikGroupAvoidance,simBase,tip,target,simIK.constraint_x+simIK.constraint_y)
        data.ikElement=ikEl
        data.ikTip=map[tip]
        simIK.setIkElementFlags(ikEnv,ikGroupAvoidance,ikEl,0)-- disable it
        sim.setObjectInt32Param(tip,sim.objintparam_visibility_layer,0)
        sim.setObjectInt32Param(target,sim.objintparam_visibility_layer,0)
        linksAndAvoidanceItems[link]=data
    end
    
    distThreshold=0.01
    obstacles=sim.createCollection(0)
    sim.addItemToCollection(obstacles,sim.handle_all,-1,0)
    sim.addItemToCollection(obstacles,sim.handle_tree,simBase,1)
    
    manipulatorLinks=sim.createCollection(0)
    sim.addItemToCollection(manipulatorLinks,sim.handle_tree,simBase,0)
    
    multipleAvoidanceTasks=true
end

function sysCall_sensing()
    local vel=sim.getObjectVelocity(simTip)
    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()
    -- 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.02,0.02,0.02,0.02} -- 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
    while not HumanInDanger do
        moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_1,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_2,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_3,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_4,data)
    end
    
    while HumanInDanger do
        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

fferri
Posts: 807
Joined: 09 Sep 2013, 19:28

Re: Change robot speed using Proximity sensor

Post by fferri »

And what is the error with the scene you posted?

Post Reply