simExtOMPL_setValidStateSamplerCallback not working as intended

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

simExtOMPL_setValidStateSamplerCallback not working as intended

Post by e2718 » 17 Jan 2018, 00:36

I believe the function simExtOMPL_setValidStateSamplerCallback is not working as intended. I think the callback functions (args 2 and 3) are not being set and you can put in invalid function names and still get a return code of 1 instead of the failure code of 0. Below is how I've tested this and came to this conclusion.

Have been working on a simple scene to do some 3d path planning with two spheres and a bar in the middle. The goal of this scene is have two modes of path planning. The first is when the sphere runs along the ground, and the other one with the default 3d path planning (so the sphere can float above the step)
download link for the scene is at https://www.dropbox.com/s/z7sgbfpe0zfz1 ... t.ttt?dl=0
Image of the scene is at https://www.dropbox.com/s/8weziqakni9vb ... t.png?dl=0

In the script in start_config, I have the following lines of interest

Code: Select all

if (sim_call_type==sim_childscriptcall_initialization) then

    -- skipping some lines

    ground_samplers = true  -- whether or not to use the custom sampler functions

    -- skipping some lines

    -- set the custom sampling params
    if ground_samplers then
        r = simExtOMPL_setValidStateSamplerCallback(t, 'ground_sampler', 'ground_near_sampler')
        --r = simExtOMPL_setValidStateSamplerCallback(t, 'foo', 'bar')  -- also works with fake function names
        print('----------------------')
        print("ground sampler set with result: " .. r)
        print('----------------------')
    end

    -- skipping some lines

end
When run this scene I get the output:

Code: Select all

----------------------
ground sampler set with result: 1
----------------------
I also get the same print result if I un-comment the line

Code: Select all

        --r = simExtOMPL_setValidStateSamplerCallback(t, 'foo', 'bar')  -- also works with fake function names
Which makes me suspect that the simExtOMPL_setValidStateSamplerCallback function does not actually check to see if the function name arguments are valid and also does not set those functions as the callback ones.

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

Re: simExtOMPL_setValidStateSamplerCallback not working as intended

Post by fferri » 18 Jan 2018, 10:29

The function is working as intended: it will set the specified stateSampler callbacks.
You can use simOMPL.printTaskInfo() to verify that the callbacks are set.

You are expecting for it to validate the given functions names, but this is not meant to happen.
If V-REP in future will have a way for validating those kind of arguments, maybe this will change.

Currently, if you set an incorrect callback name, this will result in an error later, when simCallScriptFunctionEx() is called by the planner, but will not result in an error in setValidStateSamplerCallback().

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

Re: simExtOMPL_setValidStateSamplerCallback not working as intended

Post by e2718 » 19 Jan 2018, 20:37

Ok, have started changed the child script in start_config to use simExtOMPL_printTaskInfo and it seems like something is wrong with the callback and callbackNear variables.

I have a script that calls OMPL 3d path planning with a custom sampler (to sampler all states where z=ground_height). The code that calls simExtOMPL_printTaskInfo is here

Code: Select all

    -- set the custom sampling params
    if ground_samplers then
        r = simExtOMPL_setValidStateSamplerCallback(t, 'ground_sampler', 'ground_near_sampler')
        --r = simExtOMPL_setValidStateSamplerCallback(t, 'foo', 'bar')  -- also works with fake function names
        print('----------------------')
        print("ground sampler set with result: " .. r)
        simExtOMPL_printTaskInfo(t)
        print('----------------------')
    end

The output of this is:

Code: Select all

----------------------
ground sampler set with result: 1
OMPL: task name: task
OMPL: state spaces: (dimension: 7)
OMPL:     state space: 9003
OMPL:         name: 3d
OMPL:         type: sim_ompl_statespacetype_pose3d
OMPL:         object handle: 19
OMPL:         bounds low: {-2, -2, 0.05}
OMPL:         bounds high: {2, 2, 1.05}
OMPL:         default projection: true
OMPL:         weight: 1
OMPL: collision pairs: {17, -2}
OMPL: start state: {}
OMPL: goal:
OMPL:     goal state(s):
OMPL: state validation: default
OMPL: state validity checking resolution: 0.01
OMPL: valid state sampling:
OMPL:     callback:
OMPL:         scriptId: 2050009
OMPL:         function: ground_sampler
OMPL:     callbackNear:
OMPL:         scriptId: 2050009
OMPL:         function: ground_near_sampler
OMPL: projection evaluation: default
OMPL: algorithm: sim_ompl_algorithm_RRTConnect
----------------------
From the task info, it appears the relevant samplers have been set. However, when I call simExtOMPL_solve it appears that it does not call the callback and callbackNear functions specified in the task info (the heights of the states are not ground_height=0.05). The code that calls simExtOMPL_solve is here:

Code: Select all

    -- r, path=simExtOMPL_compute(t, 8, -1, 100)
    simExtOMPL_setup(t)
    if simExtOMPL_solve(t, 8) then
        --simExtOMPL_simplifyPath(t)  -- not simplifying path to test the sampler
        simExtOMPL_interpolatePath(t, 100)
        r,path = simExtOMPL_getPath(t)
    end
 


