klampt.plan.motionplanning module

Python interface to C++ motion planning routines

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

Bases: object

property thisown

The membership flag

value()

Return type:

object

incr(n=1)

Return type:

Iterator

decr(n=1)

Return type:

Iterator

distance(x)

Return type:

int

equal(x)

Return type:

bool

copy()

Return type:

Iterator

next()

Return type:

object

previous()

Return type:

object

advance(n)

Return type:

Iterator

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

Bases: object

property thisown

The membership flag

iterator()

Return type:

Iterator

pop()

Return type:

float

append(x)

Return type:

None

empty()

Return type:

bool

size()

Return type:

int

swap(v)

Return type:

None

begin()

Return type:

Iterator

end()

Return type:

Iterator

rbegin()

Return type:

Iterator

rend()

Return type:

Iterator

clear()

Return type:

None

get_allocator()

pop_back()

Return type:

None

erase(*args)

Return type:

Iterator

push_back(x)

Return type:

None

front()

Return type:

float

back()

Return type:

float

assign(n, x)

Return type:

None

resize(*args)

Return type:

None

insert(*args)

Return type:

None

reserve(n)

Return type:

None

capacity()

Return type:

int

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

Bases: object

property thisown

The membership flag

iterator()

Return type:

Iterator

pop()

Return type:

Sequence[float]

append(x)

Return type:

None

empty()

Return type:

bool

size()

Return type:

int

swap(v)

Return type:

None

begin()

Return type:

Iterator

end()

Return type:

Iterator

rbegin()

Return type:

Iterator

rend()

Return type:

Iterator

clear()

Return type:

None

get_allocator()

pop_back()

Return type:

None

erase(*args)

Return type:

Iterator

push_back(x)

Return type:

None

front()

Return type:

Sequence[float]

back()

Return type:

Sequence[float]

assign(n, x)

Return type:

None

resize(*args)

Return type:

None

insert(*args)

Return type:

None

reserve(n)

Return type:

None

capacity()

Return type:

int

klampt.plan.motionplanning.set_random_seed(seed)

Sets the random seed used by the RobotModel.randomizeConfig() method and sampling-based motion planners.

Return type:

None

Parameters:

seed (int)

klampt.plan.motionplanning.set_plan_json_string(string)

Loads planner values from a JSON string.

Return type:

None

Parameters:

string (str)

klampt.plan.motionplanning.get_plan_json_string()

Saves planner values to a JSON string.

Return type:

str

klampt.plan.motionplanning.set_plan_type(type)

Sets the planner type.

Return type:

None

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.set_plan_setting(*args)

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

set_plan_setting (setting,value)

Return type:

None

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.destroy()

destroys internal data structures

Return type:

None

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

Bases: object

A raw interface for a configuration space.

Args:

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

Return type:

None

setFeasibility(pyFeas)

Return type:

None

Parameters:

pyFeas (object)

addFeasibilityTest(name, pyFeas)

Return type:

None

Parameters:

• name (str)

• pyFeas (object)

setVisibility(pyVisible)

Return type:

None

Parameters:

pyVisible (object)

addVisibilityTest(name, pyVisible)

Return type:

None

Parameters:

• name (str)

• pyVisible (object)

setVisibilityEpsilon(eps)

Return type:

None

Parameters:

eps (float)

setSampler(pySamp)

Return type:

None

Parameters:

pySamp (object)

setNeighborhoodSampler(pySamp)

Return type:

None

Parameters:

pySamp (object)

setDistance(pyDist)

Return type:

None

Parameters:

pyDist (object)

setInterpolate(pyInterp)

Return type:

None

Parameters:

pyInterp (object)

setProperty(key, value)

Return type:

None

Parameters:

• key (str)

• value (str)

getProperty(key)

Return type:

str

Parameters:

key (str)

isFeasible(q)

Queries whether a given configuration is feasible.

Return type:

bool

Parameters:

q (object)

isVisible(a, b)

Queries whether two configurations are visible.

Return type:

bool

Parameters:

• a (object)

• b (object)

testFeasibility(name, q)

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

Return type:

bool

Parameters:

• name (str)

• q (object)

testVisibility(name, a, b)

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

Return type:

bool

Parameters:

• name (str)

• a (object)

• b (object)

feasibilityFailures(q)

Returns a list of all failed feasibility constraints.

Return type:

object

Parameters:

q (object)

Return type:

object

visibilityFailures(a, b)

Returns a list of all failed visibility constraints.

Return type:

object

Parameters:

• a (object)

• b (object)

Return type:

object

sample()

Samples a configuration.

Return type:

object

Return type:

object

distance(a, b)

Returns the distance between two configurations.

Return type:

float

Parameters:

• a (object)

• b (object)

interpolate(a, b, u)

Interpolates between two configurations.

Return type:

object

Parameters:

• a (object)

• b (object)

• u (float)

Return type:

object

adaptiveQueriesEnabled()

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)

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

Return type:

None

Parameters:

enabled (bool, optional) – default value True

optimizeQueryOrder()

Call this to optimize the feasibility / visibility testing order.

Return type:

None

setFeasibilityDependency(name, precedingTest)

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

Return type:

None

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.

Return type:

None

Parameters:

• name (str)

• costPrior (float, optional) – default value 0.0

• feasibilityProbability (float, optional) – default value 0.0

• evidenceStrength (float, optional) – default value 1.0

setVisibilityDependency(name, precedingTest)

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

Return type:

None

Parameters:

• name (str)

• precedingTest (str)

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.

Return type:

None

Parameters:

• name (str)

• costPrior (float, optional) – default value 0.0

• visibilityProbability (float, optional) – default value 0.0

• evidenceStrength (float, optional) – default value 1.0

feasibilityCost(name)

Retrieves the empirical average cost of a given feasibility test.

Return type:

float

Parameters:

name (str)

feasibilityProbability(name)

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

Return type:

float

Parameters:

name (str)

visibilityCost(name)

Retrieves the empirical average cost of a given visibility test.

Return type:

float

Parameters:

name (str)

visibilityProbability(name)

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

Return type:

float

Parameters:

name (str)

feasibilityQueryOrder()

Retrieves the current order of feasibility tests.

Return type:

object

Return type:

object

visibilityQueryOrder()

Retrieves the current order of visibility tests.

Return type:

object

Return type:

object

getStats()

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

Return type:

object

Return type:

object

property index

int

Type:

index

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.

Args:

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

Return type:

None

setEndpoints(start, goal)

Return type:

bool

Parameters:

• start (object)

• goal (object)

setEndpointSet(start, goal, goalSample=None)

Return type:

bool

Parameters:

• start (object)

• goal (object)

• goalSample (object, optional) – default value None

setCostFunction(edgeCost=None, terminalCost=None)

Return type:

None

Parameters:

• edgeCost (object, optional) – default value None

• terminalCost (object, optional) – default value None

addMilestone(milestone)

Return type:

int

Parameters:

milestone (object)

getClosestMilestone(config)

Return type:

int

Parameters:

config (object)

getMilestone(arg2)

Return type:

object

Parameters:

arg2 (int)

Return type:

object

planMore(iterations)

Return type:

None

Parameters:

iterations (int)

getSolutionPath()

Return type:

object

Return type:

object

getPath(*args)

getPath (milestone1,milestone2): object

getPath (milestone1,goalMilestones): object

Return type:

object

Parameters:

• milestone1 (int)

• milestone2 (int, optional)

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

Return type:

object

getData(setting)

Return type:

float

Parameters:

setting (str)

getStats()

Return type:

object

Return type:

object

getRoadmap()

Return type:

object

Return type:

object

dump(fn)

Return type:

None

Parameters:

fn (str)

property index

int

Type:

index

property spaceIndex

int

Type:

spaceIndex

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

Return type:

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

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

Return type:

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

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

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)

Return type:

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

klampt.plan.motionplanning.interpolate_nd_min_time_linear(x0, x1, vmax, amax)

Return type:

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

klampt.plan.motionplanning.brake_1d(x0, v0, amax)

Return type:

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

klampt.plan.motionplanning.brake_nd(x0, v0, xmin, xmax, amax)

Return type:

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

klampt.plan.motionplanning.combine_nd_cubic(times, positions, velocities)

Return type:

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

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

Deprecated in a future version of Klampt. Use combine_nd_cubic instead

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

Deprecated in a future version of Klampt. Use get_plan_json_string instead

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

Deprecated in a future version of Klampt. Use interpolate_1d_min_accel instead

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

Deprecated in a future version of Klampt. Use interpolate_1d_min_time instead

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

Deprecated in a future version of Klampt. Use interpolate_1d_min_accel instead

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

Deprecated in a future version of Klampt. Use interpolate_nd_min_time instead

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

Deprecated in a future version of Klampt. Use interpolate_nd_min_time_linear instead

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

Deprecated in a future version of Klampt. Use set_plan_json_string instead

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

Deprecated in a future version of Klampt. Use set_plan_setting instead

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

Deprecated in a future version of Klampt. Use set_plan_type instead

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

Deprecated in a future version of Klampt. Use set_random_seed instead