control the velocity of roboter in threaded script

[Shaoxiang Wang]

hello，

I wrote a code in the threaded script. The general situation is to let the robot arm move according to the trajectory. When the sensor detects this robot arm, I hope that the robot arm will immediately reduce the movement speed. My question is: the sensor senses the movement of the robotic arm, but the robotic arm still runs the entire trajectory after running at high speed; it can only run at low speed when it is refreshed next time.

Is it because "When a threaded child script's execution is still underway, it will not be launched a second time."?

In this scenario, the movement of the manipulator is controlled at different speeds, can it only pass through a non-threaded script?

Best Wishes
Shaoxiang

Code: Select all

function sysCall_threadmain()
    -- Put some initialization code here

safetyStop= "0"
-- Get handles and postions of dummies
    targetDummy = sim.getObjectHandle("Target")
    idlePos = sim.getObjectPosition(targetDummy,-1)
    idleOrient = sim.getObjectOrientation(targetDummy,-1)
    
    pickupDummy = sim.getObjectHandle("pickupDummy")
    dummyPos = sim.getObjectPosition(pickupDummy ,-1)
    dummyOrient = sim.getObjectOrientation(pickupDummy ,-1)
-- Get release path handle
    pickupPath = createPath(idlePos,idleOrient,dummyPos,dummyOrient)
    
-- Get handles and postions of dummies
    releasePosHandle = sim.getObjectHandle("releasePos")
    releasePos = sim.getObjectPosition(releasePosHandle,-1)
    releaseOrient = sim.getObjectOrientation(releasePosHandle,-1)
-- Get release path handle
    releasePath = createPath(idlePos,idleOrient,releasePos,releaseOrient)
    
-- Get handles and postions of dummies
    putdown = sim.getObjectHandle("putdown")
    putdownPos = sim.getObjectPosition(putdown,-1)
    putdownOrient = sim.getObjectOrientation(putdown,-1)
-- Get release path handle
    putdownPath = createPath(releasePos,releaseOrient,putdownPos,putdownOrient)
    
connector = sim.getObjectHandle("Connector")

   

manipVelocity=0.8
accel = 0.5
gripperActionTime = 1
    -- Put your main loop here, e.g.:
    
    while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
       
      safetyStop = sim.getStringSignal("SafetyStop")
      if safetyStop == "1" then
         safetyStop=true
      else
         safetyStop=false
      end
   
      if not safetyStop then
      
        sim.followPath(targetDummy,pickupPath,3,0,manipVelocity,accel)
        sim.wait(1)
        -- Connect the connector to pickupDummy
        sim.setLinkDummy(connector,pickupDummy)
        -- Set link type
        sim.setObjectInt32Parameter(connector,sim.dummyintparam_link_type,sim.dummy_linktype_dynamics_loop_closure)
        sim.followPath(targetDummy,pickupPath,3,1,-manipVelocity,-accel)
      
        sim.followPath(targetDummy,releasePath,3,0,manipVelocity,accel)
        sim.wait(gripperActionTime*1)
        sim.followPath(targetDummy,putdownPath,3,0,manipVelocity,accel)
        sim.wait(gripperActionTime*2)
      
        -- Disconnect pickupDummy from connector
        sim.setLinkDummy(connector,-1)
        sim.wait(gripperActionTime*1)
        sim.followPath(targetDummy,releasePath,3,1,-manipVelocity,-accel)
        
      elseif safetyStop then
        sim.followPath(targetDummy,releasePath,3,0,manipVelocity*0.1,accel*0.1)
        sim.wait(gripperActionTime*1)
        sim.followPath(targetDummy,putdownPath,3,0,manipVelocity*0.1,accel*0.1)
        sim.wait(gripperActionTime*2)
      
        -- Disconnect pickupDummy from connector
        sim.setLinkDummy(connector,-1)
        sim.wait(gripperActionTime*1)
        sim.followPath(targetDummy,releasePath,3,1,-manipVelocity*0.1,-accel*0.1)
      
      end
      sim.switchThread()
    end
   
end

function sysCall_cleanup()
    -- Put some clean-up code here
end

-- See the user manual or the available code snippets for additional callback functions and details

function createPath(startPoint,startOrient,endPoint,endOrient)
    -- Create Path Object
    local path = sim.createPath(1)

    -- Create buffer variables
    local buffer = {startPoint[1],startPoint[2],startPoint[3],startOrient[1],startOrient[2],startOrient[3], 1,0,0,0,0,
                    endPoint[1],endPoint[2],endPoint[3],endOrient[1],endOrient[2],endOrient[3],             1,0,0,0,0}

    -- Insert 2 control points (start and endpoint)
    sim.insertPathCtrlPoints(path,0,0,2,buffer)

    -- Return handle to path
    return path
