IK_LM.step

IK_LM.step(ets, Tep, q)

Performs a single iteration of the Levenberg-Marquadt optimisation

The operation is defined by the choice of method when instantiating the class.

The next step is deined as

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

where \(\mat{W}_n = \text{diag}(\vec{w_n})(\vec{w_n} \in \mathbb{R}^n_{>0})\) is a diagonal damping matrix. The damping matrix ensures that \(\mat{A}_k\) is non-singular and positive definite. The performance of the LM method largely depends on the choice of \(\mat{W}_n\).

Chan’s Method

Chan proposed

\[\mat{W}_n = λ E_k \mat{1}_n\]

where λ is a constant which reportedly does not have much influence on performance. Use the kwarg k to adjust the weighting term λ.

Sugihara’s Method

Sugihara proposed

\[\mat{W}_n = E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n)\]

where \(\hat{\vec{w}}_n \in \mathbb{R}^n\), \(\hat{w}_{n_i} = l^2 \sim 0.01 l^2\), and \(l\) is the length of a typical link within the manipulator. We provide the variable k as a kwarg to adjust the value of \(w_n\).

Wampler’s Method

Wampler proposed \(\vec{w_n}\) to be a constant. This is set through the k kwarg.

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

Returns:

• E – The new error value

• q – The new joint coordinate vector