klampt.plan.motionplanning module

Python interface to C++ motion planning routines

class klampt.plan.motionplanning.CSpaceInterface(*args)

Bases: object

A raw interface for a configuration space. Note: the native Python CSpace interface class in cspace.py is easier to use.

You can either set a single feasibility test function using setFeasibility() or add several feasibility tests, all of which need to be satisfied, using addFeasibilityTest(). In the latter case, planners may be able to provide debugging statistics, solve Minimum Constraint Removal problems, run faster by eliminating constraint tests, etc.

Either setVisibility() or setVisibilityEpsilon() must be called to define a visibility checker between two (feasible) configurations. In the latter case, the path will be discretized at the resolution sent to setVisibilityEpsilon. If you have special single-constraint visibility tests, you can call that using addVisibilityTest (for example, for convex constraints you can set it to the lambda function that returns true regardless of its arguments).

Supported properties include “euclidean” (boolean), “metric” (string), “geodesic” (boolean), “minimum” (vector), and “maximum” (vector). These may be used by planners to make planning faster or more accurate. For a complete list see KrisLibrary/planning/CSpace.h.

C++ includes: motionplanning.h

__init__ (): CSpaceInterface

__init__ (arg2): CSpaceInterface

Parameters

arg2 (CSpaceInterface, optional) –

adaptiveQueriesEnabled()

optional: adaptive queries can be used to automatically minimize the total cost of testing feasibility / visibility using empirical estimates. Off by default.

Returns

Return type

bool

addFeasibilityTest(name, pyFeas)

Parameters

• name (str) –

• pyFeas (object) –

addVisibilityTest(name, pyVisible)

Parameters

• name (str) –

• pyVisible (object) –

destroy()

distance(a, b)

Returns the distance between two configurations.

Parameters

• a (object) –

• b (object) –

Returns

Return type

float

enableAdaptiveQueries(enabled=True)

Call this to enable adaptive queries. (It has a small overhead.)

enableAdaptiveQueries (enabled=True)

enableAdaptiveQueries ()

Parameters

enabled (bool, optional) – default value True

feasibilityCost(name)

Retrieves the empirical average cost of a given feasibility test.

Parameters

name (str) –

Returns

Return type

float

feasibilityFailures(q)

Returns a list of all failed feasibility constraints.

Parameters

q (object) –

Returns

Return type

object

feasibilityProbability(name)

Retrieves the empirical average success rate of a given feasibility test.

Parameters

name (str) –

Returns

Return type

float

feasibilityQueryOrder()

Retrieves the current order of feasibility tests.

Returns

Return type

object

getProperty(key)

Parameters

key (str) –

Returns

Return type

str

getStats()

Returns constraint testing statistics. If adaptive queries are enabled, this returns the stats on each constraint.

Returns

Return type

object

property index

CSpaceInterface_index_get(CSpaceInterface self) -> int

interpolate(a, b, u)

Interpolates between two configurations.

Parameters

• a (object) –

• b (object) –

• u (float) –

Returns

Return type

object

isFeasible(q)

Queries whether a given configuration is feasible.

Parameters

q (object) –

Returns

Return type

bool

isVisible(a, b)

Queries whether two configurations are visible.

Parameters

• a (object) –

• b (object) –

Returns

Return type

bool

optimizeQueryOrder()

Call this to optimize the feasibility / visibility testing order.

sample()

Samples a configuration.

Returns

Return type

object

setDistance(pyDist)

Parameters

pyDist (object) –

setFeasibility(pyFeas)

Parameters

pyFeas (object) –

setFeasibilityDependency(name, precedingTest)

Marks that a certain feasibility test must be performed before another.

Parameters

• name (str) –

• precedingTest (str) –

setFeasibilityPrior(name, costPrior=0.0, feasibilityProbability=0.0, evidenceStrength=1.0)

Resets the data for a certain feasibility test. Default values give a data- gathering behavior.

setFeasibilityPrior (name,costPrior=0.0,feasibilityProbability=0.0,evidenceStrength=1.0)

setFeasibilityPrior (name,costPrior=0.0,feasibilityProbability=0.0)

setFeasibilityPrior (name,costPrior=0.0)

setFeasibilityPrior (name)

Parameters

• name (str) –

• costPrior (float, optional) – default value 0.0

• feasibilityProbability (float, optional) – default value 0.0

• evidenceStrength (float, optional) – default value 1.0

setInterpolate(pyInterp)

Parameters

pyInterp (object) –

setNeighborhoodSampler(pySamp)

Parameters

pySamp (object) –

setProperty(key, value)

Parameters

• key (str) –

• value (str) –

setSampler(pySamp)

Parameters

pySamp (object) –

setVisibility(pyVisible)

Parameters

pyVisible (object) –

setVisibilityDependency(name, precedingTest)

Marks that a certain feasibility test must be performed before another.

Parameters

• name (str) –

• precedingTest (str) –

setVisibilityEpsilon(eps)