end

[coppelia]

Hello,

when using sim.followPath, the movement cannot be interrupted, unless the threaded script ends. You should instead use a combination of sim.rmlPos, sim.rmlStep and sim.rmlRemove. In that case, the movement can be interrupted at any time and adjusted accordingly. Have a look at this demo scene that illustrates how to use those Reflexxes commands.

Cheers

[Shaoxiang Wang]

Thank you very much ! I have solved the problem.

Now i have a new question. In my scene ,there are 6 paths. I want to execute these six paths in sequence.
To prevent the code from being too repetitive, I wrote an function. But the problem is that after the robot has completed the first path, it uses this equation to execute the second path.

The robot does not move.Furthermore, the following path cannot be completed. I don't know why.But if I list the codes of these six paths repeatedly without using equations, it works well.

Code: Select all

    -- do some initialization here
    
-- Get handles and postions of dummies
    tip = sim.getObjectHandle("tip")
    ownHandle = sim.getObjectHandle("Target")
    
    startPath1=sim.getObjectHandle("Path1_1")
    startPath2=sim.getObjectHandle("Path1_2")
    mittelPath1=sim.getObjectHandle("Path2_1")
    mittelPath2=sim.getObjectHandle("Path2_2")
    endPath1=sim.getObjectHandle("Path3_1")
    endPath2=sim.getObjectHandle("Path3_2")
    pathLength1=sim.getPathLength(startPath1)
    pathLength2=sim.getPathLength(startPath2)
    speed=0.02 -- end-effector velocity in m/s
    sim.setUserParameter(ownHandle,'@enable','')
    timestep=sim.getSimulationTimeStep() -- in s
    
    currentPositionRelative1=0
    currentPositionRelative2=0
    currentPositionRelative3=0
    currentPositionRelative4=0
    currentPositionRelative5=0
    currentPositionRelative6=0
    moveForward=true
    epsilon1=0.01
    epsilon2=0.01
    epsilon3=0.01
    epsilon4=0.01
    epsilon5=0.01
    epsilon6=0.01

function sysCall_threadmain()
    -- Put some initialization code here

  while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
    slowerWork=sim.getStringSignal("slowerWork")
    if slowerWork=="1" then
        slowerWork=true
    else
        slowerWork=false
    end
    
    -- path1
    if not slowerWork then
        pathAdvanceAbsolute=speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength1
        if currentPositionRelative1 < 1-epsilon1 and moveForward then
            currentPositionRelative1=currentPositionRelative1+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(startPath1,currentPositionRelative1)
            pathOrientation=sim.getOrientationOnPath(startPath1,currentPositionRelative1)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
    else
        pathAdvanceAbsolute=0.2*speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength1
        if currentPositionRelative1 < 1-epsilon1 and moveForward then
            currentPositionRelative1=currentPositionRelative1+pathAdvanceRelative
            pathPosition=sim.getPositionOnPath(startPath1,currentPositionRelative1)
            pathOrientation=sim.getOrientationOnPath(startPath1,currentPositionRelative1)
            objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
            objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
    end
  
    
     -- path2
    if currentPositionRelative1 >= 1-epsilon1 then
      actuation(epsilon2,currentPositionRelative2,startPath2,pathLength2)
    end
  end  
  
end

function sysCall_cleanup()
    -- Put some clean-up code here
end

function actuation(epsilonnach,currentPositionRelativenach,path,pathLength)  
    if not slowerWork then
        pathAdvanceAbsolute=speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength
        if currentPositionRelativenach < 1-epsilonnach and moveForward then
           currentPositionRelativenach=currentPositionRelativenach+pathAdvanceRelative
           pathPosition=sim.getPositionOnPath(path,currentPositionRelativenach)
           pathOrientation=sim.getOrientationOnPath(path,currentPositionRelativenach)
           objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
           objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
    else
        pathAdvanceAbsolute=0.2*speed*timestep -- in m
        pathAdvanceRelative=pathAdvanceAbsolute/pathLength
        if currentPositionRelativenach < 1-epsilonnach and moveForward then
           currentPositionRelativenach=currentPositionRelativenach+pathAdvanceRelative
           pathPosition=sim.getPositionOnPath(path,currentPositionRelativenach)
           pathOrientation=sim.getOrientationOnPath(path,currentPositionRelativenach)
           objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
           objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
        end
    end
    
    
end    
-- See the user manual or the available code snippets for additional callback functions and details

[coppelia]

First, you should only write code within the system callback functions, and within your own functions. Typically the first few lines should go inside of sysCall_threadmain().

As to why your code is not correctly working... this is apparently directly dependent on your programmed algorithm. There must be something wrong in there.

Cheers