Robot.ikine_LM

Robot.ikine_LM(Tep, end=None, start=None, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, seed=None, k=1.0, method='chan', kq=0.0, km=0.0, ps=0.0, pi=0.3, tool=None, **kwargs)

Levenberg-Marquardt Numerical Inverse Kinematics Solver

Parameters:

Tep (ndarray | SE3) – The desired end-effector pose or pose trajectory
end (str | Link | Gripper | None) – the link considered as the end-effector
start (str | Link | Gripper | None) – the link considered as the base frame, defaults to the robots’s base frame
q0 (ndarray | Sequence[float] | None) – The initial joint coordinates, a vector (n,), or a matrix (m, n) whose rows are the starting points of the first m searches (any further searches start from random valid coordinates)
ilimit (int) – How many iterations are allowed within a search before a new search is started
slimit (int) – How many searches are allowed before being deemed unsuccessful
tol (float) – Maximum allowed residual error E
mask (ndarray | Sequence[float] | None) – A 6 vector which assigns weights to Cartesian degrees-of-freedom error priority
joint_limits (bool) – Reject solutions with joint limit violations
seed (int | None) – A seed for the private RNG used to generate random joint coordinate vectors
k (float) – Sets the gain value for the damping matrix Wn in the next iteration
method (Literal['chan', 'wampler', 'sugihara']) – One of “chan”, “sugihara” or “wampler”. Defines which method is used to calculate the damping matrix Wn in the step method
kq (float) – The gain for joint limit avoidance. Setting to 0.0 will remove this completely from the solution
km (float) – The gain for maximisation. Setting to 0.0 will remove this completely from the solution
ps (float) – The minimum angle/distance (in radians or metres) in which the joint is allowed to approach to its limit
pi (ndarray | float) – The influence angle/distance (in radians or metres) in null space motion becomes active
tool (ndarray | SE3 | None) – a static tool transformation matrix to apply to the end of end; defaults to the robot’s own self.tool if not given

Returns:

an IKSolution containing joint coordinates q, success flag, iterations, searches, residual error value, and reason string if applicable

Return type:

Any

If Tep is a trajectory (an SE3 with N poses, or an array with shape (N, 4, 4)) each pose is solved independently from the same q0, so consecutive solutions are not guaranteed to be on the same IK branch. q has shape (N, n), success is True only if every pose succeeded, and residual is the maximum over the poses. See solve().

Changed in version 1.5.0: For a trajectory, residual is the maximum over the poses, it was the minimum.

A method which provides functionality to perform numerical inverse kinematics (IK) using the Levenberg-Marquardt method.

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

The operation is defined by the choice of the method kwarg.

The step is defined 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.

Examples

The following example makes a panda robot object, makes a goal pose Tep, and then solves for the joint coordinates which result in the pose Tep using the ikine_LM method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ikine_LM(Tep)
IKSolution: q=[-2.772, 0.3187, 2.811, -2.199, -0.1172, 1.994, 0.8779], success=True, iterations=6, searches=1, residual=4e-13

Notes

The value for the k kwarg will depend on the method chosen and the arm you are using. Use the following as a rough guide chan, k = 1.0 - 0.01, wampler, k = 0.01 - 0.0001, and sugihara, k = 0.1 - 0.0001

When using the this method, the initial joint coordinates \(q_0\), should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.

This class supports null-space motion to assist with maximising manipulability and avoiding joint limits. These are enabled by setting kq and km to non-zero values.

References

J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).
J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).