klampt.plan.motionplanning module

Python interface to C++ motion planning routines

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

Bases: object

property thisown

The membership flag

value()[source]
Return type

object

incr(n=1)[source]
Return type

Iterator

decr(n=1)[source]
Return type

Iterator

distance(x)[source]
Return type

int

equal(x)[source]
Return type

bool

copy()[source]
Return type

Iterator

next()[source]
Return type

object

previous()[source]
Return type

object

advance(n)[source]
Return type

Iterator

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

Bases: object

property thisown

The membership flag

iterator()[source]
Return type

Iterator

pop()[source]
Return type

float

append(x)[source]
Return type

None

empty()[source]
Return type

bool

size()[source]
Return type

int

swap(v)[source]
Return type

None

begin()[source]
Return type

Iterator

end()[source]
Return type

Iterator

rbegin()[source]
Return type

Iterator

rend()[source]
Return type

Iterator

clear()[source]
Return type

None

get_allocator()[source]
pop_back()[source]
Return type

None

erase(*args)[source]
Return type

Iterator

push_back(x)[source]
Return type

None

front()[source]
Return type

float

back()[source]
Return type

float

assign(n, x)[source]
Return type

None

resize(*args)[source]
Return type

None

insert(*args)[source]
Return type

None

reserve(n)[source]
Return type

None

capacity()[source]
Return type

int

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

Bases: object

property thisown

The membership flag

iterator()[source]
Return type

Iterator

pop()[source]
Return type

Sequence[float]

append(x)[source]
Return type

None

empty()[source]
Return type

bool

size()[source]
Return type

int

swap(v)[source]
Return type

None

begin()[source]
Return type

Iterator

end()[source]
Return type

Iterator

rbegin()[source]
Return type

Iterator

rend()[source]
Return type

Iterator

clear()[source]
Return type

None

get_allocator()[source]
pop_back()[source]
Return type

None

erase(*args)[source]
Return type

Iterator

push_back(x)[source]
Return type

None

front()[source]
Return type

Sequence[float]

back()[source]
Return type

Sequence[float]

assign(n, x)[source]
Return type

None

resize(*args)[source]
Return type

None

insert(*args)[source]
Return type

None

reserve(n)[source]
Return type

None

capacity()[source]
Return type

int

klampt.plan.motionplanning.set_random_seed(seed)[source]

Sets the random seed used by the motion planner.

Parameters

seed (int) –

Return type

None

klampt.plan.motionplanning.set_plan_json_string(string)[source]

Loads planner values from a JSON string.

Parameters

string (str) –

Return type

None

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

Saves planner values to a JSON string.

Return type

str

klampt.plan.motionplanning.set_plan_type(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

Return type

None

klampt.plan.motionplanning.set_plan_setting(*args)[source]

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

set_plan_setting (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.

Return type

None

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

destroys internal data structures

Return type

None

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) –

property thisown

The membership flag

destroy()[source]
Return type

None

setFeasibility(pyFeas)[source]
Parameters

pyFeas (object) –

Return type

None

addFeasibilityTest(name, pyFeas)[source]
Parameters
  • name (str) –

  • pyFeas (object) –

Return type

None

setVisibility(pyVisible)[source]
Parameters

pyVisible (object) –

Return type

None

addVisibilityTest(name, pyVisible)[source]
Parameters
  • name (str) –

  • pyVisible (object) –

Return type

None

setVisibilityEpsilon(eps)[source]
Parameters

eps (float) –

Return type

None

setSampler(pySamp)[source]
Parameters

pySamp (object) –

Return type

None

setNeighborhoodSampler(pySamp)[source]
Parameters

pySamp (object) –

Return type

None

setDistance(pyDist)[source]
Parameters

pyDist (object) –

Return type

None

setInterpolate(pyInterp)[source]
Parameters

pyInterp (object) –

Return type

None

setProperty(key, value)[source]
Parameters
  • key (str) –

  • value (str) –

Return type

None

getProperty(key)[source]
Parameters

key (str) –

Return type

str

isFeasible(q)[source]

Queries whether a given configuration is feasible.

Parameters

q (object) –

Return type

bool

isVisible(a, b)[source]

Queries whether two configurations are visible.

Parameters
  • a (object) –

  • b (object) –

Return type

bool

testFeasibility(name, q)[source]

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

Parameters
  • name (str) –

  • q (object) –

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) –

