OMPL Plugin API reference

The list of API functions below allows you to define and solve a motion planning problem with OMPL.

simExtOMPL_addGoalState

Description Add a goal state, without clearing previously set goal state(s), if any.
Lua synopsis number result=simExtOMPL_addGoalState(number taskHandle, table state)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
state: a table of numbers, whose size must be consistent with the robot's state space specified in this task object
Lua return values
result: 0 for failure

simExtOMPL_compute

Description Use OMPL to find a solution for this motion planning task. It is equivalent to executing:

simExtOMPL_setup(task)
if simExtOMPL_solve(task, maxTime) then
    simExtOMPL_simplifyPath(task, maxSimplificationTime)
    simExtOMPL_interpolatePath(task, stateCnt)
    result,path = simExtOMPL_getPath(task)
end

Lua synopsis number result, table states=simExtOMPL_compute(number taskHandle, number maxTime, number maxSimplificationTime = -1.0, number stateCnt = 0)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
maxTime: maximum time used for the path searching procedure, in seconds.
maxSimplificationTime: (optional) maximum time used for the path simplification procedure, in seconds. -1 for a default simplification procedure.
stateCnt: (optional) minimum number of states to be returned. 0 for a default behaviour.
Lua return values
result: 0 for failure
states: a table of states, representing the solution, from start to goal. States are specified linearly.

simExtOMPL_createStateSpace

Description Create a component of the state space for the motion planning problem.
Lua synopsis number stateSpaceHandle=simExtOMPL_createStateSpace(string name, number type, number objectHandle, table boundsLow, table boundsHigh, number useForProjection, number weight = 1.0, number refObjectHandle = -1)
Lua parameters
name: a name for this state space
type: type of this state space component (must be one of sim_ompl_statespacetype_position2d, sim_ompl_statespacetype_pose2d, sim_ompl_statespacetype_position3d, sim_ompl_statespacetype_pose3d, sim_ompl_statespacetype_joint_position)
objectHandle: the object handle (a joint object if type is sim_ompl_statespacetype_joint_position, otherwise a shape)
boundsLow: lower bounds (if type is pose, specify only the 3 position components)
boundsHigh: upper bounds (if type is pose, specify only the 3 position components)
useForProjection: if true, this object position or joint value will be used for computing a default projection
weight: (optional) the weight of this state space component, used for computing distance between states. Default value is 1.0
refObjectHandle: (optional) an object handle relative to which reference frame position/orientations will be evaluated. Default value is -1, for the absolute reference frame
Lua return values
stateSpaceHandle: a handle to the created state space component

simExtOMPL_createTask

Description Create a task object, used to represent the motion planning task. A task object contains informations about:
  • collision pairs (used by the default state validity checker)
  • state spaces
  • start state
  • goal state, or goal specification (e.g. pair of dummies, Lua callback, ...)
  • various Lua callbacks (projection evaluation, state validation, goal satisfaction)

Lua synopsis number taskHandle=simExtOMPL_createTask(string name)
Lua parameters
name: a name for this task object
Lua return values
taskHandle: a handle to the created task object

simExtOMPL_destroyStateSpace

Description Destroy the spacified state space component.

Note: state space components created during simulation are automatically destroyed when simulation ends.
Lua synopsis number result=simExtOMPL_destroyStateSpace(number stateSpaceHandle)
Lua parameters
stateSpaceHandle: handle to state space component
Lua return values
result: 0 for failure

simExtOMPL_destroyTask

Description Destroy the specified task object.

Note: task objects created during simulation are automatically destroyed when simulation ends.
Lua synopsis number result=simExtOMPL_destroyTask(number taskHandle)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
Lua return values
result: 0 for failure

simExtOMPL_getPath

Description Use OMPL to find a solution for this motion planning task.
Lua synopsis number result, table states=simExtOMPL_getPath(number taskHandle)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
Lua return values
result: 0 for failure
states: a table of states, representing the solution, from start to goal. States are specified linearly.

simExtOMPL_interpolatePath

Description Interpolate the path found by planner to obtain a minimum number of states.
Lua synopsis number result=simExtOMPL_interpolatePath(number taskHandle, number stateCnt = 0)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
stateCnt: (optional) minimum number of states to be returned. 0 for a default behaviour.
Lua return values
result: 0 for failure

simExtOMPL_isStateValid

Description Check if the specified state is valid. If a state validation callback has been specified, that will be used to determine the validity of the state, otherwise the default state validation method will be used.
Lua synopsis number valid=simExtOMPL_isStateValid(number taskHandle, table state)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
state: state vector
Lua return values
valid: 1 if valid, 0 otherwise

simExtOMPL_printTaskInfo

