Planning Task from a non-threaded script

Typically: "How do I... ", "How can I... " questions
Post Reply
formica
Posts: 52
Joined: 13 Mar 2013, 12:13

Planning Task from a non-threaded script

Post by formica » 03 Jul 2016, 18:31

Hi coppelia,
I'm trying to call a "compute_path" function from rosservice

Code: Select all

rosservice call /vrep/simRosCallScriptFunction setGoal@StartConfiguration 1 [] [2.0,1.0,1.0] [] ''
Everithing is ok but I can't call the same function a second time, passing a different goal, because the motion planning plugin returns this error:

Code: Select all

Debug:   BiTRRT: Discarded start state Compound state [
Compound state [
RealVectorState [2 1 1]
SO3State [0 0 0 1]
]
]

Error:   BiTRRT: Start tree has no valid states!
         at line 358 in /home/marc/Documents/ompl-1.1.0-Source/src/ompl/geometric/planners/rrt/src/BiTRRT.cpp
Any idea?

(the script code is following...)

Code: Select all

compute_path=function(inInts,inFloats,inStrings,inBuffer)
    local goalpose = {inFloats[1],inFloats[2],inFloats[3],0.0,0.0,0.0,1.0}
    
    
    local t=simExtOMPL_createTask('t')
    local ss={simExtOMPL_createStateSpace('6d',sim_ompl_statespacetype_pose3d,robotHandle,{-5,-5,0},{5,5,5},1)}
    simExtOMPL_setStateSpace(t,ss)
    simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_BiTRRT)
    simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('Sphere'),sim_handle_all})
    simExtOMPL_setStateValidationCallback(t,'stateValidation')

    --targetHandle=simGetObjectHandle('End')
    local startpos=simGetObjectPosition(robotHandle,-1)
    local startorient=simGetObjectQuaternion(robotHandle,-1)
    local startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
    simExtOMPL_setStartState(t,startpose)
    --goalpose={2.0,0.0,1.0,0.0,0.0,0.0,1.0}
    simExtOMPL_setGoalState(t,goalpose)
    --r,path=simExtOMPL_compute(t,20,-1,200)
    simSetThreadAutomaticSwitch(false)
    r,path=simExtOMPL_compute(t,20,-1,200)
    simSetThreadAutomaticSwitch(true)
    
    local tpath = insertPathIntoOctree(path)
    visualizePath(path)
    datamsg=simPackFloats(path)
    datapath=simPackFloats(path)
    --simSetStringSignal('pathstring',datapath)
    --followTrajectory(path)
    --pathmsg={}
    --pathmsg['header']={seq=0,stamp=0,frame_id="a"}
    --pathmsg['pose']={path}

    --datamsg={}
    --datamsg['header']={seq=0,stamp=0, frame_id="a"}
    --datamsg['poses']=pathmsg
    --simExtRosInterface_publish(path_pub,{datamsg})
    
    newGoal = 1
    --while true do
        -- Simply jump through the path points, no interpolation here:
       
    simExtOMPL_destroyTask(t)
        return {},{},{},''
end


if (sim_call_type==sim_childscriptcall_initialization) then
    newGoal = 0
    i = 1
    robotHandle=simGetObjectHandle('Sphere')
    octreeHandle = simGetObjectHandle('Octree')
    --path_pub=simExtRosInterface_advertise('/OMPLPath_1','std_msgs/String')
    --simExtRosInterface_publisherTreatUInt8ArrayAsString(path_pub)
    simExtROS_enablePublisher('/stringPath',1,simros_strmcmd_get_and_clear_string_signal,-1,-1,'pathstring')
    
    	
end


if (sim_call_type==sim_childscriptcall_actuation) then
    if(newGoal == 1) then
       local pc = #path/7
       for i=1,pc-1,1 do
            simSetObjectPosition(robotHandle,-1,{path[(i)*7+1],path[(i)*7+2],path[(i)*7+3]})
       end
       newGoal = 0
       path = {}
	end