Return type

bool

feasibilityFailures(q)[source]

Returns a list of all failed feasibility constraints.

Parameters

q (object) –

Return type

object

visibilityFailures(a, b)[source]

Returns a list of all failed visibility constraints.

Parameters
  • a (object) –

  • b (object) –

Return type

object

sample()[source]

Samples a configuration.

Return type

object

distance(a, b)[source]

Returns the distance between two configurations.

Parameters
  • a (object) –

  • b (object) –

Return type

float

interpolate(a, b, u)[source]

Interpolates between two configurations.

Parameters
  • a (object) –

  • b (object) –

  • u (float) –

Return type

object

adaptiveQueriesEnabled()[source]

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

Return type

bool

enableAdaptiveQueries(enabled=True)[source]

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

Parameters

enabled (bool, optional) – default value True

Return type

None

optimizeQueryOrder()[source]

Call this to optimize the feasibility / visibility testing order.

Return type

None

setFeasibilityDependency(name, precedingTest)[source]

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

Parameters
  • name (str) –

  • precedingTest (str) –

Return type

None

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.

Parameters
  • name (str) –

  • costPrior (float, optional) – default value 0.0

  • feasibilityProbability (float, optional) – default value 0.0

  • evidenceStrength (float, optional) – default value 1.0

Return type

None

setVisibilityDependency(name, precedingTest)[source]

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

Parameters
  • name (str) –

  • precedingTest (str) –

Return type

None

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.

Parameters
  • name (str) –

  • costPrior (float, optional) – default value 0.0

  • visibilityProbability (float, optional) – default value 0.0

  • evidenceStrength (float, optional) – default value 1.0

Return type

None

feasibilityCost(name)[source]

Retrieves the empirical average cost of a given feasibility test.

Parameters

name (str) –

Return type

float

feasibilityProbability(name)[source]

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

Parameters

name (str) –

Return type

float

visibilityCost(name)[source]

Retrieves the empirical average cost of a given visibility test.

Parameters

name (str) –

Return type

float

visibilityProbability(name)[source]

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

Parameters

name (str) –

Return type

float

feasibilityQueryOrder()[source]

Retrieves the current order of feasibility tests.

Return type

object

visibilityQueryOrder()[source]

Retrieves the current order of visibility tests.

Return type

object

getStats()[source]

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

Return type

object

property index

index : int

class klampt.plan.motionplanning.PlannerInterface(cspace)[source]

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) –

property thisown

The membership flag

destroy()[source]
Return type

None

setEndpoints(start, goal)[source]
Parameters
  • start (object) –

  • goal (object) –

Return type

bool

setEndpointSet(start, goal, goalSample=None)[source]
Parameters
  • start (object) –

  • goal (object) –

  • goalSample (object, optional) – default value None

Return type

bool

setCostFunction(edgeCost=None, terminalCost=None)[source]
Parameters
  • edgeCost (object, optional) – default value None

  • terminalCost (object, optional) – default value None

Return type

None

addMilestone(milestone)[source]
Parameters

milestone (object) –

Return type

int

getClosestMilestone(config)[source]
Parameters

config (object) –

Return type

int

getMilestone(arg2)[source]
Parameters

arg2 (int) –

Return type

object

planMore(iterations)[source]
Parameters

iterations (int) –

Return type

None

getSolutionPath()[source]
Return type

object

getPath(*args)[source]

getPath (milestone1,milestone2): object

getPath (milestone1,goalMilestones): object

