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
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
-- 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