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, limits=None, barriers=None, constraints=None, safety_break=True, **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. The same weight is applied to all joints, including if applicable the root joint (floating base, …).
limits (Optional[Iterable[Limit]]) – Collection of limits to enforce. By default, consists of configuration and velocity limits (set to the empty list [] to disable limits).
barriers (Optional[Iterable[Barrier]]) – Collection of barrier functions.
constraints (Optional[Iterable[Task]]) – List of kinematic tasks to be enforced strictly, as hard equality constraints in the underlying QP rather than in its cost function.
safety_break (bool) – If True, stop execution and raise an exception if the current configuration is outside limits. If False, print a warning and continue execution.
kwargs – Keyword arguments to forward to the backend QP solver.

Return type:

ndarray

Returns:

Velocity \(v\) in tangent space.

Raises:

NoSolutionFound – if the backend QP solver does not find a solution to the differential IK problem.
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, limits=None, barriers=None, constraints=None)

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. The same weight is applied to all joints, including if applicable the root joint (floating base, …).
limits (Optional[Iterable[Limit]]) – Collection of limits to enforce. By default, consists of configuration and velocity limits (set to the empty list [] to disable limits).
barriers (Optional[Iterable[Barrier]]) – Collection of barriers.
constraints (Optional[Iterable[Task]]) – List of kinematic tasks to be enforced strictly, as hard equality constraints in the underlying QP rather than in its cost function.

Return type:

Problem

Returns:

Quadratic program of the inverse kinematics problem.