For reference the entire child script is here:

Code: Select all

-- DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! (unless the code is a function definition)

visualizePath=function(path)
    if not _lineContainer then
        _lineContainer=simAddDrawingObject(sim_drawing_lines,3,0,-1,99999,{0.2,0.2,0.2})
    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)*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

-- given a limits of {min_x, max_x, min_y, max_y, min_z, max_z}
-- picks a random point in the unit box
randomPointBox=function(lower_limits, upper_limits)
    min_x, min_y, min_z = lower_limits
    max_x, max_y, max_z = upper_limits
    x_val = min_x + math.random() * (max_x - min_x)
    y_val = min_y + math.random() * (max_y - min_y)
    z_val = min_z + math.random() * (max_z - min_z)
    return {x_val, y_val, z_val}
end

-- given a center point (x, y), will pick a random point in the circle of given radius
-- algorithm from http://mathworld.wolfram.com/DiskPointPicking.html
randomPointCircle=function(center, radius)
    u = math.random()
    v = math.random() * 2 * math.pi
    x_val = center[1] + (radius * math.sqrt(u) * math.cos(v))
    y_val = center[2] + (radius * math.sqrt(u) * math.sin(v))
    return {x_val, y_val}
end
    
ground_sampler=function()
    print("ground_sampler")
    min_x, min_y, _ = lower_limits
    max_x, max_y, _ = upper_limits
    x_val = min_x + (max_x - min_x) * math.random()
    y_val = min_y + (max_y - min_y) * math.random()
    -- use default orientation of (0, 0, 0, 1)
    return {x_val, y_val, ground_height, 0.0, 0.0, 0.0, 1.0}
end

ground_near_sampler=function(state, distance)
    print("ground_near_sampler")
    min_x, min_y, _ = lower_limits
    max_x, max_y, _ = upper_limits

    sampled_point = randomPointCircle({state[1], state[2]}, distance)
    while sampled_point[1] < min_x or sampled_point[1] > max_x or sampled_point[2] < min_y or sampled_point > max_y do
        sampled_point = randomPointCircle({state[1], state[2]}, distance)
    end
    return {sampled_point[1], sampled_point[2], ground_height, 0.0, 0.0, 0.0, 1.0}
end