Parameters

eps (float) –

setVisibilityPrior(name, costPrior=0.0, visibilityProbability=0.0, evidenceStrength=1.0)

Resets the data for a certain visibility test. Default values give a data- gathering behavior.

setVisibilityPrior (name,costPrior=0.0,visibilityProbability=0.0,evidenceStrength=1.0)

setVisibilityPrior (name,costPrior=0.0,visibilityProbability=0.0)

setVisibilityPrior (name,costPrior=0.0)

setVisibilityPrior (name)

Parameters

• name (str) –

• costPrior (float, optional) – default value 0.0

• visibilityProbability (float, optional) – default value 0.0

• evidenceStrength (float, optional) – default value 1.0

testFeasibility(name, q)

Queries whether a given configuration is feasible with respect to a given constraint.

Parameters

• name (str) –

• q (object) –

Returns

Return type

bool

testVisibility(name, a, b)

Queries whether two configurations are visible with respect to a given constraint.

Parameters

• name (str) –

• a (object) –

• b (object) –

Returns

Return type

bool

visibilityCost(name)

Retrieves the empirical average cost of a given visibility test.

Parameters

name (str) –

Returns

Return type

float

visibilityFailures(a, b)

Returns a list of all failed visibility constraints.

Parameters

• a (object) –

• b (object) –

Returns

Return type

object

visibilityProbability(name)

Retrieves the empirical average success rate of a given visibility test.

Parameters

name (str) –

Returns

Return type

float

visibilityQueryOrder()

Retrieves the current order of visibility tests.

Returns

Return type

object

class klampt.plan.motionplanning.PlannerInterface(cspace)

Bases: object

An interface for a kinematic motion planner. The MotionPlan interface in cspace.py is somewhat easier to use.

On construction, uses the planner type specified by setPlanType and the settings currently specified by calls to setPlanSetting.

Point-to-point planning is enabled by sending two configurations to the setEndpoints method. This is mandatory for RRT and SBL-style planners. The start and end milestones are given by indices 0 and 1, respectively

Point-to-set planning is enabled by sending a goal test as the second argument to the setEndpoints method. It is possible also to send a special goal sampler by providing a pair of functions as the second argument consisting of the two functions (goaltest,goalsample). The first in this pair tests whether a configuration is a goal, and the second returns a sampled configuration in a superset of the goal. Ideally the goal sampler generates as many goals as possible.

To plan, call planMore(iters) until getPath(0,1) returns non-NULL. The return value is a list of configurations.

Some planners can be used multi-query mode (such as PRM). In multi-query mode, you may call addMilestone(q) to add a new milestone. addMilestone() returns the index of that milestone, which can be used in later calls to getPath().

In point-to-set mode, getSolutionPath will return the optimal path to any goal milestone.

All planners work with the standard path-length objective function. Some planners can work with other cost functions, and you can use setCostFunction to set the edge / terminal costs. Usually, the results will only be optimal on the computed graph, and the graph is not specifically computed to optimize that cost.

To get a roadmap (V,E), call getRoadmap(). V is a list of configurations (each configuration is a Python list) and E is a list of edges (each edge is a pair (i,j) indexing into V).

To dump the roadmap to disk, call dump(fn). This saves to a Trivial Graph Format (TGF) format.

C++ includes: motionplanning.h

Parameters

cspace (CSpaceInterface) –

addMilestone(milestone)

Parameters

milestone (object) –

Returns

Return type

int

destroy()

dump(fn)

Parameters

fn (str) –

getClosestMilestone(config)

Parameters

config (object) –

Returns

Return type

int

getData(setting)

Parameters

setting (str) –

Returns

Return type

float

getMilestone(arg2)

Parameters

arg2 (int) –

Returns

Return type

object

getPath(*args)

getPath (milestone1,milestone2): object

getPath (milestone1,int,goalMilestones): object

Parameters

• milestone1 (int) –

• milestone2 (int, optional) –

• int (std::vector<, optional) –

• goalMilestones (std::allocator, optional) –

Returns

Return type

(object)

getRoadmap()

Returns

Return type

object

getSolutionPath()

Returns

Return type

object

getStats()

Returns

Return type

object

property index

PlannerInterface_index_get(PlannerInterface self) -> int

planMore(iterations)

Parameters

iterations (int) –

setCostFunction(edgeCost=None, terminalCost=None)

setCostFunction (edgeCost=None)

setCostFunction ()

Parameters

• edgeCost (object, optional) – default value None

• terminalCost (object, optional) – default value None

setEndpointSet(start, goal, goalSample=None)

setEndpointSet (start,goal,goalSample=None): bool

setEndpointSet (start,goal): bool

Parameters

• start (object) –

• goal (object) –

• goalSample (object, optional) – default value None

Returns

Return type

(bool)

setEndpoints(start, goal)

Parameters

• start (object) –

• goal (object) –

Returns

Return type

bool

property spaceIndex

