klampt.model.ik module

Inverse kinematics methods. These use the underlying C++ solver in Klamp’t. Compared to the SWIG bindings in robotik.h, these helpers are more convenient (and Pythonic).

The IK solver is a Newton-Raphson solver, which acts like a local optimization. It starts from the robot model’s initial configuration and takes a number of iterations to seek the objective. The solution is placed in the robot model’s configuration. If successful, the solution is also guaranteed to satisfy the robot’s joint limits.

For basic IK, the calling sequence is:

link = robot.link("name")
goal = ik.objective(link,local=[p1,p2,p3],world=[r1,r2,r3])
robot.setConfig(startConfig)  #(optional, set the initial configuration)
if ik.solve(goal):
    print("Hooray, IK solved")
    print("Resulting config:",robot.getConfig())
else:
    print("IK failed")
    print("Final config:",robot.getConfig())

Here, the points p1,…,p3 on the link (specified in local coordinates) will be matched to the points r1,…,r3 in the world. You can also directly constrain the translation/rotation of the link via the form:

ik.objective(link,R=link.getTransform()[0],t=[x,y,z])

which will keep the orientation of the link constant, while setting its position.

A convenience function is ik.fixed_objective(link) which fixes the position of the link in its current position/orientation in world coordinates. Individual points on the link can be constrained to their coordinates using the local argument, as follows:

ik.fixed_objective(link,local=[p1,p2,p3])

or if you prefer to give world coordinates, like so:

ik.fixed_objective(link,world=[q1,q2,q3])

More advanced usage can constrain a link to another link using the ref argument to ik.objective(). You may also pass multiple goals to ik.solve().

For more control over the solution process, you may use a ik.solver() object. This object will let you set the active degrees of freedom, get the residual and Jacobian, and sample random initial solutions. (see klampt.robotsim.IKSolver)

  1. By default, the active degrees of freedom are all ancestors of the constrained links. For more control, set them using a solver object.

  2. As with any local IK method, Newton-Raphson has a tendency to get stuck in local minima. As a result you may wish to choose random initial configurations if the first solve fails.

Note

IK solving with constraints between robot links and rigid objects (GeneralizedIKObjective and GeneralizedIKSolver) is not yet implemented and will result in an exception. This functionality may be supported in the future but is not on the current development path.

Functions:

objective(body[, ref, local, world, R, t])

Returns an IKObjective for a given body.

fixed_objective(link[, ref, local, world])

Convenience function for fixing the given link at the current position in space.

fixed_rotation_objective(link[, ref, ...])

Convenience function for fixing the given link at its current orientation in space.

objects(objectives)

Returns a list of all objects touched by the given objective(s).

solver(objectives[, iters, tol, secondary])

Returns a solver for the given objective(s).

solve(objectives[, iters, tol, activeDofs, ...])

Attempts to solve the given objective(s).

residual(objectives)

Returns the residual of the given objectives.

jacobian(objectives)

Returns the jacobian of the given objectives.

solve_global(objectives[, iters, tol, ...])

Attempts to solve the given objective(s) but avoids local minima to some extent using a random-restart technique.

solve_nearby(objectives, maxDeviation[, ...])

Solves for an IK solution that does not deviate too far from the initial configuration.

Classes:

SubRobotIKSolver(r)

A version of IKSolver for subrobots.
klampt.model.ik.objective(body, ref=None, local=None, world=None, R=None, t=None)[source]

Returns an IKObjective for a given body.

There are two modes in which this can be used: :rtype: IKObjective

  1. If local and world are provided, then this objective asks to match the local point (s) on the body to the world point(s).

  2. If R and t are provided, then the objective is set to specify the transform of the body.

If ref is given, then this is the target frame; otherwise it’s the world frame. In mode 1, the world points are bound to this frame; in mode 2, the transformation from body to ref is specified as (R,t).