if (sim_call_type==sim_childscriptcall_initialization) then

    -- Put some initialization code here

    -- Make sure you read the section on "Accessing general-type objects programmatically"
    -- For instance, if you wish to retrieve the handle of a scene object, use following instruction:
    --
    -- handle=simGetObjectHandle('sceneObjectName')
    -- 
    -- Above instruction retrieves the handle of 'sceneObjectName' if this script's name has no '#' in it
    --
    -- If this script's name contains a '#' (e.g. 'someName#4'), then above instruction retrieves the handle of object 'sceneObjectName#4'
    -- This mechanism of handle retrieval is very convenient, since you don't need to adjust any code when a model is duplicated!
    -- So if the script's name (or rather the name of the object associated with this script) is:
    --
    -- 'someName', then the handle of 'sceneObjectName' is retrieved
    -- 'someName#0', then the handle of 'sceneObjectName#0' is retrieved
    -- 'someName#1', then the handle of 'sceneObjectName#1' is retrieved
    -- ...
    --
    -- If you always want to retrieve the same object's handle, no matter what, specify its full name, including a '#':
    --
    -- handle=simGetObjectHandle('sceneObjectName#') always retrieves the handle of object 'sceneObjectName' 
    -- handle=simGetObjectHandle('sceneObjectName#0') always retrieves the handle of object 'sceneObjectName#0' 
    -- handle=simGetObjectHandle('sceneObjectName#1') always retrieves the handle of object 'sceneObjectName#1'
    -- ...
    --
    -- Refer also to simGetCollisionhandle, simGetDistanceHandle, simGetIkGroupHandle, etc.
    --
    -- Following 2 instructions might also be useful: simGetNameSuffix and simSetNameSuffix

    lower_limits = {-2, -2, 0.05}
    upper_limits = {2, 2, 1.05}
    ground_samplers = true  -- whether or not to use the custom sampler functions
    ground_height = 0.05 -- default ground height

    -- path finding code
    start_handle=simGetObjectHandle('start_config')
    goal_handle=simGetObjectHandle('goal_config')

    t=simExtOMPL_createTask('task')
    ss={simExtOMPL_createStateSpace('3d', sim_ompl_statespacetype_pose3d, start_handle, lower_limits, upper_limits, 1)}
    simExtOMPL_setStateSpace(t, ss)
    simExtOMPL_setAlgorithm(t, sim_ompl_algorithm_RRTConnect)
    collision_pairs={simGetObjectHandle('start'), sim_handle_all}
    simExtOMPL_setCollisionPairs(t, collision_pairs)

    -- set the custom sampling params
    if ground_samplers then
        r = simExtOMPL_setValidStateSamplerCallback(t, 'ground_sampler', 'ground_near_sampler')
        --r = simExtOMPL_setValidStateSamplerCallback(t, 'foo', 'bar')  -- also works with fake function names
        print('----------------------')
        print("ground sampler set with result: " .. r)
        simExtOMPL_printTaskInfo(t)
        print('----------------------')
    end

    start_pos=simGetObjectPosition(start_handle, -1)
    start_orient=simGetObjectQuaternion(start_handle, -1)
    start_pose={start_pos[1], start_pos[2], start_pos[3], start_orient[1], start_orient[2], start_orient[3], start_orient[4]}
    simExtOMPL_setStartState(t, start_pose)

    goal_pos=simGetObjectPosition(goal_handle, -1)
    goal_orient=simGetObjectQuaternion(goal_handle, -1)
    goal_pose={goal_pos[1], goal_pos[2], goal_pos[3], goal_orient[1], goal_orient[2], goal_orient[3], goal_orient[4]}
    simExtOMPL_setGoalState(t, goal_pose)

    -- r, path=simExtOMPL_compute(t, 8, -1, 100)
    simExtOMPL_setup(t)
    if simExtOMPL_solve(t, 8) then
        --simExtOMPL_simplifyPath(t)  -- not simplifying path to test the sampler
        simExtOMPL_interpolatePath(t, 100)
        r,path = simExtOMPL_getPath(t)
    end

    pathLen = #path/7
    pathIndex = 1

    print('----------------------')
    print('path info')
    print('result code: ' .. r)
    print('path length: ' .. (#path/7))

    print('script handle: ' .. (simGetScriptHandle('start_config')))
    print('----------------------')
end


if (sim_call_type==sim_childscriptcall_actuation) then

    -- Put your main ACTUATION code here

    -- For example:
    --
    -- local position=simGetObjectPosition(handle,-1)
    -- position[1]=position[1]+0.001
    -- simSetObjectPosition(handle,-1,position)

    if pathIndex == pathLen - 1 then
        pathIndex = 1
    else
        pathIndex = pathIndex + 1
    end

    i = pathIndex * 7 + 1
    pos={path[i],path[i+1],path[i+2]}
    orient={path[i+3],path[i+4],path[i+5],path[i+6]}
    simSetObjectPosition(start_handle,-1,pos)
    simSetObjectQuaternion(start_handle,-1,orient)
    visualizePath(path)
end


if (sim_call_type==sim_childscriptcall_sensing) then

    -- Put your main SENSING code here

end


if (sim_call_type==sim_childscriptcall_cleanup) then

    -- Put some restoration code here

end


end
Last edited by e2718 on 19 Jan 2018, 22:54, edited 2 times in total.

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

Re: simExtOMPL_setValidStateSamplerCallback not working as intended

Post by e2718 » 19 Jan 2018, 20:43

Forgot to mention this, but also have put in print statements into the sampler calls, and it appears that the functions are not being called (none of the print statements appear in the console).

Code for the sampler functions

Code: Select all

ground_sampler=function()
    print("ground_sampler called")
    min_x, min_y, _ = lower_limits
    max_x, max_y, _ = upper_limits
    x_val = min_x + (max_x - min_x) * math.random()
    y_val = min_y + (max_y - min_y) * math.random()
    -- use default orientation of (0, 0, 0, 1)
    return {x_val, y_val, ground_height, 0.0, 0.0, 0.0, 1.0}
end

ground_near_sampler=function(state, distance)
    print("ground_near_sampler called")
    min_x, min_y, _ = lower_limits
    max_x, max_y, _ = upper_limits

    sampled_point = randomPointCircle({state[1], state[2]}, distance)
    while sampled_point[1] < min_x or sampled_point[1] > max_x or sampled_point[2] < min_y or sampled_point > max_y do
        sampled_point = randomPointCircle({state[1], state[2]}, distance)
    end
    return {sampled_point[1], sampled_point[2], ground_height, 0.0, 0.0, 0.0, 1.0}
end

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

Re: simExtOMPL_setValidStateSamplerCallback not working as intended

Post by e2718 » 25 Jan 2018, 01:19

Bumping this thread because no reply in 5 days to the update on the bug

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

Re: simExtOMPL_setValidStateSamplerCallback not working as intended

Post by fferri » 29 Jan 2018, 17:45

I'm not an expert with OMPL, but even with the StateSampling demo of OMPL, using RRTConnect does not call the valid state sampler, so you might have to choose an appropriate algorithm (the demo uses PRM).

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

Re: simExtOMPL_setValidStateSamplerCallback not working as intended

Post by coppelia » 29 Jan 2018, 20:27

additionally, if you are running on Windows, it might be not working in any case. Try with the latest V3.5.0 beta.

Cheers

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

Re: simExtOMPL_setValidStateSamplerCallback not working as intended

Post by e2718 » 07 Feb 2018, 20:40

Update:

This is a bit late, but thanks for the help. Turns out that the path planners do not call the same random samplers. Only some of them will call the custom sampler algorithms.

Post Reply