end


if (sim_call_type==sim_childscriptcall_sensing) then

	
end


if (sim_call_type==sim_childscriptcall_cleanup) then
    --simExtRosInterface_shutdownPublisher(path_pub)
	
end

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

Re: Planning Task from a non-threaded script

Post by coppelia » 04 Jul 2016, 17:26

Hello,

if you call the same function with exactly the same arguments, and if your robot is in the exact same initial state, then you should be able to call it successfully as many times as needed. Of course if your first task moved the robot to a different location, then it could happen that the planner does not find a collision-free path. Which I guess is happening in your case.

Cheers

formica
Posts: 52
Joined: 13 Mar 2013, 12:13

Re: Planning Task from a non-threaded script

Post by formica » 04 Jul 2016, 17:31

Yes, I think so. I have to follow the trajectory after computing it.
So I'm trying to re-inizialize the tree every time that I call it...
But I'm not succeeding in it. I think that I have to destroy the task and the state-space... but using simExtOMPL_destroyTask I obtain the same error...

formica
Posts: 52
Joined: 13 Mar 2013, 12:13

Re: Planning Task from a non-threaded script

Post by formica » 05 Jul 2016, 11:24

I'm trying to instantiate a set of state space and use it once a time... I destroy the task every time that I call the function through RosService... but it doesn't work.
I obtain the same error.

Any idea?

Code: Select all

stateValidation=function(state)
    --local res,d=simCheckDistance(collisionPairs[1],collisionPairs[2],maxDistance)
    --local pass= (res==1) and (d[7]>minDistance)
    result,tag,locationLow,locationHigh = simCheckOctreePointOccupancy(octreeHandle,1,{state[1],state[2],state[3]})
    if (result == 0) then
        tag = 100 -- Assegna un valore 100 ad ogni punto vuoto
    end
    --if (result==0) then
    pass = (tag == 100) or (tag==250)
    --pass = true
    return pass
end

visualizePath=function(path)
    if not _lineContainer then
        _lineContainer=simAddDrawingObject(sim_drawing_lines,2,0,-1,99999,{0,0,255})
    end
    simAddDrawingObjectItem(_lineContainer,nil)
    if path then
        local pc=#path/7
        for i=1,pc-1,1 do
            --lineDat={path[(i-1)*7+1],path[(i-1)*3+2],path[(i-1)*3+3],path[i*3+5],path[i*3+6],path[i*3+7]}
            lineDat={path[(i-1)*7+1],path[(i-1)*7+2],path[(i-1)*7+3],path[i*7+1],path[i*7+2],path[i*7+3]}
            simAddDrawingObjectItem(_lineContainer,lineDat)
        end
    end
end


--goalpos=simGetObjectPosition(targetHandle,-1)
--goalorient=simGetObjectQuaternion(targetHandle,-1)

insertPathIntoOctree=function(path)
    octreeHandle = simGetObjectHandle('Octree2')
    local table_path = {}
    local pc=#path/7
        for i=0,pc-1,1 do
            newVoxel = {path[(i)*7+1],path[(i)*7+2],path[(i)*7+3]}       
            simInsertVoxelsIntoOctree(octreeHandle,2,newVoxel,{0,0,255},{120}) 
            table_path[i*3+1] = newVoxel[1]
            table_path[i*3+2] = newVoxel[2]
            table_path[i*3+3] = newVoxel[3]
        end
    return table_path
end

deletePathFromOctree=function(path)
    octreeHandle = simGetObjectHandle('Octree2')
    local pc=#path/7
        for i=0,pc-1,1 do
            newVoxel = {path[(i)*7+1],path[(i)*7+2],path[(i)*7+3]}       
            simInsertVoxelsIntoOctree(octreeHandle,2,newVoxel) 
        end

end

