klampt.plan.robotcspace module

Classes:

`RobotCSpace`(robot[, collider])	A basic robot cspace that allows collision free motion.
`ClosedLoopRobotCSpace`(robot, iks[, collider])	A closed loop cspace.
`ImplicitManifoldRobotCSpace`(robot, ...[, ...])	A closed loop cspace with an arbitrary numerical manifold f(q)=0 to constrain the robot's motion.
`EmbeddedRobotCSpace`(ambientspace, subset[, ...])	A basic robot cspace that allows collision free motion of a subset of joints.

class klampt.plan.robotcspace.RobotCSpace(robot, collider=None)[source]

Bases: CSpace

A basic robot cspace that allows collision free motion.

Parameters:

robot (RobotModel) – the robot that’s moving.
collider (WorldCollider, optional) –
a collide.WorldCollider instance instantiated with the world in which the robot lives. Any ignored collisions in the collider will be respected in the feasibility tests of this CSpace.

If this is not provided, then only self-collisions will be checked.

Warning

If your robot has non-standard joints, like a free- floating base or continuously rotating (spin) joints, you may need to overload the sample() method. The default implementation assumes that everything with unbounded limits is a rotational joint.

Methods:

`addConstraint`(checker[, name])
`sample`()	Overload this to implement custom sampling strategies or to handle non-standard joints.
`inJointLimits`(x)	Checks joint limits of the configuration x
`selfCollision`([x])	Checks whether the robot at its current configuration is in self collision
`envCollision`([x])	Checks whether the robot at its current configuration is in collision with the environment.
`interpolate`(a, b, u)
`distance`(a, b)
`executablePath`(path)	Given a planned CSpace path, returns a path that can be executed using `klampt.model.trajectory.execute_path()`.

addConstraint(checker, name=None)[source]

sample()[source]

Overload this to implement custom sampling strategies or to handle non-standard joints. This one will handle spin joints and rotational axes of floating bases.

Return type:: Sequence[float]

inJointLimits(x)[source]

Checks joint limits of the configuration x

Return type:: bool

selfCollision(x=None)[source]

Checks whether the robot at its current configuration is in self collision

Return type:: bool

envCollision(x=None)[source]

Checks whether the robot at its current configuration is in collision with the environment.

Return type:: bool

interpolate(a, b, u)[source]

distance(a, b)[source]

executablePath(path)[source]

Given a planned CSpace path, returns a path that can be executed using klampt.model.trajectory.execute_path().

Return type:: List[Sequence[float]]

class klampt.plan.robotcspace.ClosedLoopRobotCSpace(robot, iks, collider=None)[source]

Bases: RobotCSpace

A closed loop cspace. Allows one or more IK constraints to be maintained during the robot’s motion.

solver

the solver containing all IK constraints

Type:: IKSolver

maxIters

the maximum number of iterations for numerical IK solver

Type:: int

tol

how closely the IK constraint must be met, in meters and/ or radians

Type:: float

To satisfy the IK constraint, the motion planner ensures that configuration samples are projected to the manifold of closed-loop IK solutions. To create edges between samples a and b, the straight line path a and b is projected to the manifold via an IK solve.

Methods:

`setIKActiveDofs`(activeSet)	Marks that only a subset of the DOFs of the robot are to be used for solving the IK constraint.
`sample`()	Samples directly on the contact manifold.
`sampleneighborhood`(c, r)	Samples a neighborhood in ambient space and then projects onto the contact manifold.
`solveConstraints`(x)	Given an initial configuration of the robot x, attempts to solve the IK constraints given in this space.
`closedLoop`([config, tol])	Returns true if the closed loop constraint has been met at config, or if config==None, the robot's current configuration.
`interpolate`(a, b, u)	Interpolates on the manifold.
`interpolationPath`(a, b[, epsilon])	Creates a discretized path on the contact manifold between the points a and b, with resolution epsilon.
`discretizePath`(path[, epsilon])	Given a `CSpace` path `path`, generates a path that satisfies closed-loop constraints up to the given distance between milestones.
`executablePath`(path[, epsilon])	Given a `CSpace` path, returns a path that can be executed using `klampt.model.trajectory.execute_path()`.

setIKActiveDofs(activeSet)[source]

Marks that only a subset of the DOFs of the robot are to be used for solving the IK constraint.

Parameters:: activeSet (list of int) – the robot DOF indices that should be active.

sample()[source]

Samples directly on the contact manifold. The basic method samples arbitrarily in the configuration space and then solves IK constraints.

