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

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

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

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.

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.

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.

inBounds(x)

Returns true if x is within the given bounds

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.

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.

setBounds(bound)

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

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.

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

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

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

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.

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)

addMilestone(x)

Manually adds a milestone and returns its index

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.

getPath(milestone1=None, milestone2=None)

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

The path is a list of configurations (each configuration is a Python list).

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

planMore(iterations)

Performs a given number of iterations of planning.

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.

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*, 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.