followTrajectory=function(path)
    local pc=#path/7
    for i=0,pc-1,1 do
        simSetObjectPosition(robotHandle,-1,{path[(i)*7+1],path[(i)*7+2],path[(i)*7+3]})
        
    end
end





compute_path=function(inInts,inFloats,inStrings,inBuffer)
    goalpose = {inFloats[1],inFloats[2],inFloats[3],0.0,0.0,0.0,1.0}
   
    if (pt==1) then
        t1=simExtOMPL_createTask('t1')
        ss1={simExtOMPL_createStateSpace('6d1',sim_ompl_statespacetype_pose3d,robotHandle,{-5,-5,0},{5,5,5},1)}
        simExtOMPL_setStateSpace(t1,ss1)
        simExtOMPL_setAlgorithm(t1,sim_ompl_algorithm_BiTRRT)
        simExtOMPL_setCollisionPairs(t1,{simGetObjectHandle('Sphere'),sim_handle_all})
        simExtOMPL_setStateValidationCallback(t1,'stateValidation')
        startpos=simGetObjectPosition(robotHandle,-1)
        startorient=simGetObjectQuaternion(robotHandle,-1)
        startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
        simExtOMPL_setStartState(t1,startpose)
        --goalpose={2.0,0.0,1.0,0.0,0.0,0.0,1.0}
        simExtOMPL_setGoalState(t1,goalpose)
        --r,path=simExtOMPL_compute(t,20,-1,200)
        simSetThreadAutomaticSwitch(false)
        r,path=simExtOMPL_compute(t1,20,-1,200)
        simSetThreadAutomaticSwitch(true)
    elseif (pt==2) then
        t2=simExtOMPL_createTask('t2')
        ss2={simExtOMPL_createStateSpace('6d2',sim_ompl_statespacetype_pose3d,robotHandle,{-5,-5,0},{5,5,5},1)}
        print('task2')
        simExtOMPL_setStateSpace(t2,ss2)
        simExtOMPL_setAlgorithm(t2,sim_ompl_algorithm_BiTRRT)
        simExtOMPL_setCollisionPairs(t2,{simGetObjectHandle('Sphere'),sim_handle_all})
        simExtOMPL_setStateValidationCallback(t2,'stateValidation')
        startpos=simGetObjectPosition(robotHandle,-1)
        startorient=simGetObjectQuaternion(robotHandle,-1)
        startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
        simExtOMPL_setStartState(t2,startpose)
        --goalpose={2.0,0.0,1.0,0.0,0.0,0.0,1.0}
        simExtOMPL_setGoalState(t2,goalpose)
        --r,path=simExtOMPL_compute(t,20,-1,200)
        simSetThreadAutomaticSwitch(false)
        r,path=simExtOMPL_compute(t2,20,-1,200)
        simSetThreadAutomaticSwitch(true)
    elseif (pt==3) then
        t3=simExtOMPL_createTask('t3')
        ss3={simExtOMPL_createStateSpace('6d3',sim_ompl_statespacetype_pose3d,robotHandle,{-5,-5,0},{5,5,5},1)}
        simExtOMPL_setStateSpace(t3,ss3)
        simExtOMPL_setAlgorithm(t3,sim_ompl_algorithm_BiTRRT)
        simExtOMPL_setCollisionPairs(t3,{simGetObjectHandle('Sphere'),sim_handle_all})
        simExtOMPL_setStateValidationCallback(t3,'stateValidation')
        startpos=simGetObjectPosition(robotHandle,-1)
        startorient=simGetObjectQuaternion(robotHandle,-1)
        startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
        simExtOMPL_setStartState(t3,startpose)
        --goalpose={2.0,0.0,1.0,0.0,0.0,0.0,1.0}
        simExtOMPL_setGoalState(t3,goalpose)
        --r,path=simExtOMPL_compute(t,20,-1,200)
        simSetThreadAutomaticSwitch(false)
        r,path=simExtOMPL_compute(t3,20,-1,200)
        simSetThreadAutomaticSwitch(true)
    elseif (pt==4) then
        t4=simExtOMPL_createTask('t4')
        ss4={simExtOMPL_createStateSpace('6d4',sim_ompl_statespacetype_pose3d,robotHandle,{-5,-5,0},{5,5,5},1)}
        simExtOMPL_setStateSpace(t4,ss4)
        simExtOMPL_setAlgorithm(t4,sim_ompl_algorithm_BiTRRT)
        simExtOMPL_setCollisionPairs(t4,{simGetObjectHandle('Sphere'),sim_handle_all})
        simExtOMPL_setStateValidationCallback(t4,'stateValidation')
        startpos=simGetObjectPosition(robotHandle,-1)
        startorient=simGetObjectQuaternion(robotHandle,-1)
        startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
        simExtOMPL_setStartState(t4,startpose)
        --goalpose={2.0,0.0,1.0,0.0,0.0,0.0,1.0}
        simExtOMPL_setGoalState(t4,goalpose)
        --r,path=simExtOMPL_compute(t,20,-1,200)
        simSetThreadAutomaticSwitch(false)
        r,path=simExtOMPL_compute(t4,20,-1,200)
        simSetThreadAutomaticSwitch(true)
    end

    --targetHandle=simGetObjectHandle('End')  
    local tpath = insertPathIntoOctree(path)
    visualizePath(path)
    local datamsg = {}
    datamsg=simPackFloats(path)
    datapath=simPackFloats(path)
    --simSetStringSignal('pathstring',datapath)
    --followTrajectory(path)
    --pathmsg={}
    --pathmsg['header']={seq=0,stamp=0,frame_id="a"}
    --pathmsg['pose']={path}

    --datamsg={}
    --datamsg['header']={seq=0,stamp=0, frame_id="a"}
    --datamsg['poses']=pathmsg
    --simExtRosInterface_publish(path_pub,{datamsg})
    
    newGoal = 1
    --while true do
        -- Simply jump through the path points, no interpolation here:
    --simExtOMPL_destroyStateSpace(ss[0])  
    --simExtOMPL_destroyTask(t)

    local pc=#path/7
    for i=0,pc-1,1 do
        simSetObjectPosition(robotHandle,-1,{path[(i)*7+1],path[(i)*7+2],path[(i)*7+3]})       
    end
    
    if(pt==1) then
        simExtOMPL_destroyTask(t1)
    elseif(pt==2) then
        simExtOMPL_destroyTask(t2)
    elseif(pt==3) then
        simExtOMPL_destroyTask(t3)
    elseif(pt==4) then
        simExtOMPL_destroyTask(t4)
    end
    
    pt = pt+1

    return {},{},{}, datamsg
end


if (sim_call_type==sim_childscriptcall_initialization) then
    newGoal = 0
    pt=1
    ptMax=10
    robotHandle=simGetObjectHandle('Sphere')
    octreeHandle = simGetObjectHandle('Octree')
    --path_pub=simExtRosInterface_advertise('/OMPLPath_1','std_msgs/String')
    --simExtRosInterface_publisherTreatUInt8ArrayAsString(path_pub)
    --simExtROS_enablePublisher('/stringPath',1,simros_strmcmd_get_and_clear_string_signal,-1,-1,'pathstring')
    	
end


if (sim_call_type==sim_childscriptcall_actuation) then
   
end


if (sim_call_type==sim_childscriptcall_sensing) then

	
end


if (sim_call_type==sim_childscriptcall_cleanup) then
    --simExtRosInterface_shutdownPublisher(path_pub)
	
end


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

Re: Planning Task from a non-threaded script

Post by coppelia » 05 Jul 2016, 16:24

Can you prepare a minimalistic self-contained scene that illustrates your problem? This way it will be much faster for us to test and corner the problem.

Cheers

Post Reply