# 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:
• ets (ETS) – The ETS representing the manipulators kinematics

• Tep (ndarray) – The desired end-effector pose

• q (ndarray) – The current joint coordinate vector

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