OMPL Plugin API reference

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

simOMPL.addGoalState

Description Add a goal state, without clearing previously set goal state(s), if any.
Lua synopsis simOMPL.addGoalState(int taskHandle, table state)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
state (table of float): a table of numbers, whose size must be consistent with the robot's state space specified in this task object
Lua return values -
See also

simOMPL.compute

Description Use OMPL to find a solution for this motion planning task. It is equivalent to executing: simOMPL.setup(task) if simOMPL.solve(task, maxTime) then simOMPL.simplifyPath(task, maxSimplificationTime) simOMPL.interpolatePath(task, stateCnt) path = simOMPL.getPath(task) end
Lua synopsis bool solved, table states=simOMPL.compute(int taskHandle, float maxTime, float maxSimplificationTime=-1.0, int stateCnt=0)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
maxTime (float): maximum time used for the path searching procedure, in seconds.
maxSimplificationTime (float, default: -1.0): (optional) maximum time used for the path simplification procedure, in seconds. -1 for a default simplification procedure.
stateCnt (int, default: 0): (optional) minimum number of states to be returned. 0 for a default behaviour.
Lua return values
solved (bool): true if a solution has been found.
states (table of float): a table of states, representing the solution, from start to goal. States are specified linearly.
See also

simOMPL.createStateSpace