Parameters
  • milestone1 (int) –

  • milestone2 (int, optional) –

  • goalMilestones (std::vector"< int,std::allocator< int > >", optional) –

Returns

Return type

object

Return type

object

getData(setting)[source]
Parameters

setting (str) –

Return type

float

getStats()[source]
Return type

object

getRoadmap()[source]
Return type

object

dump(fn)[source]
Parameters

fn (str) –

Return type

None

property index

index : int

property spaceIndex

spaceIndex : int

klampt.plan.motionplanning.interpolate_1d_min_time(x0, v0, x1, v1, xmin, xmax, vmax, amax)[source]
Return type

Tuple[Sequence[float], Sequence[float], Sequence[float]]

klampt.plan.motionplanning.interpolate_1d_min_accel(x0, v0, x1, v1, endTime, xmin, xmax, vmax)[source]
Return type

Tuple[Sequence[float], Sequence[float], Sequence[float]]

klampt.plan.motionplanning.interpolate_nd_min_time(x0, v0, x1, v1, xmin, xmax, vmax, amax)[source]
Return type

Tuple[Sequence[Sequence[float]], Sequence[Sequence[float]], Sequence[Sequence[float]]]

klampt.plan.motionplanning.interpolate_nd_min_accel(x0, v0, x1, v1, endTime, xmin, xmax, vmax)[source]
Return type

Tuple[Sequence[Sequence[float]], Sequence[Sequence[float]], Sequence[Sequence[float]]]

klampt.plan.motionplanning.interpolate_nd_min_time_linear(x0, x1, vmax, amax)[source]
Return type

Tuple[Sequence[float], Sequence[Sequence[float]], Sequence[Sequence[float]]]

klampt.plan.motionplanning.brake_1d(x0, v0, amax)[source]
Return type

Tuple[Sequence[float], Sequence[float], Sequence[float]]

klampt.plan.motionplanning.brake_nd(x0, v0, xmin, xmax, amax)[source]
Return type

Tuple[Sequence[Sequence[float]], Sequence[Sequence[float]], Sequence[Sequence[float]]]

klampt.plan.motionplanning.combine_nd_cubic(times, positions, velocities)[source]
Return type

Tuple[Sequence[float], Sequence[Sequence[float]], Sequence[Sequence[float]]]

klampt.plan.motionplanning.setPlanJSONString(*args, **kwargs)

Deprecated in a future version of Klampt. Use set_plan_json_string instead

klampt.plan.motionplanning.interpolateNDMinTime(*args, **kwargs)

Deprecated in a future version of Klampt. Use interpolate_nd_min_time instead

klampt.plan.motionplanning.setPlanType(*args, **kwargs)

Deprecated in a future version of Klampt. Use set_plan_type instead

klampt.plan.motionplanning.interpolate1DMinTime(*args, **kwargs)

Deprecated in a future version of Klampt. Use interpolate_1d_min_time instead

klampt.plan.motionplanning.combineNDCubic(*args, **kwargs)

Deprecated in a future version of Klampt. Use combine_nd_cubic instead

klampt.plan.motionplanning.setPlanSetting(*args, **kwargs)

Deprecated in a future version of Klampt. Use set_plan_setting instead

klampt.plan.motionplanning.interpolate1DMinAccel(*args, **kwargs)

Deprecated in a future version of Klampt. Use interpolate_1d_min_accel instead

klampt.plan.motionplanning.setRandomSeed(*args, **kwargs)

Deprecated in a future version of Klampt. Use set_random_seed instead

klampt.plan.motionplanning.interpolateNDMinTimeLinear(*args, **kwargs)

Deprecated in a future version of Klampt. Use interpolate_nd_min_time_linear instead

klampt.plan.motionplanning.getPlanJSONString(*args, **kwargs)

Deprecated in a future version of Klampt. Use get_plan_json_string instead

klampt.plan.motionplanning.interpolateNDMinAccel(*args, **kwargs)

Deprecated in a future version of Klampt. Use interpolate_1d_min_accel instead