simOMPL Plugin API reference

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

simOMPL.addGoalState
simOMPL.compute
simOMPL.createStateSpace
simOMPL.createStateSpaceForJoint
simOMPL.createTask
simOMPL.destroyStateSpace
simOMPL.destroyTask
simOMPL.drawPath
simOMPL.drawPlannerData
simOMPL.enforceBounds
simOMPL.getGoalDistance
simOMPL.getPath
simOMPL.getPathState
simOMPL.getPathStateCount
simOMPL.getPlannerData
simOMPL.getProjectedPathLength
simOMPL.getReversedPath
simOMPL.getStateSpaceDimension
simOMPL.hasApproximateSolution
simOMPL.hasExactSolution
simOMPL.hasSolution
simOMPL.interpolatePath
simOMPL.isStateValid
simOMPL.isStateWithinBounds
simOMPL.printTaskInfo
simOMPL.projectStates
simOMPL.projectionSize
simOMPL.readState
simOMPL.removeDrawingObjects
simOMPL.setAlgorithm
simOMPL.setCollisionPairs
simOMPL.setDubinsParams
simOMPL.setGoal
simOMPL.setGoalCallback
simOMPL.setGoalState
simOMPL.setGoalStates
simOMPL.setProjectionEvaluationCallback
simOMPL.setStartState
simOMPL.setStateSpace
simOMPL.setStateSpaceForJoints
simOMPL.setStateValidationCallback
simOMPL.setStateValidityCheckingResolution
simOMPL.setValidStateSamplerCallback
simOMPL.setVerboseLevel
simOMPL.setup
simOMPL.simplifyPath
simOMPL.solve
simOMPL.stateDistance
simOMPL.writeState

simOMPL.addGoalState

Description Add a goal state, without clearing previously set goal state(s), if any.
Lua synopsis simOMPL.addGoalState(string taskHandle, float[] state)
Lua parameters
taskHandle (string): 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 -
Python synopsis simOMPL.addGoalState(string taskHandle, list state)
See also simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates

simOMPL.compute

Description Use OMPL to find a solution for this motion planning task.
It is equivalent to executing: if simOMPL.solve(task, maxTime) then simOMPL.simplifyPath(task, maxSimplificationTime) simOMPL.interpolatePath(task, stateCnt) path = simOMPL.getPath(task) end
Lua synopsis bool solved, float[] states=simOMPL.compute(string taskHandle, float maxTime, float maxSimplificationTime=-1.0, int stateCnt=0)
Lua parameters
taskHandle (string): 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): maximum time used for the path simplification procedure, in seconds. -1 for a default simplification procedure.
stateCnt (int, default: 0): 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.
Python synopsis bool solved, list states=simOMPL.compute(string taskHandle, float maxTime, float maxSimplificationTime=-1.0, int stateCnt=0)
See also simOMPL.getGoalDistance simOMPL.getPath simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.simplifyPath simOMPL.solve simOMPL.drawPath simOMPL.getPath simOMPL.getPathState simOMPL.getPathStateCount simOMPL.getProjectedPathLength simOMPL.getReversedPath simOMPL.projectStates

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 string stateSpaceHandle=simOMPL.createStateSpace(string name, int type, int objectHandle, float[] boundsLow, float[] 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 or .cyclic_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): the weight of this state space component, used for computing distance between states. Default value is 1.0
refObjectHandle (int, default: -1): 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 (string): a handle to the created state space component
Python synopsis string stateSpaceHandle=simOMPL.createStateSpace(string name, int type, int objectHandle, list boundsLow, list boundsHigh, int useForProjection, float weight=1.0, int refObjectHandle=-1)
See also simOMPL.destroyStateSpace simOMPL.getStateSpaceDimension simOMPL.setDubinsParams simOMPL.setStateSpace

simOMPL.createStateSpaceForJoint

