IK_QP.step
- IK_QP.step(ets, Tep, q)[source]
Performs a single iteration of the Gauss-Newton optimisation method
The next step is defined as
\[\vec{q}_{k+1} = \vec{q}_{k} + \dot{\vec{q}}.\]where the QP is defined as
\[\begin{split}\min_x \quad f_o(\vec{x}) &= \frac{1}{2} \vec{x}^\top \mathcal{Q} \vec{x}+ \mathcal{C}^\top \vec{x}, \\ \text{subject to} \quad \mathcal{J} \vec{x} &= \vec{\nu}, \\ \mathcal{A} \vec{x} &\leq \mathcal{B}, \\ \vec{x}^- &\leq \vec{x} \leq \vec{x}^+\end{split}\]with
\[\begin{split}\vec{x} &= \begin{pmatrix} \dvec{q} \\ \vec{\delta} \end{pmatrix} \in \mathbb{R}^{(n+6)} \\ \mathcal{Q} &= \begin{pmatrix} \lambda_q \mat{1}_{n} & \mathbf{0}_{6 \times 6} \\ \mathbf{0}_{n \times n} & \lambda_\delta \mat{1}_{6} \end{pmatrix} \in \mathbb{R}^{(n+6) \times (n+6)} \\ \mathcal{J} &= \begin{pmatrix} \mat{J}(\vec{q}) & \mat{1}_{6} \end{pmatrix} \in \mathbb{R}^{6 \times (n+6)} \\ \mathcal{C} &= \begin{pmatrix} \mat{J}_m \\ \bf{0}_{6 \times 1} \end{pmatrix} \in \mathbb{R}^{(n + 6)} \\ \mathcal{A} &= \begin{pmatrix} \mat{1}_{n \times n + 6} \\ \end{pmatrix} \in \mathbb{R}^{(l + n) \times (n + 6)} \\ \mathcal{B} &= \eta \begin{pmatrix} \frac{\rho_0 - \rho_s} {\rho_i - \rho_s} \\ \vdots \\ \frac{\rho_n - \rho_s} {\rho_i - \rho_s} \end{pmatrix} \in \mathbb{R}^{n} \\ \vec{x}^{-, +} &= \begin{pmatrix} \dvec{q}^{-, +} \\ \vec{\delta}^{-, +} \end{pmatrix} \in \mathbb{R}^{(n+6)},\end{split}\]where \(\vec{\delta} \in \mathbb{R}^6\) is the slack vector, \(\lambda_\delta \in \mathbb{R}^+\) is a gain term which adjusts the cost of the norm of the slack vector in the optimiser, \(\dvec{q}^{-,+}\) are the minimum and maximum joint velocities, and \(\dvec{\delta}^{-,+}\) are the minimum and maximum slack velocities.
- Parameters:
- Raises:
numpy.LinAlgError – If a step is impossible due to a linear algebra error
- Return type:
- Returns:
E – The new error value
q – The new joint coordinate vector