klampt.plan.cspace module

This module provides convenient access to the motionplanning module functionality by defining the CSpace and MotionPlan classes.

class klampt.plan.cspace.CSpace

Bases: object

Used alongside MotionPlan to define a configuration space for motion planning.

eps

the collision tolerance used for checking edges, in the units defined by the distance(a,b) method. (by default, euclidean distance)

Type:

float

bound

a list of lower and upper bounds on the space [(l1,u1),…,(ld,ud)] where d is the dimension of the configuration space.

Type:

float

properties

a map of properties that may be used by a planner. See below documentation for more detail.

Type:

dict

cspace

a motionplanning module CSpaceInterface object

Type:

CSpaceInterface, internal

feasibilityTests

a list of one-argument functions that test feasibility of this configuration space’s constraints. E.g., if you have a collision tester that returns True if a configuration is in collision, and also want to check bounds, you can set this to a list: [self.in_bounds,lambda x not self.collides(x)] You should not write this directly but instead use addFeasibilityTest.

Type:

list of functions, internal

feasibilityTestNames

a list of names of feasibility tests. You should not write this directly but instead use addFeasibilityTest.

Type:

list of strs, internal

To define a custom CSpace, subclasses may optionally override:

• feasible(x): returns true if the vector x is in the feasible space. By default calls each function in self.feasibilityTests, which by default only tests bound constraints.

• sample(): returns a new vector x from a superset of the feasible space

• sampleneighborhood(c,r) (optional): returns a new vector x from a neighborhood of c with radius r

• visible(a,b): returns true if the path between a and b is feasible

• distance(a,b): return a distance between a and b

• interpolate(a,b,u): interpolate between a, b with parameter u

To avoid memory leaks, CSpace.close() or motionplanning.destroy() must be called when you are done. (The latter deallocates all previously created cspaces and planners)

If sample() is not defined, then subclasses should set self.bound to be a list of pairs defining an axis-aligned bounding box. The setBounds method is a convenient way of defining this.

If visible() is not defined, then paths are checked by subdivision, with the collision tolerance self.eps.

To help planners know a bit more about the CSpace, you can set the self.properties member to a map from strings to values. Useful values are

• euclidean (0 or 1): indicates a euclidean space

• geodesic (0 or 1): indicates whether the interpolation is along geodesics.

• volume (real): a size of the space

• minimum, maximum (real array): bounds on the space

• metric (string): the metric used by distance, can be “euclidean”, “weighted euclidean”, “manhattan”, “weighted manhattan”, “Linf”, etc.

• metricWeights (real array): weights used by weighted metrics

volume is necessary for Lazy PRM* and Lazy RRG* to work.

metricWeights is necessary for KD-tree point location structures to work, for FMM methods to work, etc.

minimum/maximum can be used by grid-based methods (optional for FMM, FMM*).

setBounds(bound)

Convenience function: sets the sampling bound and the space properties in one line.

close()

This method must be called to free the memory associated with the planner. Alternatively, motionplanning.destroy() can be called to free all previously constructed CSpace and MotionPlan objects.

setup(reinit=False)

Called internally by the MotionPlan class to set up planning hooks.

If reinit is not set to True, and the setup() method has been called before, a warning message will be printed. Set it to True to suppress this message.

sample()

Overload this to define a nonuniform sampler. By default, it will sample from the axis-aligned bounding box defined by self.bound. To define a different domain, set self.bound to the desired bound.

sampleneighborhood(c, r)

Overload this to define a nonuniform sampler. By default, it will sample from the axis-aligned box of radius r around c, but clamped to the bound.

addFeasibilityTest(func, name=None, dependencies=None)

Adds a new feasibility test with the given function func(x) and the specified name. If name is not provided (default) a default name is generated.

If dependencies is provided, it can be a string or a list of strings, indicating that this test must be called after some other test(s).

inBounds(x)

Returns true if x is within the given bounds

feasible(x)

Overload this to define your new feasibility test. By default the implementation simply tests the bounds constraint, or if self.feasibilityTests is not empty, tests each function in self.feasibilityTests.

isFeasible(x)

An overload for self.cspace.isFeasible. Use this to test feasibility of a configuration (rather than feasible()) if you wish to take advantage of adaptive feasibility testing and constraint testing statistics.

isVisible(x, y)

An overload for self.cspace.isVisible. Use this to test visibility of a line (rather than visible()) if you want to use the natural visibility tester, wish to take advantage of adaptive visibility testing, or want to use constraint testing statistics.

getStats()

Returns a dictionary mapping statistic names to values. Result contains fraction of feasible configurations, edges, etc. If feasibility tests are individually specified, returns stats for individual tests as well.

class klampt.plan.cspace.MotionPlan(space, type=None, **options)

Bases: object

A motion planner instantiated on a space. Currently supports only kinematic, point-to-point, or point-to-set plans.

Planner parameters must be set by calling the static MotionPlan.setOptions(param1=value1,param2=value2,…) method BEFORE calling the MotionPlan(space,type) constructor.

If type is not specified in the constructor, the planning algorithm will be chosen by default.

Note that MotionPlan.close() or motionplanning.destroy() must be called to free memory after you are done.

Multi-query roadmaps are supported for the PRM and SBLPRT algorithms. In multi-query mode, you may call addMilestone(q) to add a new milestone. addMilestone() returns the milestone’s index, which can be used in later calls to getPath().

Cost functions are supported by any restart, shortcutting, or goal set planners, as well as PRM, RRT. RRT*, PRM*, Lazy RRG*, Lazy PRM*. However, be aware that the X* planners are internally trying to optimize path length, and the result may not be asymptotically optimal for other cost functions.

