sim_ompl_algorithm_CForest path planning algorithm crashes V-REP

Report crashes, strange behaviour, or apparent bugs
Post Reply
e2718
Posts: 33
Joined: 15 Nov 2015, 05:39

sim_ompl_algorithm_CForest path planning algorithm crashes V-REP

Post by e2718 »

For some of the possible algorithms you can set in simExtOMPL_setAlgorithm, you can crash V-REP. For example, if you look at the 6DoFHolonomicPathPlanning example and you set the algorithm to sim_ompl_algorithm_CForest. V-REP will crash with these logs

Code: Select all

Info:    CForest: Space information setup was not yet called. Calling now.
Info:    CForest: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.
Info:    CForest: Number and type of instances not specified. Defaulting to 4 instances of RRTstar.
Debug:   The value of parameter 'focus_search' is now: '1'
Debug:   The value of parameter 'focus_search' is now: '1'
Debug:   The value of parameter 'focus_search' is now: '1'
Debug:   The value of parameter 'focus_search' is now: '1'
Info:    RRTstar: Space information setup was not yet called. Calling now.
Debug:   RRTstar: Planner range detected to be 0.804057
Info:    RRTstar: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.
Info:    RRTstar: Space information setup was not yet called. Calling now.
Debug:   RRTstar: Planner range detected to be 0.804057
Info:    RRTstar: Space information setup was not yet called. Calling now.
Debug:   RRTstar: Planner range detected to be 0.804057
Info:    RRTstar: Space information setup was not yet called. Calling now.
Debug:   RRTstar: Planner range detected to be 0.804057
Debug:   Starting RRTstar
Debug:   Starting RRTstar
Debug:   Starting RRTstar
Debug:   Starting RRTstar
Info:    RRTstar: Using informed sampling.
libc++abi.dylib: terminating with uncaught exception of type ompl::Exception: Direct sampling of the path-length objective requires Eigen, but this version of OMPL was compiled without Eigen support. If possible, please install Eigen and recompile OMPL. If this is not possible, you can manually create an instantiation of RejectionInfSampler to approximate the behaviour of direct informed sampling.
For conenience, the code with sim_ompl_algorithm_CForest is below.

Code: Select all

robotHandle=simGetObjectHandle('Start')
targetHandle=simGetObjectHandle('End')
t=simExtOMPL_createTask('t')
ss={simExtOMPL_createStateSpace('6d',sim_ompl_statespacetype_pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}
simExtOMPL_setStateSpace(t,ss)
simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_CForest)
simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('L_start'),sim_handle_all})
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(t,startpose)
goalpos=simGetObjectPosition(targetHandle,-1)
goalorient=simGetObjectQuaternion(targetHandle,-1)
goalpose={goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}
simExtOMPL_setGoalState(t,goalpose)
r,path=simExtOMPL_compute(t,20,-1,200)

while true do
    -- Simply jump through the path points, no interpolation here:
    for i=1,#path-7,7 do
        pos={path[i],path[i+1],path[i+2]}
        orient={path[i+3],path[i+4],path[i+5],path[i+6]}
        simSetObjectPosition(robotHandle,-1,pos)
        simSetObjectQuaternion(robotHandle,-1,orient)
        simSwitchThread()
    end
end

Additionally, not all of the OMPL algorithms that require the Eigen package will crash V-REP. When using the algorithm sim_ompl_algorithm_BITstar. You get the following output while V-REP will throw an exception but not crash.

Code: Select all

Info:    BiTRRT: Space information setup was not yet called. Calling now.
Debug:   BiTRRT: Planner range detected to be 0.804057
Info:    BiTRRT: No optimization objective specified.  Defaulting to mechanical work minimization.
Debug:   BiTRRT: Frontier threshold detected to be 0.040203
Info:    BiTRRT: Planning started with 2 states already in datastructure
Info:    BiTRRT: Created 838 states (440 start + 398 goal)
Initializing the Bullet physics engine in plugin 'DynamicsBullet_2_78'...
Engine version: 2.78
Plugin version: 9
Initialization successful.
Info:    BITstar: Space information setup was not yet called. Calling now.
Info:    BITstar: No optimization objective specified. Defaulting to optimizing path length.
OMPL: exception: Direct sampling of the path-length objective requires Eigen, but this version of OMPL was compiled without Eigen support. If possible, please install Eigen and recompile OMPL. If this is not possible, you can manually create an instantiation of RejectionInfSampler to approximate the behaviour of direct informed sampling.
For conenience, the code with sim_ompl_algorithm_BITstar is below.

Code: Select all


robotHandle=simGetObjectHandle('Start')
targetHandle=simGetObjectHandle('End')
t=simExtOMPL_createTask('t')
ss={simExtOMPL_createStateSpace('6d',sim_ompl_statespacetype_pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}
simExtOMPL_setStateSpace(t,ss)
simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_BITstar)
simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('L_start'),sim_handle_all})
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(t,startpose)
goalpos=simGetObjectPosition(targetHandle,-1)
goalorient=simGetObjectQuaternion(targetHandle,-1)
goalpose={goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}
simExtOMPL_setGoalState(t,goalpose)
r,path=simExtOMPL_compute(t,20,-1,200)

while true do
    -- Simply jump through the path points, no interpolation here:
    for i=1,#path-7,7 do
        pos={path[i],path[i+1],path[i+2]}
        orient={path[i+3],path[i+4],path[i+5],path[i+6]}
        simSetObjectPosition(robotHandle,-1,pos)
        simSetObjectQuaternion(robotHandle,-1,orient)
        simSwitchThread()
    end
end
Think the easiest fix would to be to recompile OMPL when Eigen is installed.

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

Re: sim_ompl_algorithm_CForest path planning algorithm crashes V-REP

Post by fferri »

Technically, that's not a crash but an uncaught exception.
Exceptions thrown in the main thread are automatically caught by V-REP plugins (well, most of them, like OMPL, CustomUI, and a few others) and reported as Lua errors.
If exceptions are thrown in other threads created by third party libraries, it's not possible to catch those. This may be one of those cases. I'm afraid of that.

e2718
Posts: 33
Joined: 15 Nov 2015, 05:39

Re: sim_ompl_algorithm_CForest path planning algorithm crashes V-REP

Post by e2718 »

From the error messages it seems like recompiling OMPL with Eigen should work. I don't know do this exactly though so have not tried it.

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

Re: sim_ompl_algorithm_CForest path planning algorithm crashes V-REP

Post by coppelia »

Hello,

it seems that Eigen was forgotten on some platform. Thanks for mentioning this. They have been added for the last release (V3.5.0). The problem however still remains. So it must also be something different.

Cheers

Post Reply