Description convenience function that wraps simOMPL.createStateSpace
Lua synopsis int ssHandle=simOMPL.createStateSpaceForJoint(string name, int jointHandle, int useForProjection=0, float weight=1)
Lua parameters
name (string): name of the state space
jointHandle (int): handle of the joint
useForProjection (int, default: 0): use for projection
weight (float, default: 1): weight
Lua return values
ssHandle (int): handle of the state space
Python synopsis int ssHandle=simOMPL.createStateSpaceForJoint(string name, int jointHandle, int useForProjection=0, float weight=1)
See also simOMPL.enforceBounds simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.stateDistance simOMPL.writeState

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 string taskHandle=simOMPL.createTask(string name)
Lua parameters
name (string): a name for this task object
Lua return values
taskHandle (string): a handle to the created task object
Python synopsis string taskHandle=simOMPL.createTask(string name)
See also simOMPL.addGoalState simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

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(string stateSpaceHandle)
Lua parameters
stateSpaceHandle (string): handle to state space component
Lua return values -
Python synopsis simOMPL.destroyStateSpace(string stateSpaceHandle)
See also simOMPL.createStateSpace simOMPL.getStateSpaceDimension simOMPL.setDubinsParams simOMPL.setStateSpace

simOMPL.destroyTask

Description Destroy the specified task object.

