Notation

Notation#

mink-warp follows the same frame and task conventions as Mink and Pink (Pinocchio). Subscripts are read right-to-left for transforms; superscripts indicate the frame in which a vector is expressed.

Quantity

Notation

Transform from frame \(A\) to frame \(B\)

\(T_{BA} \in SE(3)\)

Position of frame \(B\) origin in frame \(A\)

\({}^A p_B\)

World (inertial) frame

\(0\) or \(W\)

Configuration vector

\(q \in \mathbb{R}^{n_q}\) (batched: (nworld, nq))

Tangent / velocity

\(v \in \mathbb{R}^{n_v}\), \(\Delta q = v\,\mathrm{d}t\)

Task error and Jacobian

\(e(q) \in \mathbb{R}^k\), \(J(q) \in \mathbb{R}^{k \times n_v}\)

Composition (read transforms left to right):

\[T_{CA} = T_{CB}\, T_{BA}\]

Frame task#

For regulated frame \(b\), target \(t\), and world \(0\):

\[e(q) = \log(T_{bt}), \qquad J(q) = -\mathrm{jlog}_6(T_{tb})\, {}_b J_{0b}(q)\]

See FrameTask and [FrameTaskJacobian].

Stacked IK problem#

Tasks contribute to normal equations (equivalent to a weighted least-squares QP):

\[\min_{\Delta q}\ \tfrac{1}{2} \Delta q^\top H \Delta q + c^\top \Delta q, \quad H = \sum_i W_i^\top W_i + \mu I,\quad c = \sum_i -W_i^\top (\alpha_i e_i)\]

Hard limits add \(\ell \leq \Delta q \leq u\) and/or \(G \Delta q \leq h\). See Constrained IK (hard limits) and solve_ik().

The task function approach used here was formalized by Samson, Espiau and Le Borgne [Samson1991]. Lie-group errors use the logarithm map on \(SE(3)\); see Solà et al. [Sola2018] for background. LM damping on large errors follows Sugihara [Sugihara2011].

Further reading#