IK_GN.step(ets, Tep, q)[source]

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.

  • ets (ETS) – The ETS representing the manipulators kinematics

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

  • q (ndarray) – The current joint coordinate vector


numpy.LinAlgError – If a step is impossible due to a linear algebra error

Return type:

Tuple[float, ndarray]


  • E – The new error value

  • q – The new joint coordinate vector