Description Create a component of the state space for the motion planning problem. In case of a dubins state space, set additional parameters with simOMPL.setDubinsParams
Lua synopsis int stateSpaceHandle=simOMPL.createStateSpace(string name, int type, int objectHandle, table boundsLow, table boundsHigh, int useForProjection, float weight=1.0, int refObjectHandle=-1)
Lua parameters
name (string): a name for this state space
type (int): type of this state space component (see simOMPL.StateSpaceType)
objectHandle (int): the object handle (a joint object if type is simOMPL.StateSpaceType.joint_position, otherwise a shape)
boundsLow (table of float): lower bounds (if type is pose, specify only the 3 position components)
boundsHigh (table of float): upper bounds (if type is pose, specify only the 3 position components)
useForProjection (int): if true, this object position or joint value will be used for computing a default projection
weight (float, default: 1.0): (optional) the weight of this state space component, used for computing distance between states. Default value is 1.0
refObjectHandle (int, default: -1): (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 (int): a handle to the created state space component
See also

simOMPL.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 int taskHandle=simOMPL.createTask(string name)
Lua parameters
name (string): a name for this task object
Lua return values
taskHandle (int): a handle to the created task object
See also

simOMPL.destroyStateSpace

Description Destroy the spacified state space component.Note: state space components created during simulation are automatically destroyed when simulation ends.
Lua synopsis simOMPL.destroyStateSpace(int stateSpaceHandle)
Lua parameters
stateSpaceHandle (int): handle to state space component
Lua return values -
See also

simOMPL.destroyTask

Description Destroy the specified task object.Note: task objects created during simulation are automatically destroyed when simulation ends.
Lua synopsis simOMPL.destroyTask(int taskHandle)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
Lua return values -
See also

simOMPL.getData

Description Get planner data for this motion planning task.
Lua synopsis table states=simOMPL.getData(int taskHandle)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask.
Lua return values
states (table of float): a table of states, representing the configurations in the constructed search graph. States are specified linearly followed by minimum distance to the closest X_obs.
See also

simOMPL.getPath

Description Use OMPL to find a solution for this motion planning task.
Lua synopsis table states=simOMPL.getPath(int taskHandle)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
Lua return values
states (table of float): a table of states, representing the solution, from start to goal. States are specified linearly.
See also

simOMPL.interpolatePath

Description Interpolate the path found by planner to obtain a minimum number of states.
Lua synopsis simOMPL.interpolatePath(int taskHandle, int stateCnt=0)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
stateCnt (int, default: 0): (optional) minimum number of states to be returned. 0 for a default behaviour.
Lua return values -
See also

simOMPL.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 int valid=simOMPL.isStateValid(int taskHandle, table state)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
state (table of float): state vector
Lua return values
valid (int): 1 if valid, 0 otherwise
See also

simOMPL.printTaskInfo

Description Print a summary of the specified task object. Useful for debugging and submitting bug reports.
Lua synopsis simOMPL.printTaskInfo(int taskHandle)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
Lua return values -
See also

simOMPL.readState

Description Read a state vector from current simulator state.
Lua synopsis table state=simOMPL.readState(int taskHandle)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
Lua return values
state (table of float): state vector
See also

simOMPL.setAlgorithm

Description Set the search algorithm for the specified task. Default algorithm used is KPIECE1.
Lua synopsis simOMPL.setAlgorithm(int taskHandle, int algorithm)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
algorithm (int): see simOMPL.Algorithm
Lua return values -
See also

simOMPL.setCollisionPairs

Description Set the collision pairs for the specified task object.
Lua synopsis simOMPL.setCollisionPairs(int taskHandle, table collisionPairHandles)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
collisionPairHandles (table of int): 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 -
See also

simOMPL.setDubinsParams

Description Set extra state space parameters of a dubins state space.
Lua synopsis simOMPL.setDubinsParams(int stateSpaceHandle, float turningRadius, bool isSymmetric)
Lua parameters
stateSpaceHandle (int): handle to state space component
turningRadius (float): turning radius
isSymmetric (bool): true if it is symmetric, otherwise false
Lua return values -
See also

simOMPL.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 simOMPL.setGoal(int taskHandle, int robotDummy, int goalDummy, float tolerance=0.001f, table metric={1.0, 1.0, 1.0, 0.1}, int refDummy=-1)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
robotDummy (int): a dummy attached to the robot
goalDummy (int): a dummy fixed in the environment, representing the goal pose/position
tolerance (float, default: 0.001f): an optional tolerated dummy-dummy distance. Defaults to 0.001
metric (table of float, default: {1.0, 1.0, 1.0, 0.1}): an optional metric (x,y,z,angle) used to evaluate the dummy-dummy distance
refDummy (int, default: -1): an optional reference dummy, relative to which the metric will be used
Lua return values -
See also

simOMPL.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 simOMPL.setGoalCallback(int taskHandle, string callback)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
callback (string): name of the Lua callback
Lua return values -
See also

simOMPL.setGoalState

Description Set the goal state for the specified task object.
Lua synopsis simOMPL.setGoalState(int taskHandle, table state)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
state (table of float): a table of numbers, whose size must be consistent with the robot's state space specified in this task object
Lua return values -
See also

simOMPL.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 simOMPL.setProjectionEvaluationCallback(int taskHandle, string callback, int projectionSize)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
callback (string): name of the Lua callback
projectionSize (int): size of the projection (usually 2 or 3)
Lua return values -
See also

simOMPL.setStartState

Description Set the start state for the specified task object.
Lua synopsis simOMPL.setStartState(int taskHandle, table state)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
state (table of float): a table of numbers, whose size must be consistent with the robot's state space specified in this task object
Lua return values -
See also

simOMPL.setStateSpace

Description Set the state space of this task object.
Lua synopsis simOMPL.setStateSpace(int taskHandle, table stateSpaceHandles)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
stateSpaceHandles (table of int): a table of handles to state space components, created with simOMPL.createStateSpace
Lua return values -
See also

simOMPL.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 simOMPL.setStateValidationCallback(int taskHandle, string callback)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
callback (string): name of the Lua callback
Lua return values -
See also

simOMPL.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 simOMPL.setStateValidityCheckingResolution(int taskHandle, float resolution)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
resolution (float): resolution of state validity checking, expressed as fraction of state space's extent
Lua return values -
See also

simOMPL.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 simOMPL.setValidStateSamplerCallback(int taskHandle, string callback, string callbackNear)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
callback (string): the name of the Lua callback for sampling a state
callbackNear (string): the name of the Lua callback for sampling near a given state within the given distance
Lua return values -
See also

simOMPL.setVerboseLevel

Description Set the verbosity level for messages printed to application console.
Lua synopsis simOMPL.setVerboseLevel(int taskHandle, int verboseLevel)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
verboseLevel (int): level of verbosity (positive integer), 0 to suppress any message
Lua return values -
See also

simOMPL.setup

Description Setup the OMPL classes with the information contained in the task.
Lua synopsis simOMPL.setup(int taskHandle)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
Lua return values -
See also

simOMPL.simplifyPath

Description Simplify the path found by planner.
Lua synopsis simOMPL.simplifyPath(int taskHandle, float maxSimplificationTime=-1.0)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
maxSimplificationTime (float, default: -1.0): (optional) maximum time used for the path simplification procedure, in seconds. -1 for a default simplification procedure.
Lua return values -
See also

simOMPL.solve

Description Run the planning algorithm to search for a solution.
Lua synopsis bool solved=simOMPL.solve(int taskHandle, float maxTime)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
maxTime (float): maximum time used for the path searching procedure, in seconds.
Lua return values
solved (bool): true if a solution is found.
See also

simOMPL.writeState

Description Write the specified state to simulator
Lua synopsis simOMPL.writeState(int taskHandle, table state)
Lua parameters
taskHandle (int): a handle to a task object created with simOMPL.createTask
state (table of float): state vector
Lua return values -
See also



Constants

Constants used in the various functions. Refer to each constant using enumName.constantName, i.e. simUI.curve_type.xy for xy constant in simUI.curve_type enum.

simOMPL.Algorithm

BiTRRT
BITstar
BKPIECE1
CForest
EST
FMT
KPIECE1
LazyPRM
LazyPRMstar
LazyRRT
LBKPIECE1
LBTRRT
PDST
PRM
PRMstar
pRRT
pSBL
RRT
RRTConnect
RRTstar
SBL
SPARS
SPARStwo
STRIDE
TRRT

simOMPL.StateSpaceType

position2d
pose2d
position3d
pose3d
joint_position
dubins


Script functions

Script functions are used to call some lua code from the plugin side (tipically used for event handlers).

goalCallback

Description Callback for checking if the goal is satisfied.
Lua synopsis bool satisfied, float distance=simOMPL.goalCallback(table state)
Lua parameters
state (table of float): the state to test for goal satisfaction
Lua return values
satisfied (bool): true if satisfied, false otherwise
distance (float): distance to goal, if it is known. A constant value can be returned otherwise, but the performance of the algorithm will be worse.
See also

projectionEvaluationCallback

Description Callback for computing a (euclidean) projection of states.
Lua synopsis table projection=simOMPL.projectionEvaluationCallback(table state)
Lua parameters
state (table of float): the state to compute the projection for
Lua return values
projection (table of float): projected state, usualy of size 2 or 3, representing a point in plane or space.
See also

stateValidationCallback

Description Callback for checking the validity of states.
Lua synopsis bool valid=simOMPL.stateValidationCallback(table state)
Lua parameters
state (table of float): the state to compute the projection for
Lua return values
valid (bool): true if valid, false otherwise
See also

validStateSamplerCallback

Description Callback for sampling valid states from the state space.
Lua synopsis table sampledState=simOMPL.validStateSamplerCallback()
Lua parameters -
Lua return values
sampledState (table of float): a valid state
See also

validStateSamplerCallbackNear

Description Callback for sampling valid states in the proximity of a given state, within a certain distance.
Lua synopsis table sampledState=simOMPL.validStateSamplerCallbackNear(table state, float distance)
Lua parameters
state (table of float): the center state
distance (float): distance bound
Lua return values
sampledState (table of float): a valid state
See also