Note that this may be an ineffective method especially for floating-base robots, since the floating joints may be sampled arbitrarily. To maximize performance, better problem-speciifc sampling distributions should be implemented by a subclass, if possible.

Return type:: Sequence[float]

sampleneighborhood(c, r)[source]

Samples a neighborhood in ambient space and then projects onto the contact manifold.

Return type:: Sequence[float]

solveConstraints(x)[source]

Given an initial configuration of the robot x, attempts to solve the IK constraints given in this space. Return value is the best configuration found via local optimization.

Return type:: Sequence[float]

closedLoop(config=None, tol=None)[source]

Returns true if the closed loop constraint has been met at config, or if config==None, the robot’s current configuration.

Return type:: bool

interpolate(a, b, u)[source]

Interpolates on the manifold. Used by edge collision checking

Return type:: Sequence[float]

interpolationPath(a, b, epsilon=0.01)[source]: Creates a discretized path on the contact manifold between the points a and b, with resolution epsilon.

discretizePath(path, epsilon=0.01)[source]

Given a CSpace path path, generates a path that satisfies closed-loop constraints up to the given distance between milestones.

Return type:: List[Sequence[float]]

executablePath(path, epsilon=0.01)[source]

Given a CSpace path, returns a path that can be executed using klampt.model.trajectory.execute_path(). This is the same as discretizePath, but is provided for compatibility with the RobotCSpace interface. :rtype: List[Sequence[float]]

Note

This isn’t the best thing to do for robots with slow acceleration limits and/or high inertias because it ignores acceleration. A better solution can be found in the MInTOS package or the C++ code in Klampt/Cpp/Planning/RobotTimeScaling.h.

Returns:: A closely spaced sequence of configurations discretized at the given resolution.
Return type:: list of Configs

class klampt.plan.robotcspace.ImplicitManifoldRobotCSpace(robot, implicitConstraint, collider=None)[source]

Bases: RobotCSpace

A closed loop cspace with an arbitrary numerical manifold f(q)=0 to constrain the robot’s motion. The argument implicitConstraint should be a function f(q) returning a list of values that should be equal to 0 up to the given tolerance. Essentially this is a ClosedLoopRobotCSpace except with a user-provided function.

See ClosedLoopRobotCSpace.

Methods:

`sample`()	Samples directly on the contact manifold
`onManifold`(x[, tol])	Returns true if the manifold constraint has been met at x.
`solveManifold`(x[, tol, maxIters])	Solves the manifold constraint starting from x, to the given tolerance and with the given maximum iteration count.
`interpolate`(a, b, u)	Interpolates on the manifold.

sample()[source]

Samples directly on the contact manifold

Return type:: Sequence[float]

onManifold(x, tol=None)[source]

Returns true if the manifold constraint has been met at x.

Return type:: bool

solveManifold(x, tol=None, maxIters=None)[source]: Solves the manifold constraint starting from x, to the given tolerance and with the given maximum iteration count. Default uses the values set as attributes of this class.

interpolate(a, b, u)[source]: Interpolates on the manifold. Used by edge collision checking

class klampt.plan.robotcspace.EmbeddedRobotCSpace(ambientspace, subset, xinit=None)[source]

Bases: EmbeddedCSpace

A basic robot cspace that allows collision free motion of a subset of joints. The subset is given by the indices in the list “subset” provided to the constructor. The configuration space is R^k where k is the number of DOFs in the subset.

Parameters:

ambientspace (RobotCSpace) – a RobotCSpace, ClosedLoopRobotCSpace, etc.
subset (list of ints) – the indices of moving DOFs
xinit (configuration, optional) – the reference configuration, or None to use the robot’s current configuration as the reference.

Methods:

`disableInactiveCollisions`()	This modifies the collider in ambientspace to only check collisions between moving pairs.
`discretizePath`(path[, epsilon])	Only useful for ClosedLoopRobotCSpace
`executablePath`(path)	Sends a planned path so that it is executed correctly by the controller (assumes a fully actuated robot).

disableInactiveCollisions()[source]: This modifies the collider in ambientspace to only check collisions between moving pairs. Should be called before setup() in most cases.

discretizePath(path, epsilon=0.01)[source]: Only useful for ClosedLoopRobotCSpace

executablePath(path)[source]

Sends a planned path so that it is executed correctly by the controller (assumes a fully actuated robot).

Return type:

List[Sequence[float]]

Parameters:

path (list of Configs) – a path in the embedded space or the ambient space, as returned by a planner.
controller (SimRobotController) – the robot’s controller