Derivations

Variable Name	Symbol
Configuration	\(q\)
Configuration displacement	\(\Delta q\)
Integration timestep	\(dt\)
Velocity in tangent space	\(v = \frac{\Delta q}{dt}\)
Configuration limits	\(q_{\text{min}}, q_{\text{max}}\)
Maximum joint velocity magnitude	\(v_{\text{max}}\)
Identity matrix	\(I\)

Limits

Configuration limit

Applying a first-order Taylor expansion on the configuration yields:

\[\begin{split}\begin{aligned} q_{\text{min}} &\leq q \oplus v \cdot dt \leq q_{\text{max}} \\ q_{\text{min}} &\leq q \oplus \Delta q \leq q_{\text{max}} \\ q_{\text{min}} &\ominus q \leq \Delta q \leq q_{\text{max}} \ominus q \end{aligned}\end{split}\]

Rewriting as \(G \Delta q \leq h\) and separating the inequalities:

\[\begin{split}\begin{aligned} &+I \cdot \Delta q \leq q_{\text{max}} \ominus q \\ &-I \cdot \Delta q \leq q \ominus q_{\text{min}} \end{aligned}\end{split}\]

Stacking these inequalities gives:

\[\begin{split}\begin{aligned} G &= \begin{bmatrix} +I \\ -I \end{bmatrix}, \\ h &= \begin{bmatrix} q_{\text{max}} \ominus q \\ q \ominus q_{\text{min}} \end{bmatrix} \end{aligned}\end{split}\]

Velocity limit

Given the maximum joint velocity magnitudes \(v_{\text{max}}\), the joint velocity limits can be expressed as:

\[\begin{split}\begin{aligned} -v_{\text{max}} &\leq v \leq v_{\text{max}} \\ -v_{\text{max}} &\leq \frac{\Delta q}{dt} \leq v_{\text{max}} \\ -v_{\text{max}} \cdot dt &\leq \Delta q \leq v_{\text{max}} \cdot dt \end{aligned}\end{split}\]

Rewriting as \(G \Delta q \leq h\) and separating the inequalities:

\[\begin{split}\begin{aligned} &+I \cdot \Delta q \leq v_{\text{max}} \cdot dt \\ &-I \cdot \Delta q \leq v_{\text{max}} \cdot dt \end{aligned}\end{split}\]

Stacking these inequalities gives:

\[\begin{split}\begin{aligned} G \Delta q &\leq h \\ \begin{bmatrix} +I \\ -I \end{bmatrix} \Delta q &\leq \begin{bmatrix} v_{\text{max}} \cdot dt \\ v_{\text{max}} \cdot dt \end{bmatrix} \end{aligned}\end{split}\]

Tasks

Posture task

A posture task penalizes the deviation of the current configuration from a preferred posture. Its error and Jacobian are

\[e(q) = q^{\star} \ominus q, \qquad J(q) = I_{n_v}\]

First-order task dynamics

Using the generic relation \(J(q)\,\Delta q = -\alpha\,e(q)\) gives

\[I_{n_v}\,\Delta q = -\alpha\,e(q) \;\;\Longrightarrow\;\; \boxed{\; \Delta q = -\alpha\,e(q) \;}\]

Quadratic-program formulation

The task enters the QP as the weighted least-squares term

\[\min_{\Delta q}\; \tfrac12 \left\lVert J\,\Delta q + \alpha\,e(q) \right\rVert_{W}^{2},\]

with a diagonal, positive weight matrix \(W = \mathrm{diag}(\lambda_i)\). Substituting \(J = I_{n_v}\) and equating the gradient to zero yields

\[W\bigl(\Delta q + \alpha\,e(q)\bigr) = 0 \;\;\Longrightarrow\;\; \boxed{\; \Delta q = -\alpha\,e(q) \;},\]

identical to the first-order dynamics and independent of the weights because \(W\) is full-rank.

Velocity interpretation

Interpreting the displacement over the solver timestep \(dt\) as a velocity command gives

\[\dot q_{\text{des}} = \frac{\Delta q}{dt} = \frac{\alpha}{dt}\bigl(q^{\star} - q\bigr) = k_p\bigl(q^{\star} - q\bigr),\]

where \(k_p = \alpha / dt\). When the implementation uses \(dt = 1\), the proportional gain reduces to \(k_p = \alpha\). Hence the posture task behaves as a joint-space proportional controller that is always full-rank and therefore provides robust regularization whenever primary tasks are ill-conditioned.

Damping task

The damping task penalizes joint motion itself, i.e.the displacement Δq. It does not target a reference posture: its desired error is identically zero.

Error and Jacobian

\[e(q) = 0, \qquad J(q) = I_{n_v}\]

Quadratic cost

Inserted in the generic task objective

\[\min_{\Delta q}\; \tfrac12 \bigl\lVert J\,\Delta q + \alpha\,e(q) \bigr\rVert_{W}^{2}\]

with \(e(q)=0\), \(J = I_{n_v}\), and default gain \(\alpha = 1\), the cost reduces to

\[\boxed{\; \tfrac12\,\Delta q^{\top} W \,\Delta q \;} \;=\; \tfrac12 \sum_{i=1}^{n_v} \lambda_i\,\Delta q_i^{2},\]

where \(W = \operatorname{diag}(\lambda_i)\) comes from the user-supplied cost vector.

Optimality conditions

Taking the gradient and setting it to zero gives

\[W \,\Delta q = 0 \;\;\Longrightarrow\;\; \boxed{\; \Delta q = 0 \;}.\]

Thus, in the absence of other tasks, the damping task enforces \(\Delta q = 0\). When other tasks are present, the QP minimizes joint motion inside their solution set, yielding the minimum-norm velocity.

Connection to Levenberg-Marquardt / Tikhonov

If a primary task with Hessian \(J_{h}^{\top}J_{h}\) becomes rank-deficient, adding the damping task makes the combined Hessian

\[H = J_{h}^{\top}J_{h} \;+\; W\]

strictly positive-definite (provided any \(\lambda_i>0\)). This is exactly Tikhonov regularization, preventing large joint speeds near singularities and selecting a unique solution.

Velocity interpretation

With the library’s convention v = Δq / dt and \(\Delta q = 0\), the commanded velocity is

\[\dot q_{\text{des}} = 0.\]

The damping task therefore commands zero velocity; it acts as a regularizer that suppresses unnecessary motion when higher-priority tasks leave residual degrees of freedom.