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 NewtonRaphson 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)
By default, the active degrees of freedom are all ancestors of the constrained links. For more control, set them using a solver object.
As with any local IK method, NewtonRaphson 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:

Returns an IKObjective for a given body. 

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

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

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

Returns a solver for the given objective(s). 

Attempts to solve the given objective(s). 

Returns the residual of the given objectives. 

Returns the jacobian of the given objectives. 

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

Solves for an IK solution that does not deviate too far from the initial configuration. 
Classes:
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:
If local and world are provided, then this objective asks to match the local point (s) on the body to the world point(s).
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 (3vector, or a list of 3vectors, optional) – the local coordinates on body should be constrained to the corresponding points in world
world (3vector, or a list of 3vectors, 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 (3vector, optional) – the translation that the link’s origin
take on. (should) –
 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.
 Return type
IKObjective

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:
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 (3vector, or a list of 3vectors, optional) – the local coordinates on body should be constrained in place
world (3vector, or a list of 3vectors, optional) – the world coordinates on body should be constrained in place
 Return type
IKObjective

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:
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 worldspace 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 (3vector, optional) – the local direction on on body that should be constrained in place.
world_axis (3vector, optional) – the world coordinates of the direction that should be constrained.
 Return type
IKObjective

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:
klampt.robotsim.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
robot (
RobotModel
, optional) –solver (
IKSolver
, optional) –
Methods:
copy
()Copy constructor.
setActiveDofs
(active)Sets the active degrees of freedom.
Returns the active degrees of freedom.
setJointLimits
(qmin, qmax)Sets limits on the robot’s configuration.
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.
Returns the solvers’ bias configuration.
Computes the matrix describing the instantaneous derivative of the objective with respect to the active Dofs.

setActiveDofs
(active)[source]¶ Sets the active degrees of freedom. Respects subrobotness.
 Parameters
active (
list of int
) – Return type
None

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.
 Parameters
qmin (
list of floats
) –qmax (
list of floats
) –
 Return type
None

getJointLimits
()[source]¶ Returns the limits on the robot’s configuration (by default this is the robot’s joint limits.
Respects subrobotness.
(qmin, qmax): Pair of lists of joint limits.

setBiasConfig
(bias_config)[source]¶ Biases the solver to approach a given configuration. Setting an empty vector clears the bias term. Respects subrobotness.
 Parameters
bias_config (
list of floats
) – Return type
None

klampt.model.ik.
solver
(objectives, iters=None, tol=None, secondary=None)[source]¶ Returns a solver for the given objective(s).
 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 theIKObjective
constructors from therobotsim
module, you must set the.robot
member of each objective to theRobotModel
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 (1e3) is used.
secondary (IKObjective or list of IKObjective, optional) – if given, tries to optimize a secondary objective. Make sure to use
solver.minimize()
rather thansolver.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.
 Return type
IKSolver

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.
 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 subrobots 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). Return type
bool

klampt.model.ik.
residual
(objectives)[source]¶ Returns the residual of the given objectives.
 Return type
Sequence
[float
]

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 randomrestart technique.
 Parameters
iters, tol, activeDofs, secondary (objectives,) – 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.
 Return type
bool

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.
Note
Currently only supports singlerobot objectives!
 Parameters
iters, tol, activeDofs, feasibilityCheck (objectives,) – 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.
 Return type
bool