klampt.plan.cspaceutils module

General utilities for creating and operating with C-spaces.

Functions:

default_sampleneighborhood(c, r)
default_visible(a, b)
default_distance(a, b)
default_interpolate(a, b, u)
make_default(space)	Helper: makes a space's callbacks perform the default Cartesian space operations.

Classes:

CompositeCSpace(spaces)	A cartesian product of multiple spaces, given as a list upon construction.
EmbeddedCSpace(ambientspace, subset[, xinit])	A subspace of an ambient space, with the active DOFs given by a list of DOF indices of that ambient space.
AffineEmbeddedCSpace(ambientspace, A[, b])	A subspace of an ambient space, with the active DOFs given by variables x that are transformed to the ambient space by the transform x_amb = A*x+b.
SubsetMotionPlan(space, subset, q0[, type])	An adaptor that "lifts" a motion planner in an EmbeddedCSpace to a higher dimensional ambient space.
EmbeddedMotionPlan(space, q0[, type])	An adaptor that "lifts" a motion planner in an EmbeddedCSpace or AffineEmbeddedCSpace to a higher dimensional ambient space.

klampt.plan.cspaceutils.default_sampleneighborhood(c, r)

klampt.plan.cspaceutils.default_visible(a, b)

klampt.plan.cspaceutils.default_distance(a, b)

klampt.plan.cspaceutils.default_interpolate(a, b, u)

klampt.plan.cspaceutils.make_default(space)

Helper: makes a space’s callbacks perform the default Cartesian space operations.

class klampt.plan.cspaceutils.CompositeCSpace(spaces)

Bases: CSpace

A cartesian product of multiple spaces, given as a list upon construction. The feasible method can be overloaded to include interaction tests.

Methods:

subDims()
split(x)
join(xs)
feasible(x)	Overload this to define your new feasibility test.
sample()	Overload this to define a nonuniform sampler.

subDims()

split(x)

join(xs)

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.

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.

class klampt.plan.cspaceutils.EmbeddedCSpace(ambientspace, subset, xinit=None)

Bases: CSpace

A subspace of an ambient space, with the active DOFs given by a list of DOF indices of that ambient space.

Note

A MotionPlan constructed on this object operates in the embedded space, NOT the ambient space. To push endpoints to the embedded space you will need to call EmbeddedCSpace.project(), and to pull a plan back to the ambient space, use the EmbeddedCSpace.liftPlan() method.

To make this more convenient, the SubsetMotionPlan class is provided for you. EmbeddedMotionPlan does the same thing.

Note

Sampling does not work directly when the ambient space has implicit manifold constraints, e.g., closed-loop constraints. sample and sampleneighborhood will need to be customized so that the constraint solving is done without perturbing the seed configuration except for the dofs in the moving subset.

ambientspace

the ambient configuration space

Type:

CSpace

mapping

the list of active indices into the ambient configuration space

Type:

list

xinit

the initial configuration in the ambient space (by default, 0 vector)

Type:

list, optional

Methods:

project(xamb)	Ambient space -> embedded space
lift(xemb)	Embedded space -> ambient space
liftPath(path)	Given a CSpace path, lifts it to a full ambient C-space path
projectPath(path_amb)	Given an ambient C-space path, projects it to a C-space path
feasible(x)	Overload this to define your new feasibility test.
sample()	Overload this to define a nonuniform sampler.

project(xamb)

Ambient space -> embedded space

lift(xemb)

Embedded space -> ambient space

liftPath(path)

Given a CSpace path, lifts it to a full ambient C-space path

projectPath(path_amb)

Given an ambient C-space path, projects it to a C-space path

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.

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.

class klampt.plan.cspaceutils.AffineEmbeddedCSpace(ambientspace, A, b=None)

Bases: CSpace

A subspace of an ambient space, with the active DOFs given by variables x that are transformed to the ambient space by the transform x_amb = A*x+b. Here A is a sparse matrix.

Note

A MotionPlan constructed on this object operates in the embedded space, NOT the ambient space. To push endpoints to the embedded space you will need to call AffineEmbeddedCSpace.project(), and to pull a plan back to the ambient space, use the AffineEmbeddedCSpace.liftPlan() method.

To make this more convenient, the EmbeddedMotionPlan class is provided for you.

Note

Sampling does not work directly when the ambient space has implicit manifold constraints, e.g., closed-loop constraints. sample and sampleneighborhood will need to be customized so that the constraint solving is done without perturbing the seed configuration except for the dofs in the moving subset.

