Inverse kinematics

The main function solve inverse kinematics is solve_ik(). Here is for instance how it appears in a closed-loop inverse kinematics:

rate = RateLimiter(frequency=100.0)
while True:
    # [...] <- update task targets here
    velocity = solve_ik(configuration, tasks, rate.dt, solver=solver)
    configuration.integrate_inplace(velocity, rate.dt)
    rate.sleep()

See the examples/ folder in the repository for complete use cases.

pink.solve_ik.solve_ik(configuration, tasks, dt, solver, damping=1e-12, **kwargs)

Compute a velocity tangent to the current robot configuration.

The computed velocity satisfies at (weighted) best the set of kinematic tasks given in argument.

Parameters:

configuration (Configuration) – Robot configuration to read kinematics from.
tasks (Iterable[Task]) – List of kinematic tasks.
dt (float) – Integration timestep in [s].
solver (str) – Backend quadratic programming (QP) solver.
damping (float) – weight of Tikhonov (everywhere) regularization. Its unit is \([\mathrm{cost}]^2 / [\mathrm{tangent}]\) where \([\mathrm{tangent}]\) is “the” unit of robot velocities. Improves numerical stability, but larger values slow down all tasks.
kwargs – Keyword arguments to forward to the backend QP solver.

Return type:

ndarray

Returns:

Velocity \(v\) in tangent space.

Raises:

NotWithinConfigurationLimits – if the current configuration is not within limits.

Note

Our Tikhonov damping is isotropic despite tangent velocities not being homogeneous. If it helps we can add a tangent-space scaling to damp the floating base differently from joint angular velocities.

It is also possible to ask Pink to only build the underlying inverse kinematics problem via the build_ik() function:

pink.solve_ik.build_ik(configuration, tasks, dt, damping=1e-12)

Build quadratic program from current configuration and tasks.

This quadratic program is, in standard form:

\[\begin{split}\begin{split}\begin{array}{ll} \underset{\Delta q}{\mbox{minimize}} & \frac{1}{2} {\Delta q}^T H {\Delta q} + c^T {\Delta q} \\ \mbox{subject to} & G {\Delta q} \leq h \end{array}\end{split}\end{split}\]

where \(\Delta q\) is a vector of joint displacements corresponding to the joint velocities \(v = {\Delta q} / {\mathrm{d}t}\).

Parameters:

configuration (Configuration) – Robot configuration to read kinematics from.
tasks (Iterable[Task]) – List of kinematic tasks.
dt (float) – Integration timestep in [s].
damping (float) – weight of Tikhonov (everywhere) regularization. Its unit is \([\mathrm{cost}]^2 / [\mathrm{tangent}]\) where \([\mathrm{tangent}]\) is “the” unit of robot velocities. Improves numerical stability, but larger values slow down all tasks.

Return type:

Problem

Returns:

Quadratic program of the inverse kinematics problem.