ETS.ik_NR

ETS.ik_NR(Tep, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, pinv=True, pinv_damping=0.0)[source]

Fast numerical inverse kinematics using Newton-Raphson optimization

sol = ets.ik_NR(Tep) are the joint coordinates (n) corresponding to the robot end-effector pose Tep which is an SE3 or ndarray object. This method can be used for robots with any number of degrees of freedom. This is a fast solver implemented in C++.

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

Note

When using this method with redundant robots (>6 DoF), pinv must be set to True

Parameters:
  • Tep (Union[ndarray, SE3]) – The desired end-effector pose or pose trajectory

  • q0 (Optional[ndarray]) – initial joint configuration (default to random valid joint configuration contrained by the joint limits of the robot)

  • ilimit (int) – maximum number of iterations per search

  • slimit (int) – maximum number of search attempts

  • tol (float) – final error tolerance

  • mask (Optional[ndarray]) – a mask vector which weights the end-effector error priority. Corresponds to translation in X, Y and Z and rotation about X, Y and Z respectively

  • joint_limits (bool) – constrain the solution to being within the joint limits of the robot (reject solution with invalid joint configurations and perfrom another search up to the slimit)

  • pinv (int) – Use the psuedo-inverse instad of the normal matrix inverse

  • pinv_damping (float) – Damping factor for the psuedo-inverse

Return type:

Tuple[ndarray, int, int, int, float]

Returns:

  • sol – tuple (q, success, iterations, searches, residual)

  • The return value sol is a tuple with elements

  • ============ ========== ===============================================

  • Element Type Description

  • ============ ========== ===============================================

  • q ndarray(n) joint coordinates in units of radians or metres

  • success int whether a solution was found

  • iterations int total number of iterations

  • searches int total number of searches

  • residual float final value of cost function

  • ============ ========== ===============================================

  • If success == 0 the q values will be valid numbers, but the

  • solution will be in error. The amount of error is indicated by

  • the residual.

Synopsis

Each iteration uses the Newton-Raphson optimisation method

\[\vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k\]

Examples

The following example gets the ets of 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_GN method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ik_NR(Tep)
(array([-1.0805, -0.5328,  0.9086, -2.1781,  0.4612,  1.9018,  0.4239]), 1, 489, 32, 2.8033063270234757e-09)

Notes

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.

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).

See also

ik_LM

A fast numerical inverse kinematics solver using Levenberg-Marquadt optimisation

ik_GN

A fast numerical inverse kinematics solver using Gauss-Newton optimisation