Initializes a plan with a given CSpace and a given type. Optionally, planner options can be set via keyword arguments.

Valid values for type 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

(this list may be out-of-date; the most current documentation is listed in src/motionplanning.h)

close()

This method must be called to free the memory associated with the planner. Alternatively, motionplanning.destroy() can be called to free all previously constructed CSpace and MotionPlan objects.

static setOptions(**opts)

Sets a numeric or string-valued setting for the next constructed planner. Arguments are specified by key-value pair.

Valid keys are:

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

• “connectionThreshold”: a milestone connection threshold

• “perturbationRadius”: (for RRT and SBL)

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

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

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

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

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

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

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

• “pointLocation”: a string designating a point location data structure. “kdtree” is supported, optionally followed by a weight vector (used in PRM, RRT, 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}”)

(this list may be out-of-date; the most current documentation is listed in Klampt/Python/klampt/src/motionplanning.h)

An exception may be thrown if an invalid setting is chosen.

setEndpoints(start, goal)

Sets the start and goal configuration or goal condition.

Parameters:

• start (list of floats) – start configuration

• goal (list of floats, function, or pair of functions) –

  defines the goal condition. Can be:

  • a configuration: performs point-to-point planning.

  • a function: the goal is a set, and goal is a 1-argument function f(q) that returns true if the configuration q is at in the goal set and false otherwise.

  • a pair of functions (f,s): f(q) tests whether the configuration is at the goal, and s() generates a new sample in the goal set.

setCostFunction(edgeCost=None, terminalCost=None)

Sets a cost function to be used when retrieving a solution path. Some planners cannot accept objective functions.

The total cost of a path x0,x1,…,xn is:

edgeCost(x0,x1) + edgeCost(x1,x2) + … + edgeCost(xn-1,xn) + terminalCost(xn)

Parameters:

• edgeCost (function, optional) – has signature f(a,b)->float where a,b are configurations.

• terminalCost (function, optional) – has signature f(q)->float where q is a configuration.

addMilestone(x)

Manually adds a milestone at configuration x and returns its index

getClosestMilestone(x)

Returns the index of the closest milestone to configuration x

planMore(iterations)

Performs a given number of iterations of planning.

getPath(milestone1=None, milestone2=None)

Returns the path between the two milestones. If no arguments are provided, this returns the optimal path between the start and goal.

Parameters:

• milestone1 (int, optional) – the start milestone

• milestone2 (int or list of int, optional) – the goal milestone. If this is a list, the optimal path to some goal milestone in this list is returned.

Returns:

The result path, given as a list of configurations. Each configuration is a Python list.

Note that for non-euclidean spaces (e.g., closed loop, SE2, or SE3 spaces) the CSpace’s interpolate() function must be used between milestones to properly interpolate along the path.

Return type:

list of configurations

getRoadmap()

Returns a graph (V,E) containing the planner’s roadmap.

V is a list of configurations (each configuration is a list of floats) and E is a list of edges (each edge is a pair (i,j) indexing into V).

getStats()

Returns a dictionary mapping statistic names to values. Result is planner-dependent

pathCost(path)

Helper function to calculate the cost of a path. If no cost function was previously set with setCostFunction, this is just the CSpace length.

klampt.plan.cspace.optimizingPlanners = {'fmm*', 'lazyprm*', 'lazyrrg*', 'prm*', 'rrt*'}

The set of natively optimizing planners.

Goal set planners, random-restart, and shortcut planners also support optimization

Type:

set

klampt.plan.cspace.costAcceptingPlanners = {'lazyprm*', 'lazyrrg*', 'prm', 'prm*', 'rrt', 'rrt*'}

The set of planners that natively accept costs.

Goal set planners, random-restart, and shortcut planners also support costs.

Type:

set

klampt.plan.cspace.configurePlanner(*args, **kwargs)

klampt.plan.cspace.configure_planner(space, start, goal, edgeCost=None, terminalCost=None, optimizing=True, type='auto', stepsize=None, knn=10, shortcut='auto', restart='auto', restartIters=1000, pointLocation='auto', **otherSettings)

Automatically sets up a MotionPlan with reasonable options, double checking if the options are compatible with the given inputs.

Parameters:

• space (CSpace) – the space you’d like to plan for.

• start (list of floats) – the start configuration

• goal (list of floats or function or (function,function) tuple) – the goal configuration or condition. See MotionPlan.setEndpoints().

• edgeCost (function, optional) – the edge cost. See MotionPlan.setCostFunction().

• terminalCost (function, optional) – the terminal cost. See MotionPlan.setCostFunction().

• optimizing (bool, optional) – whether you expect to be planning past the first path found to obtain a better solution.

• type (str, optional) – the planner type string. If ‘auto’, the planner type is set automatically.

• stepsize (float, optional) – if given, sets the growth radius or grid resolution of the planner. If not, this is auto-determined by the size of the space.

• knn (int, optional) – for prm planner, the number of nearest neighbors to test.

• shortcut (bool, optional) – whether to shortcut the resulting path

• restart (bool, optional) – whether to do random-restarts

• restartIters (int, optional) – how many iterations to run between restarts

• pointLocation (str, optional) – what point location data structure to use. By default, either ‘kdtree’ or ‘balltree’ are selected, depending on whether you space is assumed Cartesian or not.

• otherSettings (keyword dict, optional) – other MotionPlan keywords can be added to override any of the auto-determined settings.

Returns:

a pair giving the MotionPlan object that can be called to produce a plan, and a dictionary giving the relevant settings.

Return type:

(MotionPlan,dict)