Description Print a summary of the specified task object. Useful for debugging and submitting bug reports.
Lua synopsis number result=simExtOMPL_printTaskInfo(number taskHandle)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
Lua return values
result: 0 for failure

simExtOMPL_readState

Description Read a state vector from current simulator state.
Lua synopsis number result, table state=simExtOMPL_readState(number taskHandle)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
Lua return values
result: 1 in case of success
state: state vector

simExtOMPL_setAlgorithm

Description Set the search algorithm for the specified task. Default algorithm used is KPIECE1.
Lua synopsis number result=simExtOMPL_setAlgorithm(number taskHandle, number algorithm)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
algorithm: one of sim_ompl_algorithm_BiTRRT, sim_ompl_algorithm_BITstar, sim_ompl_algorithm_BKPIECE1, sim_ompl_algorithm_CForest, sim_ompl_algorithm_EST, sim_ompl_algorithm_FMT, sim_ompl_algorithm_KPIECE1, sim_ompl_algorithm_LazyPRM, sim_ompl_algorithm_LazyPRMstar, sim_ompl_algorithm_LazyRRT, sim_ompl_algorithm_LBKPIECE1, sim_ompl_algorithm_LBTRRT, sim_ompl_algorithm_PDST, sim_ompl_algorithm_PRM, sim_ompl_algorithm_PRMstar, sim_ompl_algorithm_pRRT, sim_ompl_algorithm_pSBL, sim_ompl_algorithm_RRT, sim_ompl_algorithm_RRTConnect, sim_ompl_algorithm_RRTstar, sim_ompl_algorithm_SBL, sim_ompl_algorithm_SPARS, sim_ompl_algorithm_SPARStwo, sim_ompl_algorithm_STRIDE, sim_ompl_algorithm_TRRT
Lua return values
result: 0 for failure

simExtOMPL_setCollisionPairs

Description Set the collision pairs for the specified task object.
Lua synopsis number result=simExtOMPL_setCollisionPairs(number taskHandle, table collisionPairHandles)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
collisionPairHandles: a table containing 2 entity handles for each collision pair. A collision pair is represented by a collider and a collidee, that will be tested against each other. The first pair could be used for robot self-collision testing, and a second pair could be used for robot-environment collision testing. The collider can be an object or a collection handle. The collidee can be an object or collection handle, or sim_handle_all, in which case the collider will be checked agains all other collidable objects in the scene.
Lua return values
result: 0 for failure

simExtOMPL_setGoal

Description Set the goal for the specificed task object by a dummy pair. One of the two dummies is part of the robot. The other dummy is fixed in the environment. When the task is solved, the position or pose of the two dummies will (approximatively) be the same. Dummy-dummy distances are relative to an optional reference dummy, and are evaluated using an optional metric
Lua synopsis number result=simExtOMPL_setGoal(number taskHandle, number robotDummy, number goalDummy, number tolerance = 0.001f, table metric = {1.0, 1.0, 1.0, 0.1}, number refDummy = -1)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
robotDummy: a dummy attached to the robot
goalDummy: a dummy fixed in the environment, representing the goal pose/position
tolerance: an optional tolerated dummy-dummy distance. Defaults to 0.001
metric: an optional metric (x,y,z,angle) used to evaluate the dummy-dummy distance
refDummy: an optional reference dummy, relative to which the metric will be used
Lua return values
result: 0 for failure

simExtOMPL_setGoalCallback

Description Set a custom goal callback for the specified task. The argument passed to the callback is the state to test for goal satisfaction. The return values must be a boolean indicating wether the goal is satisfied, and a float indicating the distance to the goal, i.e.:

boolean satisfied, number distance=goalSatisfied(table state)

If a distance to the goal is not known, a constant value can be used, but the performance of the algorithm will be worse.
Lua synopsis number result=simExtOMPL_setGoalCallback(number taskHandle, string callback)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
callback: name of the Lua callback
Lua return values
result: 0 for failure

simExtOMPL_setGoalState

Description Set the goal state for the specified task object.
Lua synopsis number result=simExtOMPL_setGoalState(number taskHandle, table state)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
state: a table of numbers, whose size must be consistent with the robot's state space specified in this task object
Lua return values
result: 0 for failure

simExtOMPL_setProjectionEvaluationCallback

Description Set a custom projection evaluation. The argument of the callback will be a state, and the return value must be a table of numbers, with a size equal to the projectionSize argument, i.e.

table projection=evaluateProjection(table state)
Lua synopsis number result=simExtOMPL_setProjectionEvaluationCallback(number taskHandle, string callback, number projectionSize)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
callback: name of the Lua callback
projectionSize: size of the projection (usually 2 or 3)
Lua return values
result: 0 for failure

simExtOMPL_setStartState

Description Set the start state for the specified task object.
Lua synopsis number result=simExtOMPL_setStartState(number taskHandle, table state)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
state: a table of numbers, whose size must be consistent with the robot's state space specified in this task object
Lua return values
result: 0 for failure

