klampt.plan.motionplanning module

Python interface to C++ motion planing routines

class klampt.plan.motionplanning.CSpaceInterface(*args)[source]

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()[source]

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)[source]
Parameters:
  • name (str) –
  • pyFeas (object) –
addVisibilityTest(name, pyVisible)[source]
Parameters:
  • name (str) –
  • pyVisible (object) –
destroy()[source]
distance(a, b)[source]

Returns the distance between two configurations.

Parameters:
  • a (object) –
  • b (object) –
Returns:

Return type:

(float)

enableAdaptiveQueries(enabled=True)[source]

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

enableAdaptiveQueries (enabled=True)

enableAdaptiveQueries ()

Parameters:enabled (bool, optional) – default value True
feasibilityCost(name)[source]

Retrieves the empirical average cost of a given feasibility test.

Parameters:name (str) –
Returns:
Return type:(float)
feasibilityFailures(q)[source]

Returns a list of all failed feasibility constraints.

Parameters:q (object) –
Returns:
Return type:(object)
feasibilityProbability(name)[source]

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

Parameters:name (str) –
Returns:
Return type:(float)
feasibilityQueryOrder()[source]

Retrieves the current order of feasibility tests.

Returns:
Return type:(object)
getProperty(key)[source]
Parameters:key (str) –
Returns:
Return type:(str)
getStats()[source]

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

Returns:
Return type:(object)
index

CSpaceInterface_index_get(CSpaceInterface self) -> int

interpolate(a, b, u)[source]

Interpolates between two configurations.

Parameters:
  • a (object) –
  • b (object) –
  • u (float) –
Returns:

Return type:

(object)

isFeasible(q)[source]

Queries whether a given configuration is feasible.

Parameters:q (object) –
Returns:
Return type:(bool)
isVisible(a, b)[source]

Queries whether two configurations are visible.

Parameters:
  • a (object) –
  • b (object) –
Returns:

Return type:

(bool)

optimizeQueryOrder()[source]

Call this to optimize the feasibility / visibility testing order.

sample()[source]

Samples a configuration.

Returns:
Return type:(object)
setDistance(pyDist)[source]
Parameters:pyDist (object) –
setFeasibility(pyFeas)[source]
Parameters:pyFeas (object) –
setFeasibilityDependency(name, precedingTest)[source]

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)[source]

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)[source]
Parameters:pyInterp (object) –
setNeighborhoodSampler(pySamp)[source]
Parameters:pySamp (object) –
setProperty(key, value)[source]
Parameters:
  • key (str) –
  • value (str) –
setSampler(pySamp)[source]
Parameters:pySamp (object) –
setVisibility(pyVisible)[source]
Parameters:pyVisible (object) –
setVisibilityDependency(name, precedingTest)[source]

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

Parameters:
  • name (str) –
  • precedingTest (str) –
setVisibilityEpsilon(eps)[source]
Parameters:eps (float) –
setVisibilityPrior(name, costPrior=0.0, visibilityProbability=0.0, evidenceStrength=1.0)[source]

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)[source]

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)[source]

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)[source]

Retrieves the empirical average cost of a given visibility test.

Parameters:name (str) –
Returns:
Return type:(float)
visibilityFailures(a, b)[source]

Returns a list of all failed visibility constraints.

Parameters:
  • a (object) –
  • b (object) –
Returns:

Return type:

(object)

visibilityProbability(name)[source]

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

Parameters:name (str) –
Returns:
Return type:(float)
visibilityQueryOrder()[source]

Retrieves the current order of visibility tests.

Returns:
Return type:(object)
class klampt.plan.motionplanning.PlannerInterface(cspace)[source]

Bases: object

An interface for a 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.

PRM can be used in either point-to-point or multi-query mode. 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().

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

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)[source]
Parameters:milestone (object) –
Returns:
Return type:(int)
destroy()[source]
dump(fn)[source]
Parameters:fn (str) –
getData(setting)[source]
Parameters:setting (str) –
Returns:
Return type:(float)
getPath(milestone1, milestone2)[source]
Parameters:
  • milestone1 (int) –
  • milestone2 (int) –
Returns:

Return type:

(object)

getPathEndpoints()[source]
Returns:
Return type:(object)
getRoadmap()[source]
Returns:
Return type:(object)
getStats()[source]
Returns:
Return type:(object)
index

PlannerInterface_index_get(PlannerInterface self) -> int

planMore(iterations)[source]
Parameters:iterations (int) –
setEndpointSet(start, goal, goalSample=None)[source]

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)[source]
Parameters:
  • start (object) –
  • goal (object) –
Returns:

Return type:

(bool)

spaceIndex

PlannerInterface_spaceIndex_get(PlannerInterface self) -> int

klampt.plan.motionplanning.destroy()[source]

destroys internal data structures

klampt.plan.motionplanning.getPlanJSONString()[source]

Saves planner values to a JSON string.

Returns:
Return type:(str)
klampt.plan.motionplanning.setPlanJSONString(string)[source]

Loads planner values from a JSON string.

Parameters:string (str) –
klampt.plan.motionplanning.setPlanSetting(*args)[source]

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

setPlanSetting (setting,value)

Parameters:
  • setting (str) –
  • value (float or str) –

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 (default value: “{foundSolution:1;maxIters:1000}”)
klampt.plan.motionplanning.setPlanType(type)[source]

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)[source]

Sets the random seed used by the configuration sampler.

Parameters:seed (int) –