Inverse kinematics

Inverse kinematics#

mink.solve_ik.solve_ik(configuration: Configuration, tasks: Sequence[BaseTask], dt: float, solver: str, damping: float = 1e-12, safety_break: bool = False, limits: Sequence[Limit] | None = None, constraints: Sequence[Task | Elastic] | None = None, **kwargs) → ndarray#

Solve the differential inverse kinematics problem.

Computes a velocity tangent to the current robot configuration. The computed velocity satisfies at (weighted) best the set of provided kinematic tasks.

Elastic constraints are first held exactly. If every multiplier satisfies \(|\lambda_i| < \rho_i\), that solution is also the elastic one. Otherwise the penalized problem of build_ik() is solved, as in the elastic mode of [SNOPT]. A step therefore costs one QP while the elastic constraints hold and two while one yields.

Parameters:

  • configuration (Configuration) – Robot configuration.

  • tasks (Sequence[BaseTask]) – List of kinematic tasks.

  • dt (float) – Integration timestep in [s].

  • solver (str) – Backend quadratic programming (QP) solver.

  • damping (float) – Levenberg-Marquardt damping applied to all tasks. Higher values improve numerical stability but slow down task convergence. This value applies to all dofs, including floating-base coordinates.

  • 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.

  • limits (Sequence[Limit] | None) – List of limits to enforce. Set to empty list to disable. If None, defaults to a configuration limit.

  • constraints (Sequence[Task | Elastic] | None) – List of tasks to enforce as equality constraints. These tasks will be satisfied exactly rather than in a least-squares sense. Wrap a task in Elastic to let it yield when it cannot be met.

  • kwargs – Keyword arguments to forward to the backend QP solver.

Raises:

  • NotWithinConfigurationLimits – If the current configuration is outside the joint limits and safety_break is True.

  • NoSolutionFound – If the QP solver fails to find a solution.

Returns:

Velocity \(v\) in tangent space.

Return type:

ndarray

mink.solve_ik.build_ik(configuration: Configuration, tasks: Sequence[BaseTask], dt: float, damping: float = 1e-12, limits: Sequence[Limit] | None = None, constraints: Sequence[Task | Elastic] | None = None) → Problem#

Build the quadratic program given the current configuration and tasks.

The quadratic program is defined as:

\[\begin{split}\begin{align*} \min_{\Delta q} & \frac{1}{2} \Delta q^T H \Delta q + c^T \Delta q \\ \text{s.t.} \quad & G \Delta q \leq h \\ & A \Delta q = b \end{align*}\end{split}\]

where \(v = \Delta q / dt\) is the velocity in tangent space.

Each penalized component \(i\) of an Elastic constraint, with residual \(r_i = J_i \Delta q + \alpha e_i\) and penalty \(\rho_i\), adds a slack \(s_i\) with cost \(\rho_i^{1/2} s_i\) and the rows \(-s_i \leq \rho_i^{1/2} r_i \leq s_i\). At the optimum \(s_i = \rho_i^{1/2} |r_i|\), so the added cost is the \(\ell_1\) penalty \(\rho_i |r_i|\). The decision variable is then \(x = [\Delta q; s]\). The penalty is linear in \(\Delta q\) away from \(r_i = 0\), so the objective also gains the trust region \(\tfrac{1}{2} \max(\rho) \|\Delta q\|^2\). Without it, a yielding component would step to the edge of the feasible set and chatter near singularities.

Parameters:

  • configuration (Configuration) – Robot configuration.

  • tasks (Sequence[BaseTask]) – List of kinematic tasks.

  • dt (float) – Integration timestep in [s].

  • damping (float) – Levenberg-Marquardt damping. Higher values improve numerical stability but slow down task convergence. This value applies to all dofs, including floating-base coordinates.

  • limits (Sequence[Limit] | None) – List of limits to enforce. Set to empty list to disable. If None, defaults to a configuration limit.

  • constraints (Sequence[Task | Elastic] | None) – List of tasks to enforce as equality constraints. These tasks will be satisfied exactly rather than in a least-squares sense. Wrap a task in Elastic to let it yield when it cannot be met.

Returns:

Quadratic program of the inverse kinematics problem. With elastic constraints, its first \(n_v\) variables are \(\Delta q\).

Return type:

Problem

Elastic constraints#

class mink.elastic.Elastic#

Equality constraint that yields instead of making the QP infeasible.

Passed through constraints like a plain task, it enforces \(J \Delta q = -\alpha e\) through the exact \(\ell_1\) penalty

\[\rho^T \left| J \Delta q + \alpha e \right|,\]

with \(|\cdot|\) taken componentwise. Let \(\lambda\) be the multiplier the task would have as a hard constraint. If \(|\lambda_i| < \rho_i\) for every component, the solution is that of the hard constraint [SoftConstraintsMPC]. A component that would need more force yields, pulling with constant force \(\rho_i\), and the QP stays feasible.

As with hard constraints, the task’s cost and lm_damping are ignored. The penalty is the only per-component scale. A zero penalty leaves that component unconstrained. Penalties carry the units of the task error, so a FrameTask usually takes a lower orientation penalty, letting orientation give way first.

Example:

frame_task = FrameTask("attachment_site", "site", 1.0, 1.0)
solve_ik(
    configuration,
    [posture_task],
    dt,
    "daqp",
    limits=limits,
    constraints=[Elastic(frame_task, penalty=[1e3] * 3 + [1e1] * 3)],
)
task#

The constrained task.

penalty#

Per-component \(\ell_1\) penalty \(\rho\), a scalar or a vector with the dimension of the task error.