ambientspace

the ambient configuration space

Type:

CSpace

A

the map from embedded space to ambient space

Type:

list of dict, numpy array, or scipy sparse matrix

b

the offset in the ambient space (by default, the 0 vector)

Type:

list, optional

Methods:

fromRobotDrivers(robot, ambientspace[, drivers])	Creates an AffineEmbeddedCSpace for a robot's drivers.
project(xamb)	Ambient space -> embedded space
lift(xemb)	Embedded space -> ambient space
liftPath(path)	Given a CSpace path, lifts it to a full ambient C-space path
projectPath(path_amb)	Given an ambient C-space path, projects it to a C-space path
feasible(x)	Overload this to define your new feasibility test.
sample()	Overload this to define a nonuniform sampler.

static fromRobotDrivers(robot, ambientspace, drivers=None)

Creates an AffineEmbeddedCSpace for a robot’s drivers.

Parameters:

• robot (RobotModel) – the robot that you’d like to move

• ambientspace (CSpace) – an ambient space on the robot’s full DOFs, e.g., RobotCSpace.

• drivers (list of int or list of str, optional) – if provided, gives a list of robot’s active drivers.

project(xamb)

Ambient space -> embedded space

lift(xemb)

Embedded space -> ambient space

liftPath(path)

Given a CSpace path, lifts it to a full ambient C-space path

projectPath(path_amb)

Given an ambient C-space path, projects it to a C-space path

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.

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.

class klampt.plan.cspaceutils.SubsetMotionPlan(space, subset, q0, type=None, **options)

Bases: MotionPlan

An adaptor that “lifts” a motion planner in an EmbeddedCSpace to a higher dimensional ambient space. Used for planning in subsets of robot DOFs.

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)

Methods:

project(xamb)	Ambient space -> embedded space
lift(xemb)	Embedded space -> ambient space
setEndpoints(start, goal)	Takes care of projecting the start and goal (represented in the ambient space) down to the subset.
addMilestone(x)	Manually adds a milestone from the ambient space, and returns its index
getPath([milestone1, milestone2])	Lifts the motion planner's lower-dimensional path back to the ambient space
getRoadmap()	Lifts the motion planner's lower-dimensional roadmap back to the ambient space

project(xamb)

Ambient space -> embedded space

lift(xemb)

Embedded space -> ambient space

setEndpoints(start, goal)

Takes care of projecting the start and goal (represented in the ambient space) down to the subset. Works with both config and set goals.

addMilestone(x)

Manually adds a milestone from the ambient space, and returns its index

getPath(milestone1=None, milestone2=None)

Lifts the motion planner’s lower-dimensional path back to the ambient space

getRoadmap()

Lifts the motion planner’s lower-dimensional roadmap back to the ambient space

class klampt.plan.cspaceutils.EmbeddedMotionPlan(space, q0, type=None, **options)

Bases: MotionPlan

An adaptor that “lifts” a motion planner in an EmbeddedCSpace or AffineEmbeddedCSpace to a higher dimensional ambient space.

Used for planning in robots with frozen DOFs (EmbeddedCSpace) or “affine” drivers (AffineEmbeddedCSpace).

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)

Methods:

setEndpoints(start, goal)	Takes care of projecting the start and goal (represented in the ambient space) down to the subset.
addMilestone(x)	Manually adds a milestone from the ambient space, and returns its index
getPath([milestone1, milestone2])	Lifts the motion planner's lower-dimensional path back to the ambient space
getRoadmap()	Lifts the motion planner's lower-dimensional roadmap back to the ambient space
setCostFunction([edgeCost, terminalCost])	Sets edge / terminal costs from functions in the ambient space
pathCost(p)	Calculates the cost of a path in the ambient space.

setEndpoints(start, goal)

Takes care of projecting the start and goal (represented in the ambient space) down to the subset. Works with both config and set goals.

addMilestone(x)

Manually adds a milestone from the ambient space, and returns its index

getPath(milestone1=None, milestone2=None)

Lifts the motion planner’s lower-dimensional path back to the ambient space

getRoadmap()

Lifts the motion planner’s lower-dimensional roadmap back to the ambient space

setCostFunction(edgeCost=None, terminalCost=None)

Sets edge / terminal costs from functions in the ambient space

pathCost(p)

Calculates the cost of a path in the ambient space.