IK_GN.step

IK_GN.step(ets, Tep, q)

Performs a single iteration of the Gauss-Newton optimisation method

The next step is defined as

\[\begin{split}\vec{q}_{k+1} &= \vec{q}_k + \left( {\mat{J}(\vec{q}_k)}^\top \mat{W}_e \ {\mat{J}(\vec{q}_k)} \right)^{-1} \bf{g}_k \\ \bf{g}_k &= {\mat{J}(\vec{q}_k)}^\top \mat{W}_e \vec{e}_k\end{split}\]

where \(\mat{J} = {^0\mat{J}}\) is the base-frame manipulator Jacobian. If \(\mat{J}(\vec{q}_k)\) is non-singular, and \(\mat{W}_e = \mat{1}_n\), then the above provides the pseudoinverse solution. However, if \(\mat{J}(\vec{q}_k)\) is singular, the above can not be computed and the GN solution is infeasible.

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:

Tuple[float, ndarray]

Returns:

• E – The new error value

• q – The new joint coordinate vector