Note: task objects created during simulation are automatically destroyed when simulation ends.
Lua synopsis simOMPL.destroyTask(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values -
Python synopsis simOMPL.destroyTask(string taskHandle)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

simOMPL.drawPath

Description draw a solution path for the specified motion planning task (as lines)
Lua synopsis int[] dwos=simOMPL.drawPath(string taskHandle, float[] path, float lineSize, float[3] color, int extraAttributes)
Lua parameters
taskHandle (string): the handle of the task
path (table of float): the path, as returned by simOMPL.getPath
lineSize (float): size of the line (in pixels)
color (table of float, size 3): color of the lines
extraAttributes (int): extra attributes to pass to sim.addDrawingObject
Lua return values
dwos (table of int): a table of handles of new drawing objects
Python synopsis list dwos=simOMPL.drawPath(string taskHandle, list path, float lineSize, list color, int extraAttributes)
See also simOMPL.compute simOMPL.getPath simOMPL.getPathState simOMPL.getPathStateCount simOMPL.getProjectedPathLength simOMPL.getReversedPath simOMPL.projectStates simOMPL.drawPlannerData simOMPL.removeDrawingObjects

simOMPL.drawPlannerData

Description draw planner data (graph) extracted from the specified motion planning task
Lua synopsis int[] dwos=simOMPL.drawPlannerData(string taskHandle, float pointSize, float lineSize, float[3] color, float[3] startColor, float[3] goalColor)
Lua parameters
taskHandle (string): handle of the task
pointSize (float): size of nodes (in meters)
lineSize (float): size of lines (in pixels)
color (table of float, size 3): color of nodes and lines
startColor (table of float, size 3): color of start nodes
goalColor (table of float, size 3): color of goal nodes
Lua return values
dwos (table of int): a table of handles of new drawing objects
Python synopsis list dwos=simOMPL.drawPlannerData(string taskHandle, float pointSize, float lineSize, list color, list startColor, list goalColor)
See also simOMPL.drawPath simOMPL.removeDrawingObjects

simOMPL.enforceBounds

Description Bring the state within the bounds of the state space.
Lua synopsis float[] state=simOMPL.enforceBounds(string taskHandle, float[] state)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
state (table of float): state vector
Lua return values
state (table of float): state vector
Python synopsis list state=simOMPL.enforceBounds(string taskHandle, list state)
See also simOMPL.createStateSpaceForJoint simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.stateDistance simOMPL.writeState

simOMPL.getGoalDistance

Description Get the distance to the desired goal for the top solution.
Lua synopsis float distance=simOMPL.getGoalDistance(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values
distance (float): distance to the desired goal for the top solution, or -1.0 if there are no solutions available
Python synopsis float distance=simOMPL.getGoalDistance(string taskHandle)
See also simOMPL.compute simOMPL.getPath simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.simplifyPath simOMPL.solve

simOMPL.getPath

Description Return the top solution path, if one is found.
The top path is the shortest one that was found, preference being given to solutions that are not approximate.
Lua synopsis float[] states=simOMPL.getPath(string taskHandle)
Lua parameters
taskHandle (string): 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.
Python synopsis list states=simOMPL.getPath(string taskHandle)
See also simOMPL.compute simOMPL.getGoalDistance simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.simplifyPath simOMPL.solve simOMPL.compute simOMPL.drawPath simOMPL.getPathState simOMPL.getPathStateCount simOMPL.getProjectedPathLength simOMPL.getReversedPath simOMPL.projectStates

simOMPL.getPathState

Description extract the state at specified index from the given path
Lua synopsis float[] state=simOMPL.getPathState(string taskHandle, float[] path, int index)
Lua parameters
taskHandle (string): the handle of the task
path (table of float): the path, as returned by simOMPL.getPath
index (int): the index, starting from 1
Lua return values
state (table of float): a state extracted from the path
Python synopsis list state=simOMPL.getPathState(string taskHandle, list path, int index)
See also simOMPL.compute simOMPL.drawPath simOMPL.getPath simOMPL.getPathStateCount simOMPL.getProjectedPathLength simOMPL.getReversedPath simOMPL.projectStates simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.stateDistance simOMPL.writeState

simOMPL.getPathStateCount

Description get the number of states in the given path
Lua synopsis int count=simOMPL.getPathStateCount(string taskHandle, float[] path)
Lua parameters
taskHandle (string): the handle of the task
path (table of float): the path, as returned by simOMPL.getPath
Lua return values
count (int): the number of states in the path
Python synopsis int count=simOMPL.getPathStateCount(string taskHandle, list path)
See also simOMPL.compute simOMPL.drawPath simOMPL.getPath simOMPL.getPathState simOMPL.getProjectedPathLength simOMPL.getReversedPath simOMPL.projectStates simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathState simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.stateDistance simOMPL.writeState

simOMPL.getPlannerData

Description Get planner data for this motion planning task.
Data is represented as a graph. For each graph vertex, a state and a tag are returned. A list of edges is returned as well, as pairs of vertex indices, describing connectivity of vertices. Some vertices are marked start or goal.
Lua synopsis float[] states, int[] tags, float[] tagsReal, int[] edges, float[] edgeWeights, int[] startVertices, int[] goalVertices=simOMPL.getPlannerData(string taskHandle)
Lua parameters
taskHandle (string): 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.
tags (table of int): a table of tags (one integer for every vertex)
tagsReal (table of float): a table of tags (one real value for every vertex)
edges (table of int): the pairs of vertex indices describing vertex connectivity
edgeWeights (table of float): the weight (cost) associated to each edge
startVertices (table of int): the indices of vertices marked 'start'
goalVertices (table of int): the indices of vertices marked 'goal'
Python synopsis list states, list tags, list tagsReal, list edges, list edgeWeights, list startVertices, list goalVertices=simOMPL.getPlannerData(string taskHandle)
See also simOMPL.compute simOMPL.getGoalDistance simOMPL.getPath simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.simplifyPath simOMPL.solve

simOMPL.getProjectedPathLength

Description get the length of the path projected onto the default projection
Lua synopsis simOMPL.getProjectedPathLength(string taskHandle, float[] path)
Lua parameters
taskHandle (string): the handle of the task
path (table of float): the path, as returned by simOMPL.getPath
Lua return values -
Python synopsis simOMPL.getProjectedPathLength(string taskHandle, list path)
See also simOMPL.compute simOMPL.drawPath simOMPL.getPath simOMPL.getPathState simOMPL.getPathStateCount simOMPL.getReversedPath simOMPL.projectStates simOMPL.projectStates simOMPL.projectionSize simOMPL.setProjectionEvaluationCallback

simOMPL.getReversedPath

Description reverse the given path
Lua synopsis float[] reversedPath=simOMPL.getReversedPath(string taskHandle, float[] path)
Lua parameters
taskHandle (string): the handle of the task
path (table of float): the path, as returned by simOMPL.getPath
Lua return values
reversedPath (table of float): the reversed path
Python synopsis list reversedPath=simOMPL.getReversedPath(string taskHandle, list path)
See also simOMPL.compute simOMPL.drawPath simOMPL.getPath simOMPL.getPathState simOMPL.getPathStateCount simOMPL.getProjectedPathLength simOMPL.projectStates

simOMPL.getStateSpaceDimension

Description Get the dimension of the state space, i.e. the number of elements of a state.
Lua synopsis int dim=simOMPL.getStateSpaceDimension(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values
dim (int): dimension of the state space
Python synopsis int dim=simOMPL.getStateSpaceDimension(string taskHandle)
See also simOMPL.createStateSpace simOMPL.destroyStateSpace simOMPL.setDubinsParams simOMPL.setStateSpace

simOMPL.hasApproximateSolution

Description Return true if the top found solution is approximate.
An approximate solution does not actually reach the desired goal, but hopefully is closer to it.
Lua synopsis bool result=simOMPL.hasApproximateSolution(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values
result (bool): true if the top found solution is approximate
Python synopsis bool result=simOMPL.hasApproximateSolution(string taskHandle)
See also simOMPL.compute simOMPL.getGoalDistance simOMPL.getPath simOMPL.getPlannerData simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.simplifyPath simOMPL.solve

simOMPL.hasExactSolution

Description Returns true if an exact solution path has been found.
Specifically returns hasSolution() and not hasApproximateSolution()
Lua synopsis bool result=simOMPL.hasExactSolution(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values
result (bool): true if an exact solution path has been found
Python synopsis bool result=simOMPL.hasExactSolution(string taskHandle)
See also simOMPL.compute simOMPL.getGoalDistance simOMPL.getPath simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.simplifyPath simOMPL.solve

simOMPL.hasSolution

Description Returns true if a solution path has been found (could be approximate).
Lua synopsis bool result=simOMPL.hasSolution(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values
result (bool): true if a solution path has been found (could be approximate)
Python synopsis bool result=simOMPL.hasSolution(string taskHandle)
See also simOMPL.compute simOMPL.getGoalDistance simOMPL.getPath simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.simplifyPath simOMPL.solve

simOMPL.interpolatePath

Description Interpolate the path found by planner to obtain a minimum number of states.
Lua synopsis simOMPL.interpolatePath(string taskHandle, int stateCnt=0)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
stateCnt (int, default: 0): minimum number of states to be returned. 0 for a default behaviour.
Lua return values -
Python synopsis simOMPL.interpolatePath(string taskHandle, int stateCnt=0)
See also simOMPL.compute simOMPL.getGoalDistance simOMPL.getPath simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.setAlgorithm simOMPL.simplifyPath simOMPL.solve

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 bool valid=simOMPL.isStateValid(string taskHandle, float[] state)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
state (table of float): state vector
Lua return values
valid (bool): true if the state is valid
Python synopsis bool valid=simOMPL.isStateValid(string taskHandle, list state)
See also simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.stateDistance simOMPL.writeState

simOMPL.isStateWithinBounds

Description Check if the specified state is inside the bounding box.
Lua synopsis bool valid=simOMPL.isStateWithinBounds(string taskHandle, float[] state)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
state (table of float): state vector
Lua return values
valid (bool): true if the state is within the bounding box
Python synopsis bool valid=simOMPL.isStateWithinBounds(string taskHandle, list state)
See also simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.projectStates simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.stateDistance simOMPL.writeState

simOMPL.printTaskInfo

Description Print a summary of the specified task object.
Useful for debugging and submitting bug reports.
Lua synopsis simOMPL.printTaskInfo(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values -
Python synopsis simOMPL.printTaskInfo(string taskHandle)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

simOMPL.projectStates

Description Project a state using the task's projection evaluation routine.
If multiple states are specified (e.g. a path) multiple projections will be returned.
Lua synopsis float[] projection=simOMPL.projectStates(string taskHandle, float[] state)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
state (table of float): state vector(s)
Lua return values
projection (table of float): state projection vector(s)
Python synopsis list projection=simOMPL.projectStates(string taskHandle, list state)
See also simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.stateDistance simOMPL.writeState simOMPL.getProjectedPathLength simOMPL.projectionSize simOMPL.setProjectionEvaluationCallback simOMPL.compute simOMPL.drawPath simOMPL.getPath simOMPL.getPathState simOMPL.getPathStateCount simOMPL.getProjectedPathLength simOMPL.getReversedPath

simOMPL.projectionSize

Description return the dimension of the projection
Lua synopsis int size=simOMPL.projectionSize(string taskHandle)
Lua parameters
taskHandle (string): the handle of the task
Lua return values
size (int): of the projection
Python synopsis int size=simOMPL.projectionSize(string taskHandle)
See also simOMPL.getProjectedPathLength simOMPL.projectStates simOMPL.setProjectionEvaluationCallback

simOMPL.readState

Description Read a state vector from current simulator state.
Lua synopsis float[] state=simOMPL.readState(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values
state (table of float): state vector
Python synopsis list state=simOMPL.readState(string taskHandle)
See also simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.setStateSpaceForJoints simOMPL.stateDistance simOMPL.writeState

simOMPL.removeDrawingObjects

Description remove the drawing objects created with related functions
Lua synopsis simOMPL.removeDrawingObjects(string taskHandle, int[] dwos)
Lua parameters
taskHandle (string): handle of the task
dwos (table of int): table of handles to drawing objects, as returned by the functions
Lua return values -
Python synopsis simOMPL.removeDrawingObjects(string taskHandle, list dwos)
See also simOMPL.drawPath simOMPL.drawPlannerData

simOMPL.setAlgorithm

Description Set the search algorithm for the specified task. Default algorithm used is KPIECE1.
Lua synopsis simOMPL.setAlgorithm(string taskHandle, int algorithm)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
algorithm (int): see simOMPL.Algorithm
Lua return values -
Python synopsis simOMPL.setAlgorithm(string taskHandle, int algorithm)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup simOMPL.compute simOMPL.getGoalDistance simOMPL.getPath simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.simplifyPath simOMPL.solve

simOMPL.setCollisionPairs

Description Set the collision pairs for the specified task object.
Lua synopsis simOMPL.setCollisionPairs(string taskHandle, int[] collisionPairHandles)
Lua parameters
taskHandle (string): 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 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 -
Python synopsis simOMPL.setCollisionPairs(string taskHandle, list collisionPairHandles)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

simOMPL.setDubinsParams

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

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(string taskHandle, int robotDummy, int goalDummy, float tolerance=0.001, float[] metric={1.0, 1.0, 1.0, 0.1}, int refDummy=-1)
Lua parameters
taskHandle (string): 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.001): tolerated dummy-dummy distance
metric (table of float, default: {1.0, 1.0, 1.0, 0.1}): a metric (x,y,z,angle) used to evaluate the dummy-dummy distance
refDummy (int, default: -1): an reference dummy, relative to which the metric will be used
Lua return values -
Python synopsis simOMPL.setGoal(string taskHandle, int robotDummy, int goalDummy, float tolerance=0.001, list metric={1.0, 1.0, 1.0, 0.1}, int refDummy=-1)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup simOMPL.addGoalState simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates

simOMPL.setGoalCallback

Description Set a custom goal callback for the specified task. See goalCallback for the arguments and the return values of the callback.
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(string taskHandle, func callback)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
callback (func): callback
Lua return values -
Python synopsis simOMPL.setGoalCallback(string taskHandle, func callback)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup simOMPL.addGoalState simOMPL.setGoal simOMPL.setGoalState simOMPL.setGoalStates

simOMPL.setGoalState

Description Set the goal state for the specified task object.
Lua synopsis simOMPL.setGoalState(string taskHandle, float[] state)
Lua parameters
taskHandle (string): 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 -
Python synopsis simOMPL.setGoalState(string taskHandle, list state)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup simOMPL.addGoalState simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalStates

simOMPL.setGoalStates

Description set multiple goal states at once, equivalent to calling simOMPL.setGoalState, simOMPL.addGoalState, simOMPL.addGoalState...
Lua synopsis simOMPL.setGoalStates(string taskHandle, table states)
Lua parameters
taskHandle (string): the handle of the task
states (table, size 1..*): a table of tables, one element for each goal state
Lua return values -
Python synopsis simOMPL.setGoalStates(string taskHandle, list states)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup simOMPL.addGoalState simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState

simOMPL.setProjectionEvaluationCallback

Description Set a custom projection evaluation. See projectionEvaluationCallback for the arguments and the return values of the callback.
Lua synopsis simOMPL.setProjectionEvaluationCallback(string taskHandle, func callback, int projectionSize)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
callback (func): callback
projectionSize (int): size of the projection (usually 2 or 3)
Lua return values -
Python synopsis simOMPL.setProjectionEvaluationCallback(string taskHandle, func callback, int projectionSize)
See also simOMPL.getProjectedPathLength simOMPL.projectStates simOMPL.projectionSize simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

simOMPL.setStartState

Description Set the start state for the specified task object.
Lua synopsis simOMPL.setStartState(string taskHandle, float[] state)
Lua parameters
taskHandle (string): 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 -
Python synopsis simOMPL.setStartState(string taskHandle, list state)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

simOMPL.setStateSpace

Description Set the state space of this task object.
Lua synopsis simOMPL.setStateSpace(string taskHandle, string[] stateSpaceHandles)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
stateSpaceHandles (table of string): a table of handles to state space components, created with simOMPL.createStateSpace
Lua return values -
Python synopsis simOMPL.setStateSpace(string taskHandle, list stateSpaceHandles)
See also simOMPL.createStateSpace simOMPL.destroyStateSpace simOMPL.getStateSpaceDimension simOMPL.setDubinsParams simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

simOMPL.setStateSpaceForJoints

Description convenience function that wraps simOMPL.setStateSpace
Lua synopsis simOMPL.setStateSpaceForJoints(string taskHandle, int[] jointHandles, int[] useForProjection={}, float[] weight={})
Lua parameters
taskHandle (string): the handle of the task
jointHandles (table of int): handles of the joints
useForProjection (table of int, default: {}): use for projection, same size as jointHandles
weight (table of float, default: {}): weights, same size as jointHandles
Lua return values -
Python synopsis simOMPL.setStateSpaceForJoints(string taskHandle, list jointHandles, list useForProjection={}, list weight={})
See also simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.readState simOMPL.stateDistance simOMPL.writeState

simOMPL.setStateValidationCallback

Description Set a custom state validation. See stateValidationCallback for the arguments and the return values of the callback.
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.
Lua synopsis simOMPL.setStateValidationCallback(string taskHandle, func callback)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
callback (func): callback
Lua return values -
Python synopsis simOMPL.setStateValidationCallback(string taskHandle, func callback)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

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% of the state space's extent.
Lua synopsis simOMPL.setStateValidityCheckingResolution(string taskHandle, float resolution)
Lua parameters
taskHandle (string): 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 -
Python synopsis simOMPL.setStateValidityCheckingResolution(string taskHandle, float resolution)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel simOMPL.setup

simOMPL.setValidStateSamplerCallback

Description Set a valid state sampler callbacks for the specified task. See validStateSamplerCallback and validStateSamplerCallbackNear for the arguments and the return values of the callbacks.
The valid state sampler callbacks must generate valid states.
Lua synopsis simOMPL.setValidStateSamplerCallback(string taskHandle, func callback, func callbackNear)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
callback (func): callback for sampling a state
callbackNear (func): callback for sampling near a given state within the given distance
Lua return values -
Python synopsis simOMPL.setValidStateSamplerCallback(string taskHandle, func callback, func callbackNear)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setVerboseLevel simOMPL.setup

simOMPL.setVerboseLevel

Description Set the verbosity level for messages printed to application console.
Lua synopsis simOMPL.setVerboseLevel(string taskHandle, int verboseLevel)
Lua parameters
taskHandle (string): 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 -
Python synopsis simOMPL.setVerboseLevel(string taskHandle, int verboseLevel)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setup

simOMPL.setup

Description Setup the OMPL classes with the information contained in the task.
Lua synopsis simOMPL.setup(string taskHandle)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
Lua return values -
Python synopsis simOMPL.setup(string taskHandle)
See also simOMPL.addGoalState simOMPL.createTask simOMPL.destroyTask simOMPL.printTaskInfo simOMPL.setAlgorithm simOMPL.setCollisionPairs simOMPL.setGoal simOMPL.setGoalCallback simOMPL.setGoalState simOMPL.setGoalStates simOMPL.setProjectionEvaluationCallback simOMPL.setStartState simOMPL.setStateSpace simOMPL.setStateValidationCallback simOMPL.setStateValidityCheckingResolution simOMPL.setValidStateSamplerCallback simOMPL.setVerboseLevel

simOMPL.simplifyPath

Description Simplify the path found by planner.
Lua synopsis simOMPL.simplifyPath(string taskHandle, float maxSimplificationTime=-1.0)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
maxSimplificationTime (float, default: -1.0): maximum time used for the path simplification procedure, in seconds. -1 for a default simplification procedure.
Lua return values -
Python synopsis simOMPL.simplifyPath(string taskHandle, float maxSimplificationTime=-1.0)
See also simOMPL.compute simOMPL.getGoalDistance simOMPL.getPath simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.solve

simOMPL.solve

Description Run the planning algorithm to search for a solution.
Lua synopsis bool solved=simOMPL.solve(string taskHandle, float maxTime)
Lua parameters
taskHandle (string): 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.
Python synopsis bool solved=simOMPL.solve(string taskHandle, float maxTime)
See also simOMPL.compute simOMPL.getGoalDistance simOMPL.getPath simOMPL.getPlannerData simOMPL.hasApproximateSolution simOMPL.hasExactSolution simOMPL.hasSolution simOMPL.interpolatePath simOMPL.setAlgorithm simOMPL.simplifyPath

simOMPL.stateDistance

Description Compute the distanbce between two states.
Lua synopsis float distance=simOMPL.stateDistance(string taskHandle, float[] a, float[] b)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
a (table of float): state vector
b (table of float): state vector
Lua return values
distance (float): computed distance
Python synopsis float distance=simOMPL.stateDistance(string taskHandle, list a, list b)
See also simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.writeState

simOMPL.writeState

Description Write the specified state to simulator
Lua synopsis simOMPL.writeState(string taskHandle, float[] state)
Lua parameters
taskHandle (string): a handle to a task object created with simOMPL.createTask
state (table of float): state vector
Lua return values -
Python synopsis simOMPL.writeState(string taskHandle, list state)
See also simOMPL.createStateSpaceForJoint simOMPL.enforceBounds simOMPL.getPathState simOMPL.getPathStateCount simOMPL.isStateValid simOMPL.isStateWithinBounds simOMPL.projectStates simOMPL.readState simOMPL.setStateSpaceForJoints simOMPL.stateDistance



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
RRT
RRTConnect
RRTstar
SBL
SPARS
SPARStwo
STRIDE
TRRT

simOMPL.StateSpaceType

position2d
pose2d
position3d
pose3d
joint_position
dubins
cyclic_joint_position


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(float[] 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.
Python synopsis bool satisfied, float distance=simOMPL.goalCallback(list state)
See also

projectionEvaluationCallback

Description Callback for computing a (euclidean) projection of states.
Lua synopsis float[] projection=simOMPL.projectionEvaluationCallback(float[] 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.
Python synopsis list projection=simOMPL.projectionEvaluationCallback(list state)
See also

stateValidationCallback

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

validStateSamplerCallback

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

validStateSamplerCallbackNear

Description Callback for sampling valid states in the proximity of a given state, within a certain distance.
Lua synopsis float[] sampledState=simOMPL.validStateSamplerCallbackNear(float[] 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
Python synopsis list sampledState=simOMPL.validStateSamplerCallbackNear(list state, float distance)
See also