Parameters:

  • body (RobotModelLink or RigidObjectModel) – the link that should be constrained.

  • ref (RobotModelLink or RigidObjectModel, optional) – the link that body should be constrained to, or None for the world frame.

  • local (3-vector, or a list of 3-vectors, optional) – the local coordinates on body should be constrained to the corresponding points in world

  • world (3-vector, or a list of 3-vectors, optional) – the coordinates to which the points in local should be constrained. These are given in world coordinates (if ref`=None) or in `ref’s frame.

  • R (so3 element; list of 9 floats, optional) – the rotation that the link should take on.

  • t (3-vector, optional) – the translation that the link’s origin

  • on. (should take) –

Returns:

An IK objective describing the constraint.

If body and ref are not on the same robot, then a GeneralizedIKObjective may be returned. TODO: not implemented yet.

klampt.model.ik.fixed_objective(link, ref=None, local=None, world=None)[source]

Convenience function for fixing the given link at the current position in space.

The arguments are interpreted as follows: :rtype: IKObjective

  • If local and world are not provided, the entire link is constrained.

  • If only local is provided, these points are fixed to their current positions in space.

  • If only world is provided, the points on the link with the given world position are constrained in place.

Parameters:

  • link (RobotModelLink) – the link that should be constrained.

  • ref (RobotModelLink or RigidObjectModel, optional) – the link that link should be constrained to, or None for the world frame.

  • local (3-vector, or a list of 3-vectors, optional) – the local coordinates on body should be constrained in place

  • world (3-vector, or a list of 3-vectors, optional) – the world coordinates on body should be constrained in place

klampt.model.ik.fixed_rotation_objective(link, ref=None, local_axis=None, world_axis=None)[source]

Convenience function for fixing the given link at its current orientation in space.

The arguments are interpreted as follows: :rtype: IKObjective

  • If local_axis and world_axis are not provided, the entire link’s orientation is constrained.

  • If only local_axis is provided, the link is constrained to rotate about this local axis.

  • If only world_axis is provided, the link is constrained to rotate about this world-space axis.

Parameters:

  • link (RobotModelLink) – the link that should be constrained.

  • ref (RobotModelLink or RigidObjectModel, optional) – the link that link should be constrained to, or None for the world frame.

  • local_axis (3-vector, optional) – the local direction on on body that should be constrained in place.

  • world_axis (3-vector, optional) – the world coordinates of the direction that should be constrained.

klampt.model.ik.objects(objectives)[source]

Returns a list of all objects touched by the given objective(s). Not currently implemented.

class klampt.model.ik.SubRobotIKSolver(r)[source]

Bases: IKSolver

A version of IKSolver for subrobots. Allows addressing of configurations and links according to the indices of the SubRobotModel provided upon initialization.

Initializes an IK solver. Given a RobotModel, an empty solver is created. Given an IK solver, acts as a copy constructor.

__init__ (robot): IKSolver

__init__ (solver): IKSolver

Parameters:

Methods:

copy()

Copy constructor.

setActiveDofs(active)

Sets the active degrees of freedom.

getActiveDofs()

Returns the active degrees of freedom.

setJointLimits(qmin, qmax)

Sets limits on the robot's configuration.

getJointLimits()

Returns the limits on the robot's configuration (by default this is the robot's joint limits.

setBiasConfig(bias_config)

Biases the solver to approach a given configuration.

getBiasConfig()

Returns the solvers' bias configuration.

getJacobian()

Computes the matrix describing the instantaneous derivative of the objective with respect to the active Dofs.
copy()[source]

Copy constructor.

Return type:

SubRobotIKSolver

setActiveDofs(active)[source]

Sets the active degrees of freedom. Respects subrobotness.

Return type:

None

Parameters:

active (list of int) –

getActiveDofs()[source]

Returns the active degrees of freedom. Respects subrobotness.

Return type:

List[int]

setJointLimits(qmin, qmax)[source]

Sets limits on the robot’s configuration. If empty, this turns off joint limits. Respects subrobotness.

Return type:

None

Parameters:

  • qmin (list of floats) –

  • qmax (list of floats) –

getJointLimits()[source]

Returns the limits on the robot’s configuration (by default this is the robot’s joint limits.

Respects subrobotness.

Returns:

(qmin, qmax) giving lists of joint limits.

Return type:

tuple

setBiasConfig(bias_config)[source]

Biases the solver to approach a given configuration. Setting an empty vector clears the bias term. Respects subrobotness.

Return type:

None

Parameters:

bias_config (list of floats) –

getBiasConfig()[source]

Returns the solvers’ bias configuration. Respects subrobotness.

Return type:

Sequence[float]

getJacobian()[source]

Computes the matrix describing the instantaneous derivative of the objective with respect to the active Dofs. Respects subrobotness.

Returns:

Matrix (6 x N) J; such that J @ qdot = EE velocity.

Return type:

ndarray

klampt.model.ik.solver(objectives, iters=None, tol=None, secondary=None)[source]

Returns a solver for the given objective(s).

Return type:

IKSolver

Parameters:

  • objectives (IKObjective or list of IKObjectives) –

    the objective(s) that should be solved.

    Note that these should be result(s) from the objective() function. OR, if you are making your own goals by calling the IKObjective constructors from the robotsim module, you must set the .robot member of each objective to the RobotModel being solved.

  • iters (int, optional) – if given, the max # of iterations for the solver is set to this value. Otherwise, the default value (100) is used

  • tol (float, optional) – if given, the constraint solving tolerance for the solver is set to this value. Otherwise, the default value (1e-3) is used.

  • secondary (IKObjective or list of IKObjective, optional) – if given, tries to optimize a secondary objective. Make sure to use solver.minimize() rather than solver.solve()

Note

In rare cases, this may return a list of IKSolver’s if you give it objectives on different robots. They should be solved independently for efficiency.

klampt.model.ik.solve(objectives, iters=1000, tol=0.001, activeDofs=None, secondary=None)[source]

Attempts to solve the given objective(s). Either a single objective or a list of simultaneous objectives can be provided.

Return type:

bool

Parameters:

  • objectives – either a single IKObjective or a list of IKObjectives.

  • iters (int, optional) – a maximum number of iterations.

  • tol (float, optional) – a maximum error tolerance on satisfying the objectives

  • activeDofs (list, optional) – a list of link indices or names to use for IK solving. By default, will determine these automatically.

  • secondary (IKObjective or list of IKObjective or tuple (func,gradient) – optional): if given, tries to optimize a secondary objective. If a tuple, it provides both a function and its gradient.

Note

You cannot use sub-robots and activeDofs at the same time. Undefined behavior will result.

Returns:

True if a solution is successfully found to the given tolerance, within the provided number of iterations. The robot(s) are then set to the solution configuration. If a solution is not found, False is returned and the robot(s) are set to the best found configuration.

This function will be smart about multiple objects / robots. If the objectives are on disjoint robots then disjoint IK problems will be solved individually, which is fater.

(The objectives should be a result from the objective() function. Beware that if you are making your own goals by calling the IKObjective constructors from the robotsim module, the .robot member of these goals must be set).

klampt.model.ik.residual(objectives)[source]

Returns the residual of the given objectives.

Return type:

Sequence[float]

klampt.model.ik.jacobian(objectives)[source]

Returns the jacobian of the given objectives.

klampt.model.ik.solve_global(objectives, iters=1000, tol=0.001, activeDofs=None, secondary=None, numRestarts=100, feasibilityCheck=None, startRandom=False)[source]

Attempts to solve the given objective(s) but avoids local minima to some extent using a random-restart technique.

Return type:

bool

Parameters:

  • secondary (objectives, iters, tol, activeDofs,) – same as solve()

  • numRestarts (int, optional) – the number of random restarts. The larger this is, the more likely a solution will be found, but if no solution exists, the routine will take more time.

  • feasibilityCheck (function, optional) – if provided, it will be a function feasible() that returns True if the robot/world in the current configuration is feasible, and False otherwise.

  • startRandom (bool, optional) – True if you wish to forgo the robot’s current configuration and start from random initial configurations.

Returns:

True if a feasible solution is successfully found to the given tolerance, within the provided number of iterations.

klampt.model.ik.solve_nearby(objectives, maxDeviation, iters=1000, tol=0.001, activeDofs=None, secondary=None, numRestarts=0, feasibilityCheck=None)[source]

Solves for an IK solution that does not deviate too far from the initial configuration. :rtype: bool

Note

Currently only supports single-robot objectives!

Parameters:

  • feasibilityCheck (objectives, iters, tol, activeDofs,) – same as solve_global() except for…

  • maxDeviation (float) – the maximum absolute amount in configuration space that each configuration element is allowed to change.

  • numRestarts (int) – same as in solve_global() , but is by default set to zero.