simExtOMPL_setStateSpace

Description Set the state space of this task object.
Lua synopsis number result=simExtOMPL_setStateSpace(number taskHandle, table stateSpaceHandles)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
stateSpaceHandles: a table of handles to state space components, created with simExtOMPL_createStateSpace
Lua return values
result: 0 for failure

simExtOMPL_setStateValidationCallback

Description Set a custom state validation. By default state validation is performed by collision checking, between robot's collision objects and environment's objects. By specifying a custom state validation, it is possible to perform any arbitrary check on a state to determine wether it is valid or not. Argument to the callback is the state to validate, and return value must be a boolean indicating the validity of the state, i.e.:

boolean valid=stateValidator(table state)
Lua synopsis number result=simExtOMPL_setStateValidationCallback(number taskHandle, string callback)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
callback: name of the Lua callback
Lua return values
result: 0 for failure

simExtOMPL_setStateValidityCheckingResolution

Description Set the resolution of state validity checking, expressed as fraction of state space's extent. Default resolution is 0.01 which is 1% fs the state space's extent.
Lua synopsis number result=simExtOMPL_setStateValidityCheckingResolution(number taskHandle, number resolution)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
resolution: resolution of state validity checking, expressed as fraction of state space's extent
Lua return values
result: 0 for failure

simExtOMPL_setValidStateSamplerCallback

Description The valid state sampler callbacks must generate valid states. There are two callbacks to implement:
  • the valid state sampling callback: table sampledState=sample()
  • the near valid state sampling callback: table sampledState=sampleNear(table state, number distance)

Lua synopsis number result=simExtOMPL_setValidStateSamplerCallback(number taskHandle, string callback, string callbackNear)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
callback: the name of the Lua callback for sampling a state
callbackNear: the name of the Lua callback for sampling near a given state within the given distance
Lua return values
result: 0 for failure

simExtOMPL_setVerboseLevel

Description Set the verbosity level for messages printed to application console.
Lua synopsis number result=simExtOMPL_setVerboseLevel(number taskHandle, number verboseLevel)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
verboseLevel: level of verbosity (positive integer), 0 to suppress any message
Lua return values
result: 0 for failure

simExtOMPL_setup

Description Setup the OMPL classes with the information contained in the task.
Lua synopsis number result=simExtOMPL_setup(number taskHandle)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
Lua return values
result: 0 for failure

simExtOMPL_simplifyPath

Description Simplify the path found by planner.
Lua synopsis number result=simExtOMPL_simplifyPath(number taskHandle, number maxSimplificationTime = -1.0)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
maxSimplificationTime: (optional) maximum time used for the path simplification procedure, in seconds. -1 for a default simplification procedure.
Lua return values
result: 0 for failure

simExtOMPL_solve

Description Run the planning algorithm to search for a solution.
Lua synopsis number result=simExtOMPL_solve(number taskHandle, number maxTime)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
maxTime: maximum time used for the path searching procedure, in seconds.
Lua return values
result: 0 for failure

simExtOMPL_writeState

Description Write the specified state to simulator
Lua synopsis number result=simExtOMPL_writeState(number taskHandle, table state)
Lua parameters
taskHandle: a handle to a task object created with simExtOMPL_createTask
state: state vector
Lua return values
result: 0 for failure



Constants

Algorithm

sim_ompl_algorithm_BiTRRT
sim_ompl_algorithm_BITstar
sim_ompl_algorithm_BKPIECE1
sim_ompl_algorithm_CForest
sim_ompl_algorithm_EST
sim_ompl_algorithm_FMT
sim_ompl_algorithm_KPIECE1
sim_ompl_algorithm_LazyPRM
sim_ompl_algorithm_LazyPRMstar
sim_ompl_algorithm_LazyRRT
sim_ompl_algorithm_LBKPIECE1
sim_ompl_algorithm_LBTRRT
sim_ompl_algorithm_PDST
sim_ompl_algorithm_PRM
sim_ompl_algorithm_PRMstar
sim_ompl_algorithm_pRRT
sim_ompl_algorithm_pSBL
sim_ompl_algorithm_RRT
sim_ompl_algorithm_RRTConnect
sim_ompl_algorithm_RRTstar
sim_ompl_algorithm_SBL
sim_ompl_algorithm_SPARS
sim_ompl_algorithm_SPARStwo
sim_ompl_algorithm_STRIDE
sim_ompl_algorithm_TRRT

StateSpaceType

sim_ompl_statespacetype_position2d
sim_ompl_statespacetype_pose2d
sim_ompl_statespacetype_position3d
sim_ompl_statespacetype_pose3d
sim_ompl_statespacetype_joint_position