PlannerInterface_spaceIndex_get(PlannerInterface self) -> int

class klampt.plan.motionplanning.SwigPyIterator(*args, **kwargs)

Bases: object

advance(n)

copy()

decr(n=1)

distance(x)

equal(x)

incr(n=1)

next()

previous()

value()

klampt.plan.motionplanning.combineNDCubic(doubleMatrix times, doubleMatrix positions, doubleMatrix velocities)

klampt.plan.motionplanning.destroy()

destroys internal data structures

class klampt.plan.motionplanning.doubleMatrix(*args)

Bases: object

append(x)

assign(n, x)

back()

begin()

capacity()

clear()

empty()

end()

erase(*args)

front()

get_allocator()

insert(*args)

iterator()

pop()

pop_back()

push_back(x)

rbegin()

rend()

reserve(n)

resize(*args)

size()

swap(v)

class klampt.plan.motionplanning.doubleVector(*args)

Bases: object

append(x)

assign(n, x)

back()

begin()

capacity()

clear()

empty()

end()

erase(*args)

front()

get_allocator()

insert(*args)

iterator()

pop()

pop_back()

push_back(x)

rbegin()

rend()

reserve(n)

resize(*args)

size()

swap(v)

klampt.plan.motionplanning.getPlanJSONString()

Saves planner values to a JSON string.

Returns

Return type

str

klampt.plan.motionplanning.interpolate1DMinAccel(double x0, double v0, double x1, double v1, double endTime, double xmin, double xmax, double vmax)

klampt.plan.motionplanning.interpolate1DMinTime(double x0, double v0, double x1, double v1, double xmin, double xmax, double vmax, double amax)

klampt.plan.motionplanning.interpolateNDMinAccel(doubleVector x0, doubleVector v0, doubleVector x1, doubleVector v1, double endTime, doubleVector xmin, doubleVector xmax, doubleVector vmax)

klampt.plan.motionplanning.interpolateNDMinTime(doubleVector x0, doubleVector v0, doubleVector x1, doubleVector v1, doubleVector xmin, doubleVector xmax, doubleVector vmax, doubleVector amax)

klampt.plan.motionplanning.interpolateNDMinTimeLinear(doubleVector x0, doubleVector x1, doubleVector vmax, doubleVector amax)

klampt.plan.motionplanning.setPlanJSONString(string)

Loads planner values from a JSON string.

Parameters

string (str) –

klampt.plan.motionplanning.setPlanSetting(*args)

Sets a numeric or string-valued setting for the planner.

setPlanSetting (setting,value)

Parameters

• setting (str) –

• value (str or float) –

Valid numeric values are:

• “knn”: k value for the k-nearest neighbor connection strategy (only for PRM)

• “connectionThreshold”: a milestone connection threshold

• “perturbationRadius”: (for RRT and SBL)

• “bidirectional”: 1 if bidirectional planning is requested (for RRT)

• “grid”: 1 if a point selection grid should be used (for SBL)

• “gridResolution”: resolution for the grid, if the grid should be used (for SBL with grid, FMM, FMM*)

• “suboptimalityFactor”: allowable suboptimality (for RRT*, lazy PRM*, lazy RRG*)

• “randomizeFrequency”: a grid randomization frequency (for SBL)

• “shortcut”: nonzero if you wish to perform shortcutting after a first plan is found.

• “restart”: nonzero if you wish to restart the planner to get better paths with the remaining time.

Valid string values are:

• “pointLocation”: a string designating a point location data structure. “kdtree” is supported, optionally followed by a weight vector (for PRM, RRT*, PRM*, LazyPRM*, LazyRRG*)

• “restartTermCond”: used if the “restart” setting is true. This is a JSON string defining the termination condition.

  The default value is “{foundSolution:1;maxIters:1000}”, which indicates that the planner will restart if it has found a solution, or 1000 iterations have passed.

  To restart after a certain amount of time has elasped, use “{timeLimit:X}”. If you are using an optimizing planner, e.g., shortcutting, you should set foundSolution:0.

klampt.plan.motionplanning.setPlanType(type)

Sets the planner type.

Parameters

type (str) –

Valid values are

• prm: the Probabilistic Roadmap algorithm

• rrt: the Rapidly Exploring Random Trees algorithm

• sbl: the Single-Query Bidirectional Lazy planner

• sblprt: the probabilistic roadmap of trees (PRT) algorithm with SBL as the inter-root planner.

• rrt*: the RRT* algorithm for optimal motion planning

• prm*: the PRM* algorithm for optimal motion planning

• lazyprm*: the Lazy-PRM* algorithm for optimal motion planning

• lazyrrg*: the Lazy-RRG* algorithm for optimal motion planning

• fmm: the fast marching method algorithm for resolution-complete optimal motion planning

• fmm*: an anytime fast marching method algorithm for optimal motion planning

klampt.plan.motionplanning.setRandomSeed(seed)

Sets the random seed used by the configuration sampler.

Parameters

seed (int) –