roboticstoolbox-python 1.4.0__py3-none-any.whl

roboticstoolbox/robot/IK.py

@@ -0,0 +1,1378 @@
+#!/usr/bin/env python
+
+"""
+@author Jesse Haviland
+"""
+
+import numpy as np
+from abc import ABC, abstractmethod
+from typing import Tuple, cast
+import roboticstoolbox as rtb
+from dataclasses import dataclass
+from spatialmath import SE3
+from roboticstoolbox.tools.types import ArrayLike, NDArray
+
+
+try:
+    import qpsolvers as qp
+
+    _qp = True
+except ImportError:  # pragma nocover
+    _qp = False
+
+
+@dataclass
+class IKSolution:
+    """
+    A dataclass for representing an IK solution
+
+    Attributes
+    ----------
+    q
+        The joint coordinates of the solution (ndarray). Note that these
+        will not be valid if failed to find a solution
+    success
+        True if a valid solution was found
+    iterations
+        How many iterations were performed
+    searches
+        How many searches were performed
+    residual
+        The final error value from the cost function
+    reason
+        The reason the IK problem failed if applicable
+
+
+    .. versionchanged:: 1.0.3
+        Added IKSolution dataclass to replace the IKsolution named tuple
+
+    """
+
+    q: np.ndarray
+    success: bool
+    iterations: int = 0
+    searches: int = 0
+    residual: float = 0.0
+    reason: str = ""
+
+    def __iter__(self):
+        return iter(
+            (
+                self.q,
+                self.success,
+                self.iterations,
+                self.searches,
+                self.residual,
+                self.reason,
+            )
+        )
+
+    def __str__(self):
+        if self.q is not None:
+            q_str = np.array2string(
+                self.q,
+                separator=", ",
+                formatter={
+                    "float": lambda x: "{:.4g}".format(0 if abs(x) < 1e-6 else x)
+                },
+            )  # np.round(self.q, 4)
+        else:
+            q_str = None
+
+        if self.iterations == 0 and self.searches == 0:
+            # Check for analytic
+            if self.success:
+                return f"IKSolution: q={q_str}, success=True"
+            else:
+                return f"IKSolution: q={q_str}, success=False, reason={self.reason}"
+        else:
+            # Otherwise it is a numeric solution
+            if self.success:
+                return (
+                    f"IKSolution: q={q_str}, success=True,"
+                    f" iterations={self.iterations}, searches={self.searches},"
+                    f" residual={self.residual:.3g}"
+                )
+            else:
+                return (
+                    f"IKSolution: q={q_str}, success=False, reason={self.reason},"
+                    f" iterations={self.iterations}, searches={self.searches},"
+                    f" residual={np.round(self.residual, 4):.3g}"
+                )
+
+
+class IKSolver(ABC):
+    r"""
+    An abstract super class for numerical inverse kinematics (IK)
+
+    This class provides basic functionality to perform numerical IK. Superclasses
+    can inherit this class and implement the `solve` method and redefine any other
+    methods necessary.
+
+    :param name: The name of the IK algorithm
+    :param ilimit: How many iterations are allowed within a search before a new search
+        is started
+    :param slimit: How many searches are allowed before being deemed unsuccessful
+    :param tol: Maximum allowed residual error E, where
+        :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form
+        in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`).
+        Because `E` is quadratic, `tol` does not bound the linear-scale position/
+        orientation error directly — with the default unit weighting, components of
+        :math:`\vec{e}` are only guaranteed to be within roughly
+        :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on
+        the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific
+        linear-scale accuracy
+    :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom
+        error priority
+    :param joint_limits: Reject solutions with joint limit violations
+    :param seed: A seed for the private RNG used to generate random joint coordinate
+        vectors
+
+    .. seealso::
+
+        :class:`IK_NR` Implements this class using the Newton-Raphson method
+
+        :class:`IK_GN` Implements this class using the Gauss-Newton method
+
+        :class:`IK_LM` Implements this class using the Levemberg-Marquadt method
+
+        :class:`IK_QP` Implements this class using a quadratic programming approach
+
+    .. versionchanged:: 1.0.3
+        Added the abstract super class IKSolver
+
+    """
+
+    def __init__(
+        self,
+        name: str = "IK Solver",
+        ilimit: int = 30,
+        slimit: int = 100,
+        tol: float = 1e-6,
+        mask: ArrayLike | None = None,
+        joint_limits: bool = True,
+        seed: int | None = None,
+    ):
+        # Solver parameters
+        self.name = name
+        self.slimit = slimit
+        self.ilimit = ilimit
+        self.tol = tol
+
+        # Random number generator
+        self._private_random = np.random.default_rng(seed=seed)
+
+        if mask is None:
+            mask = np.ones(6)
+
+        self.We = np.diag(mask)  # type: ignore
+        self.joint_limits = joint_limits
+
+    def solve(
+        self,
+        ets: "rtb.ETS",
+        Tep: SE3 | np.ndarray,
+        q0: ArrayLike | None = None,
+    ) -> IKSolution:
+        """
+        Solves the IK problem
+
+        :param ets: The ETS representing the manipulators kinematics
+        :param Tep: The desired end-effector pose
+        :param q0: The initial joint coordinate vector
+        :returns: An IKSolution containing joint coordinates ``q``, ``success`` flag,
+            ``iterations``, ``searches``, ``residual`` error value, and ``reason``
+            string if applicable
+        :rtype: IKSolution
+
+        This method will attempt to solve the IK problem and obtain joint coordinates
+        which result the the end-effector pose `Tep`.
+        """
+        # Get the largest jindex in the ETS. If this is greater than ETS.n
+        # then we need to pad the q vector with zeros
+        max_jindex: int = 0
+
+        for j in ets.joints():
+            if j.jindex > max_jindex:  # type: ignore
+                max_jindex = j.jindex  # type: ignore
+
+        q0_method = np.zeros((self.slimit, max_jindex + 1))
+
+        if q0 is None:
+            q0_method[:, ets.jindices] = self._random_q(ets, self.slimit)
+
+        elif not isinstance(q0, np.ndarray):
+            q0 = np.array(q0)
+
+        if q0 is not None and q0.ndim == 1:
+            q0_method[:, ets.jindices] = self._random_q(ets, self.slimit)
+
+            q0_method[0, ets.jindices] = q0
+
+        if q0 is not None and q0.ndim == 2:
+            q0_method[:, ets.jindices] = self._random_q(ets, self.slimit)
+
+            q0_method[: q0.shape[0], ets.jindices] = q0
+
+        q0 = q0_method
+
+        traj = False
+
+        methTep: np.ndarray
+
+        if isinstance(Tep, SE3):
+            if len(Tep) > 1:
+                traj = True
+                methTep = np.empty((len(Tep), 4, 4))
+
+                for i, T in enumerate(Tep):
+                    methTep[i] = T.A
+            else:
+                methTep = cast(NDArray, Tep.A)
+        elif Tep.ndim == 3:
+            traj = True
+            methTep = Tep
+        elif Tep.shape != (4, 4):
+            raise ValueError("Tep must be a 4x4 SE3 matrix")
+        else:
+            methTep = Tep
+
+        if traj:
+            q = np.empty((methTep.shape[0], ets.n))
+            success = True
+            interations = 0
+            searches = 0
+            residual = np.inf
+            reason = ""
+
+            for i, T in enumerate(methTep):
+                sol = self._solve(ets, T, q0)
+                q[i] = sol.q
+                if not sol.success:
+                    success = False
+                    reason = sol.reason
+                interations += sol.iterations
+                searches += sol.searches
+
+                if sol.residual < residual:
+                    residual = sol.residual
+
+            return IKSolution(
+                q=q,
+                success=success,
+                iterations=interations,
+                searches=searches,
+                residual=residual,
+                reason=reason,
+            )
+
+        else:
+            sol = self._solve(ets, methTep, q0)
+
+        return sol
+
+    def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution:
+        # Iteration count
+        i = 0
+        total_i = 0
+
+        # Error flags
+        found_with_limits = False
+        linalg_error = 0
+
+        # Initialise variables
+        E = 0.0
+        q = q0[0]
+
+        for search in range(self.slimit):
+            q = q0[search].copy()
+            i = 0
+
+            while i < self.ilimit:
+                i += 1
+
+                # step() reports E for q as it was *before* this iteration's
+                # update. An undamped update (GN/NR) can overshoot, so if E is
+                # already below tol we must return this pre-step q, not the
+                # mutated one step() hands back - otherwise we can report
+                # success with a q whose actual residual is far above tol.
+                q_prev = q.copy()
+
+                # Attempt a step
+                try:
+                    E, q[ets.jindices] = self.step(ets, Tep, q)
+
+                except np.linalg.LinAlgError:
+                    # Abandon search and try again
+                    linalg_error += 1
+                    break
+
+                # Check if we have arrived
+                if E < self.tol:
+                    q = q_prev
+
+                    # Wrap q to be within +- 180 deg
+                    # If your robot has larger than 180 deg range on a joint
+                    # this line should be modified in incorporate the extra range
+                    q = (q + np.pi) % (2 * np.pi) - np.pi
+
+                    # Check if we have violated joint limits
+                    jl_valid = self._check_jl(ets, q)
+
+                    if not jl_valid and self.joint_limits:
+                        # Abandon search and try again
+                        found_with_limits = True
+                        break
+                    else:
+                        return IKSolution(
+                            q=q[ets.jindices],
+                            success=True,
+                            iterations=total_i + i,
+                            searches=search + 1,
+                            residual=E,
+                            reason="Success",
+                        )
+            total_i += i
+
+        # If we make it here, then we have failed
+        reason = "iteration and search limit reached"
+
+        if linalg_error:
+            reason += f", {linalg_error} numpy.LinAlgError encountered"
+
+        if found_with_limits:
+            reason += ", solution found but violates joint limits"
+
+        return IKSolution(
+            q=q,
+            success=False,
+            iterations=total_i,
+            searches=self.slimit,
+            residual=E,
+            reason=reason,
+        )
+
+    def error(self, Te: np.ndarray, Tep: np.ndarray) -> Tuple[np.ndarray, float]:
+        r"""
+        Calculates the error between Te and Tep
+
+        :param Te: The current end-effector pose
+        :param Tep: The desired end-effector pose
+        :returns: Tuple of ``(e, E)`` where ``e`` is the angle-axis error (6 vector)
+            and ``E`` is the quadratic error weighted by We
+        :rtype: tuple[numpy.ndarray, float]
+
+        Calculates the angle-axis error between current end-effector pose Te and
+        the desired end-effector pose Tep. Also calculates the quadratic error E
+        which is weighted by the diagonal matrix We.
+
+        .. math::
+
+            E = \frac{1}{2} \vec{e}^{\top} \mat{W}_e \vec{e}
+
+        where :math:`\vec{e} \in \mathbb{R}^6` is the angle-axis error.
+        """
+        e = rtb.angle_axis(Te, Tep)
+        E = float(0.5 * e @ self.We @ e)
+
+        return e, E
+
+    @abstractmethod
+    def step(
+        self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray
+    ) -> Tuple[float, np.ndarray]:
+        """
+        Abstract step method
+
+        :param ets: The ETS representing the manipulators kinematics
+        :param Tep: The desired end-effector pose
+        :param q: The current joint coordinate vector
+        :raises numpy.LinAlgError: If a step is impossible due to a linear algebra error
+        :returns: Tuple of ``(E, q)`` where ``E`` is the new error value and ``q`` is
+            the new joint coordinate vector
+        :rtype: tuple[float, numpy.ndarray]
+
+        Superclasses will implement this method to perform a step of the
+        implemented IK algorithm.
+        """
+        pass  # pragma: nocover
+
+    def _random_q(self, ets: "rtb.ETS", i: int = 1) -> np.ndarray:
+        """
+        Generate a random valid joint configuration using a private RNG
+
+        :param ets: The ETS representing the manipulators kinematics
+        :param i: number of configurations to generate
+        :returns: An ``i x n`` ndarray of random valid joint configurations, where n
+            is the number of joints in the ``ets``
+        :rtype: numpy.ndarray
+
+        Generates a random q vector within the joint limits defined by ``ets.qlim``.
+        """
+
+        if i == 1:
+            q = np.zeros((1, ets.n))
+
+            for i in range(ets.n):
+                q[0, i] = self._private_random.uniform(ets.qlim[0, i], ets.qlim[1, i])
+
+        else:
+            q = np.zeros((i, ets.n))
+
+            for j in range(i):
+                for i in range(ets.n):
+                    q[j, i] = self._private_random.uniform(
+                        ets.qlim[0, i], ets.qlim[1, i]
+                    )
+
+        return q
+
+    def _check_jl(self, ets: "rtb.ETS", q: np.ndarray) -> bool:
+        """
+        Checks if the joints are within their respective limits
+
+        :param ets: the ETS
+        :param q: the current joint coordinate vector
+        :returns: True if joints within feasible limits otherwise False
+        :rtype: bool
+
+        """
+
+        # Loop through the joints in the ETS
+        for i in range(ets.n):
+            # Get the corresponding joint limits
+            ql0 = ets.qlim[0, i]
+            ql1 = ets.qlim[1, i]
+
+            # Check if q exceeds the limits
+            if q[i] < ql0 or q[i] > ql1:
+                return False
+
+        # If we make it here, all the joints are fine
+        return True
+
+
+def _null_Σ(ets: "rtb.ETS", q: NDArray, ps: float, pi: NDArray | float):
+    """
+    Formulates a relationship between joint limits and the joint velocity.
+    When this is projected into the null-space of the differential kinematics
+    to attempt to avoid exceeding joint limits
+
+    :param q: The joint coordinates of the robot
+    :param ps: The minimum angle/distance (in radians or metres) in which the joint is
+        allowed to approach to its limit
+    :param pi: The influence angle/distance (in radians or metres) in which the velocity
+        damper becomes active
+
+    :return: Σ
+    """
+
+    if isinstance(pi, float) or isinstance(pi, int):
+        pi = pi * np.ones(ets.n)
+
+    # Add cost to going in the direction of joint limits, if they are within
+    # the influence distance
+    Σ = np.zeros((ets.n, 1))
+
+    for i in range(ets.n):
+        qi = q[i]
+        ql0 = ets.qlim[0, i]
+        ql1 = ets.qlim[1, i]
+
+        if qi - ql0 <= pi[i]:
+            Σ[i, 0] = -np.power(((qi - ql0) - pi[i]), 2) / np.power((ps - pi[i]), 2)
+        if ql1 - qi <= pi[i]:
+            Σ[i, 0] = np.power(((ql1 - qi) - pi[i]), 2) / np.power((ps - pi[i]), 2)
+
+    return -Σ
+
+
+def _calc_qnull(
+    ets: "rtb.ETS",
+    q: np.ndarray,
+    J: np.ndarray,
+    λΣ: float,
+    λm: float,
+    ps: float,
+    pi: np.ndarray | float,
+):
+    """
+    Calculates the desired null-space motion according to the gains λΣ and λm.
+    This is a helper method that is used within the `step` method of an IK solver
+
+    :return: qnull - the desired null-space motion
+    """
+
+    qnull_grad = np.zeros(ets.n)
+    qnull = np.zeros(ets.n)
+
+    # Add the joint limit avoidance if the gain is above 0
+    if λΣ > 0:
+        Σ = _null_Σ(ets, q, ps, pi)
+        qnull_grad += (1.0 / λΣ * Σ).flatten()
+
+    # Add the manipulability maximisation if the gain is above 0
+    if λm > 0:
+        Jm = ets.jacobm(q)
+        qnull_grad += (1.0 / λm * Jm).flatten()
+
+    # Calculate the null-space motion
+    if λΣ > 0 or λm > 0:
+        null_space = np.eye(ets.n) - np.linalg.pinv(J) @ J
+        qnull = null_space @ qnull_grad
+
+    return qnull.flatten()
+
+
+class IK_NR(IKSolver):
+    r"""
+    Newton-Raphson Numerical Inverse Kinematics Solver
+
+    A class which provides functionality to perform numerical inverse kinematics (IK)
+    using the Newton-Raphson method. See `step` method for mathematical description.
+
+    .. note::
+
+        When using this class with redundant robots (>6 DoF), `pinv` must be set to `True`
+
+    :param name: The name of the IK algorithm
+    :param ilimit: How many iterations are allowed within a search before a new search
+        is started
+    :param slimit: How many searches are allowed before being deemed unsuccessful
+    :param tol: Maximum allowed residual error E, where
+        :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form
+        in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`).
+        Because `E` is quadratic, `tol` does not bound the linear-scale position/
+        orientation error directly — with the default unit weighting, components of
+        :math:`\vec{e}` are only guaranteed to be within roughly
+        :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on
+        the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific
+        linear-scale accuracy
+    :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom
+        error priority
+    :param joint_limits: Reject solutions with joint limit violations
+    :param seed: A seed for the private RNG used to generate random joint coordinate
+        vectors
+    :param pinv: If True, will use the pseudoinverse in the `step` method instead of
+        the normal inverse
+    :param kq: The gain for joint limit avoidance. Setting to 0.0 will remove this
+        completely from the solution
+    :param km: The gain for maximisation. Setting to 0.0 will remove this completely
+        from the solution
+    :param ps: The minimum angle/distance (in radians or metres) in which the joint is
+        allowed to approach to its limit
+    :param pi: The influence angle/distance (in radians or metres) in null space motion
+        becomes active
+
+    Example::
+
+        The following example gets the ``ets`` of a ``panda`` robot object, instantiates
+        the IK_NR solver class using default parameters, makes a goal pose ``Tep``,
+        and then solves for the joint coordinates which result in the pose ``Tep``
+        using the ``solve`` method.
+
+    .. runblock:: pycon
+    >>> import roboticstoolbox as rtb
+    >>> panda = rtb.models.Panda().ets()
+    >>> solver = rtb.IK_NR(pinv=True)
+    >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
+    >>> solver.solve(panda, Tep)
+
+    .. rubric:: Notes
+
+    When using the NR method, the initial joint coordinates :math:`q_0`, should correspond
+    to a non-singular manipulator pose, since it uses the manipulator Jacobian. When the
+    the problem is solvable, it converges very quickly. However, this method frequently
+    fails to converge on the goal.
+
+    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.
+
+    .. rubric:: 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).
+
+    .. seealso::
+
+        :class:`IKSolver` An abstract super class for numerical IK solvers
+
+        :class:`IK_GN` Implements the IKSolver class using the Gauss-Newton method
+
+        :class:`IK_LM` Implements the IKSolver class using the Levemberg-Marquadt method
+
+        :class:`IK_QP` Implements the IKSolver class using a quadratic programming approach
+
+    .. versionchanged:: 1.0.3
+        Added the Newton-Raphson IK solver class
+
+    """
+
+    def __init__(
+        self,
+        name: str = "IK Solver",
+        ilimit: int = 30,
+        slimit: int = 100,
+        tol: float = 1e-6,
+        mask: ArrayLike | None = None,
+        joint_limits: bool = True,
+        seed: int | None = None,
+        pinv: bool = False,
+        kq: float = 0.0,
+        km: float = 0.0,
+        ps: float = 0.0,
+        pi: np.ndarray | float = 0.3,
+        **kwargs,
+    ):
+        super().__init__(
+            name=name,
+            ilimit=ilimit,
+            slimit=slimit,
+            tol=tol,
+            mask=mask,
+            joint_limits=joint_limits,
+            seed=seed,
+            **kwargs,
+        )
+
+        self.pinv = pinv
+        self.kq = kq
+        self.km = km
+        self.ps = ps
+        self.pi = pi
+
+        self.name = f"NR (pinv={pinv})"
+
+        if self.kq > 0.0:
+            self.name += " Σ"
+
+        if self.km > 0.0:
+            self.name += " Jm"
+
+    def step(
+        self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray
+    ) -> Tuple[float, np.ndarray]:
+        r"""
+        Performs a single iteration of the Newton-Raphson optimisation method
+
+        :param ets: The ETS representing the manipulators kinematics
+        :param Tep: The desired end-effector pose
+        :param q: The current joint coordinate vector
+        :raises numpy.LinAlgError: If a step is impossible due to a linear algebra error
+        :returns: Tuple of ``(E, q)`` where ``E`` is the new error value and ``q`` is
+            the new joint coordinate vector
+        :rtype: tuple[float, numpy.ndarray]
+
+        .. math::
+
+            \vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k
+
+        """
+
+        Te = ets.eval(q)
+        e, E = self.error(Te, Tep)
+
+        J = ets.jacob0(q)
+
+        # Null-space motion
+        qnull = _calc_qnull(
+            ets=ets, q=q, J=J, λΣ=self.kq, λm=self.km, ps=self.ps, pi=self.pi
+        )
+
+        if self.pinv:
+            q[ets.jindices] += np.linalg.pinv(J) @ e + qnull
+        else:
+            q[ets.jindices] += np.linalg.inv(J) @ e + qnull
+
+        return E, q[ets.jindices]
+
+
+class IK_LM(IKSolver):
+    r"""
+    Levemberg-Marquadt Numerical Inverse Kinematics Solver
+
+    A class which provides functionality to perform numerical inverse kinematics (IK)
+    using the Levemberg-Marquadt method. See ``step`` method for mathematical description.
+
+    :param name: The name of the IK algorithm
+    :param ilimit: How many iterations are allowed within a search before a new search
+        is started
+    :param slimit: How many searches are allowed before being deemed unsuccessful
+    :param tol: Maximum allowed residual error E, where
+        :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form
+        in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`).
+        Because `E` is quadratic, `tol` does not bound the linear-scale position/
+        orientation error directly — with the default unit weighting, components of
+        :math:`\vec{e}` are only guaranteed to be within roughly
+        :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on
+        the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific
+        linear-scale accuracy
+    :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom
+        error priority
+    :param joint_limits: Reject solutions with joint limit violations
+    :param seed: A seed for the private RNG used to generate random joint coordinate
+        vectors
+    :param k: Sets the gain value for the damping matrix Wn in the ``step`` method. See
+        notes
+    :param method: One of "chan", "sugihara" or "wampler". Defines which method is used
+        to calculate the damping matrix Wn in the ``step`` method
+    :param kq: The gain for joint limit avoidance. Setting to 0.0 will remove this
+        completely from the solution
+    :param km: The gain for maximisation. Setting to 0.0 will remove this completely
+        from the solution
+    :param ps: The minimum angle/distance (in radians or metres) in which the joint is
+        allowed to approach to its limit
+    :param pi: The influence angle/distance (in radians or metres) in null space motion
+        becomes active
+
+    Example::
+
+        The following example gets the ``ets`` of a ``panda`` robot object, instantiates
+        the IK_LM solver class using default parameters, makes a goal pose ``Tep``,
+        and then solves for the joint coordinates which result in the pose ``Tep``
+        using the `solve` method.
+
+    .. runblock:: pycon
+    >>> import roboticstoolbox as rtb
+    >>> panda = rtb.models.Panda().ets()
+    >>> solver = rtb.IK_LM()
+    >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
+    >>> solver.solve(panda, Tep)
+
+    .. rubric:: 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 :math:`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.
+
+    .. rubric:: 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).
+
+    .. seealso::
+
+        :class:`IKSolver` An abstract super class for numerical IK solvers
+
+        :class:`IK_NR` Implements the IKSolver class using the Newton-Raphson method
+
+        :class:`IK_GN` Implements the IKSolver class using the Gauss-Newton method
+
+        :class:`IK_QP` Implements the IKSolver class using a quadratic programming approach
+
+    .. versionchanged:: 1.0.3
+        Added the Levemberg-Marquadt IK solver class
+
+    """
+
+    def __init__(
+        self,
+        name: str = "IK Solver",
+        ilimit: int = 30,
+        slimit: int = 100,
+        tol: float = 1e-6,
+        mask: ArrayLike | None = None,
+        joint_limits: bool = True,
+        seed: int | None = None,
+        k: float = 1.0,
+        method="chan",
+        kq: float = 0.0,
+        km: float = 0.0,
+        ps: float = 0.0,
+        pi: np.ndarray | float = 0.3,
+        **kwargs,
+    ):
+        super().__init__(
+            name=name,
+            ilimit=ilimit,
+            slimit=slimit,
+            tol=tol,
+            mask=mask,
+            joint_limits=joint_limits,
+            seed=seed,
+            **kwargs,
+        )
+
+        if method.lower().startswith("sugi"):
+            self.method = 1
+            method_name = "Sugihara"
+        elif method.lower().startswith("wamp"):
+            self.method = 2
+            method_name = "Wampler"
+        else:
+            self.method = 0
+            method_name = "Chan"
+
+        self.k = k
+        self.kq = kq
+        self.km = km
+        self.ps = ps
+        self.pi = pi
+
+        self.name = f"LM ({method_name} λ={k})"
+
+        if self.kq > 0.0:
+            self.name += " Σ"
+
+        if self.km > 0.0:
+            self.name += " Jm"
+
+    def step(self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray):
+        r"""
+        Performs a single iteration of the Levenberg-Marquadt optimisation
+
+        :param ets: The ETS representing the manipulators kinematics
+        :param Tep: The desired end-effector pose
+        :param q: The current joint coordinate vector
+        :raises numpy.LinAlgError: If a step is impossible due to a linear algebra error
+        :returns: Tuple of ``(E, q)`` where ``E`` is the new error value and ``q`` is
+            the new joint coordinate vector
+        :rtype: tuple[float, numpy.ndarray]
+
+        The operation is defined by the choice of `method` when instantiating the class.
+
+        The next step is defined as
+
+        .. math::
+            \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
+
+        where :math:`\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 :math:`\mat{A}_k` is
+        non-singular and positive definite. The performance of the LM method largely depends
+        on the choice of :math:`\mat{W}_n`.
+
+        **Chan's Method**
+
+        Chan proposed
+
+        .. math::
+            \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
+
+        .. math::
+            \mat{W}_n
+            =
+            E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n)
+
+        where :math:`\hat{\vec{w}}_n \in \mathbb{R}^n`, :math:`\hat{w}_{n_i} = l^2 \sim 0.01 l^2`,
+        and :math:`l` is the length of a typical link within the manipulator. We provide the
+        variable `k` as a kwarg to adjust the value of :math:`w_n`.
+
+        **Wampler's Method**
+
+        Wampler proposed :math:`\vec{w_n}` to be a constant. This is set through the `k` kwarg.
+        """
+
+        Te = ets.eval(q)
+        e, E = self.error(Te, Tep)
+
+        if self.method == 1:
+            # Sugihara's method
+            Wn = E * np.eye(ets.n) + self.k * np.eye(ets.n)
+        elif self.method == 2:
+            # Wampler's method
+            Wn = self.k * np.eye(ets.n)
+        else:
+            # Chan's method
+            Wn = self.k * E * np.eye(ets.n)
+
+        J = ets.jacob0(q)
+        g = J.T @ self.We @ e
+
+        # Null-space motion
+        qnull = _calc_qnull(
+            ets=ets, q=q, J=J, λΣ=self.kq, λm=self.km, ps=self.ps, pi=self.pi
+        )
+
+        q[ets.jindices] += np.linalg.inv(J.T @ self.We @ J + Wn) @ g + qnull
+
+        return E, q[ets.jindices]
+
+
+class IK_GN(IKSolver):
+    r"""
+    Gauss-Newton Numerical Inverse Kinematics Solver
+
+    A class which provides functionality to perform numerical inverse kinematics (IK)
+    using the Gauss-Newton method. See `step` method for mathematical description.
+
+    .. note::
+
+        When using this class with redundant robots (>6 DoF), ``pinv`` must be set to ``True``
+
+    :param name: The name of the IK algorithm
+    :param ilimit: How many iterations are allowed within a search before a new search
+        is started
+    :param slimit: How many searches are allowed before being deemed unsuccessful
+    :param tol: Maximum allowed residual error E, where
+        :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form
+        in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`).
+        Because `E` is quadratic, `tol` does not bound the linear-scale position/
+        orientation error directly — with the default unit weighting, components of
+        :math:`\vec{e}` are only guaranteed to be within roughly
+        :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on
+        the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific
+        linear-scale accuracy
+    :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom
+        error priority
+    :param joint_limits: Reject solutions with joint limit violations
+    :param seed: A seed for the private RNG used to generate random joint coordinate
+        vectors
+    :param pinv: If True, will use the pseudoinverse in the `step` method instead of
+        the normal inverse
+    :param kq: The gain for joint limit avoidance. Setting to 0.0 will remove this
+        completely from the solution
+    :param km: The gain for maximisation. Setting to 0.0 will remove this completely
+        from the solution
+    :param ps: The minimum angle/distance (in radians or metres) in which the joint is
+        allowed to approach to its limit
+    :param pi: The influence angle/distance (in radians or metres) in null space motion
+        becomes active
+
+    Example::
+
+        The following example gets the ``ets`` of a ``panda`` robot object, instantiates
+        the `IK_GN` solver class using default parameters, makes a goal pose ``Tep``,
+        and then solves for the joint coordinates which result in the pose ``Tep``
+        using the `solve` method.
+
+    .. runblock:: pycon
+    >>> import roboticstoolbox as rtb
+    >>> panda = rtb.models.Panda().ets()
+    >>> solver = rtb.IK_GN(pinv=True)
+    >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
+    >>> solver.solve(panda, Tep)
+
+    .. rubric:: Notes
+
+    When using the this method, the initial joint coordinates :math:`q_0`, should correspond
+    to a non-singular manipulator pose, since it uses the manipulator Jacobian. When the
+    the problem is solvable, it converges very quickly.
+
+    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.
+
+    .. rubric:: 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).
+
+    .. seealso::
+
+        :class:`IKSolver` An abstract super class for numerical IK solvers
+
+        :class:`IK_NR` Implements IKSolver using the Newton-Raphson method
+
+        :class:`IK_LM` Implements IKSolver using the Levemberg-Marquadt method
+
+        :class:`IK_QP` Implements IKSolver using a quadratic programming approach
+
+    .. versionchanged:: 1.0.3
+        Added the Gauss-Newton IK solver class
+
+    """
+
+    def __init__(
+        self,
+        name: str = "IK Solver",
+        ilimit: int = 30,
+        slimit: int = 100,
+        tol: float = 1e-6,
+        mask: ArrayLike | None = None,
+        joint_limits: bool = True,
+        seed: int | None = None,
+        pinv: bool = False,
+        kq: float = 0.0,
+        km: float = 0.0,
+        ps: float = 0.0,
+        pi: np.ndarray | float = 0.3,
+        **kwargs,
+    ):
+        super().__init__(
+            name=name,
+            ilimit=ilimit,
+            slimit=slimit,
+            tol=tol,
+            mask=mask,
+            joint_limits=joint_limits,
+            seed=seed,
+            **kwargs,
+        )
+
+        self.pinv = pinv
+        self.kq = kq
+        self.km = km
+        self.ps = ps
+        self.pi = pi
+
+        self.name = f"GN (pinv={pinv})"
+
+        if self.kq > 0.0:
+            self.name += " Σ"
+
+        if self.km > 0.0:
+            self.name += " Jm"
+
+    def step(
+        self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray
+    ) -> Tuple[float, np.ndarray]:
+        r"""
+        Performs a single iteration of the Gauss-Newton optimisation method
+
+        :param ets: The ETS representing the manipulators kinematics
+        :param Tep: The desired end-effector pose
+        :param q: The current joint coordinate vector
+        :raises numpy.LinAlgError: If a step is impossible due to a linear algebra error
+        :returns: Tuple of ``(E, q)`` where ``E`` is the new error value and ``q`` is
+            the new joint coordinate vector
+        :rtype: tuple[float, numpy.ndarray]
+
+        The next step is defined as
+
+        .. math::
+
+            \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
+
+        where :math:`\mat{J} = {^0\mat{J}}` is the base-frame manipulator Jacobian. If
+        :math:`\mat{J}(\vec{q}_k)` is non-singular, and :math:`\mat{W}_e = \mat{1}_n`, then
+        the above provides the pseudoinverse solution. However, if :math:`\mat{J}(\vec{q}_k)`
+        is singular, the above can not be computed and the GN solution is infeasible.
+        """
+
+        Te = ets.eval(q)
+        e, E = self.error(Te, Tep)
+
+        J = ets.jacob0(q)
+
+        # Null-space motion
+        qnull = _calc_qnull(
+            ets=ets, q=q, J=J, λΣ=self.kq, λm=self.km, ps=self.ps, pi=self.pi
+        )
+
+        if self.pinv:
+            q[ets.jindices] += np.linalg.pinv(J) @ e + qnull
+        else:
+            q[ets.jindices] += np.linalg.inv(J) @ e + qnull
+
+        return E, q[ets.jindices]
+
+
+class IK_QP(IKSolver):
+    r"""
+    Quadratic Progamming Numerical Inverse Kinematics Solver
+
+    A class which provides functionality to perform numerical inverse kinematics (IK)
+    using a quadratic progamming approach. See `step` method for mathematical
+    description.
+
+    :param name: The name of the IK algorithm
+    :param ilimit: How many iterations are allowed within a search before a new search
+        is started
+    :param slimit: How many searches are allowed before being deemed unsuccessful
+    :param tol: Maximum allowed residual error E, where
+        :math:`E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}` is a *quadratic* form
+        in the 6-vector angle-axis pose error :math:`\vec{e}` (see :meth:`error`).
+        Because `E` is quadratic, `tol` does not bound the linear-scale position/
+        orientation error directly — with the default unit weighting, components of
+        :math:`\vec{e}` are only guaranteed to be within roughly
+        :math:`\sqrt{2 \cdot \text{tol}}` (e.g. `tol=1e-6` guarantees pose error on
+        the order of 1e-3, not 1e-6). Pick `tol` accordingly if you need a specific
+        linear-scale accuracy
+    :param mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom
+        error priority
+    :param joint_limits: Reject solutions with joint limit violations
+    :param seed: A seed for the private RNG used to generate random joint coordinate
+        vectors
+    :param kj: A gain for joint velocity norm minimisation
+    :param ks: A gain which adjusts the cost of slack (intentional error)
+    :param kq: The gain for joint limit avoidance. Setting to 0.0 will remove this
+        completely from the solution
+    :param km: The gain for maximisation. Setting to 0.0 will remove this completely
+        from the solution
+    :param ps: The minimum angle/distance (in radians or metres) in which the joint is
+        allowed to approach to its limit
+    :param pi: The influence angle/distance (in radians or metres) in null space motion
+        becomes active
+    :raises ImportError: If the package ``qpsolvers`` is not installed
+
+    Example::
+
+        The following example gets the ``ets`` of a ``panda`` robot object, instantiates
+        the `IK_QP` solver class using default parameters, makes a goal pose ``Tep``,
+        and then solves for the joint coordinates which result in the pose ``Tep``
+        using the `solve` method.
+
+    .. runblock:: pycon
+    >>> import roboticstoolbox as rtb
+    >>> panda = rtb.models.Panda().ets()
+    >>> solver = rtb.IK_QP()
+    >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
+    >>> solver.solve(panda, Tep)
+
+    .. rubric:: Notes
+
+    When using the this method, the initial joint coordinates :math:`q_0`, should correspond
+    to a non-singular manipulator pose, since it uses the manipulator Jacobian. When the
+    the problem is solvable, it converges very quickly.
+
+    .. rubric:: References
+
+    - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
+      Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
+
+    .. seealso::
+
+        :class:`IKSolver` An abstract super class for numerical IK solvers
+
+        :class:`IK_NR` Implements IKSolver class using the Newton-Raphson method
+
+        :class:`IK_GN` Implements IKSolver class using the Gauss-Newton method
+
+        :class:`IK_LM` Implements IKSolver class using the Levemberg-Marquadt method
+
+    .. versionchanged:: 1.0.3
+        Added the Quadratic Programming IK solver class
+
+    """
+
+    def __init__(
+        self,
+        name: str = "IK Solver",
+        ilimit: int = 30,
+        slimit: int = 100,
+        tol: float = 1e-6,
+        mask: ArrayLike | None = None,
+        joint_limits: bool = True,
+        seed: int | None = None,
+        kj=0.01,
+        ks=1.0,
+        kq: float = 0.0,
+        km: float = 0.0,
+        ps: float = 0.0,
+        pi: np.ndarray | float = 0.3,
+        **kwargs,
+    ):
+        if not _qp:  # pragma: nocover
+            raise ImportError(
+                "the package qpsolvers is required for this class. \nInstall using 'pip"
+                " install qpsolvers'"
+            )
+
+        super().__init__(
+            name=name,
+            ilimit=ilimit,
+            slimit=slimit,
+            tol=tol,
+            mask=mask,
+            joint_limits=joint_limits,
+            seed=seed,
+            **kwargs,
+        )
+
+        self.kj = kj
+        self.ks = ks
+        self.kq = kq
+        self.km = km
+        self.ps = ps
+        self.pi = pi
+
+        self.name = "QP)"
+
+        if self.kq > 0.0:
+            self.name += " Σ"
+
+        if self.km > 0.0:
+            self.name += " Jm"
+
+    def step(
+        self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray
+    ) -> Tuple[float, np.ndarray]:
+        r"""
+        Performs a single iteration of the QP optimisation method
+
+        :param ets: The ETS representing the manipulators kinematics
+        :param Tep: The desired end-effector pose
+        :param q: The current joint coordinate vector
+        :raises numpy.LinAlgError: If a step is impossible due to a linear algebra error
+        :returns: Tuple of ``(E, q)`` where ``E`` is the new error value and ``q`` is
+            the new joint coordinate vector
+        :rtype: tuple[float, numpy.ndarray]
+
+        The next step is defined as
+
+        .. math::
+
+            \vec{q}_{k+1} = \vec{q}_{k} + \dot{\vec{q}}.
+
+        where the QP is defined as
+
+        .. math::
+
+            \min_x \quad f_o(\vec{x}) &= \frac{1}{2} \vec{x}^\top \mathcal{Q} \vec{x}+ \mathcal{C}^\top \vec{x}, \\
+            \text{subject to} \quad \mathcal{J} \vec{x} &= \vec{\nu},  \\
+            \mathcal{A} \vec{x} &\leq \mathcal{B},  \\
+            \vec{x}^- &\leq \vec{x} \leq \vec{x}^+
+
+        with
+
+        .. math::
+
+            \vec{x} &=
+            \begin{pmatrix}
+                \dvec{q} \\ \vec{\delta}
+            \end{pmatrix} \in \mathbb{R}^{(n+6)}  \\
+            \mathcal{Q} &=
+            \begin{pmatrix}
+                \lambda_q \mat{1}_{n} & \mathbf{0}_{6 \times 6} \\ \mathbf{0}_{n \times n} & \lambda_\delta \mat{1}_{6}
+            \end{pmatrix} \in \mathbb{R}^{(n+6) \times (n+6)} \\
+            \mathcal{J} &=
+            \begin{pmatrix}
+                \mat{J}(\vec{q}) & \mat{1}_{6}
+            \end{pmatrix} \in \mathbb{R}^{6 \times (n+6)} \\
+            \mathcal{C} &=
+            \begin{pmatrix}
+                \mat{J}_m \\ \bf{0}_{6 \times 1}
+            \end{pmatrix} \in \mathbb{R}^{(n + 6)} \\
+            \mathcal{A} &=
+            \begin{pmatrix}
+                \mat{1}_{n \times n + 6} \\
+            \end{pmatrix} \in \mathbb{R}^{(l + n) \times (n + 6)} \\
+            \mathcal{B} &=
+            \eta
+            \begin{pmatrix}
+                \frac{\rho_0 - \rho_s}
+                        {\rho_i - \rho_s} \\
+                \vdots \\
+                \frac{\rho_n - \rho_s}
+                        {\rho_i - \rho_s}
+            \end{pmatrix} \in \mathbb{R}^{n} \\
+            \vec{x}^{-, +} &=
+            \begin{pmatrix}
+                \dvec{q}^{-, +} \\
+                \vec{\delta}^{-, +}
+            \end{pmatrix} \in \mathbb{R}^{(n+6)},
+
+        where :math:`\vec{\delta} \in \mathbb{R}^6` is the slack vector,
+        :math:`\lambda_\delta \in \mathbb{R}^+` is a gain term which adjusts the
+        cost of the norm of the slack vector in the optimiser,
+        :math:`\dvec{q}^{-,+}` are the minimum and maximum joint velocities, and
+        :math:`\dvec{\delta}^{-,+}` are the minimum and maximum slack velocities.
+
+        """
+
+        Te = ets.eval(q)
+        e, E = self.error(Te, Tep)
+        J = ets.jacob0(q)
+
+        if isinstance(self.pi, float) or isinstance(self.pi, int):
+            self.pi = self.pi * np.ones(ets.n)
+
+        # Quadratic component of objective function
+        Q = np.eye(ets.n + 6)
+
+        # Joint velocity component of Q
+        Q[: ets.n, : ets.n] *= self.kj
+
+        # Slack component of Q
+        Q[ets.n :, ets.n :] = self.ks * (1 / np.sum(np.abs(e))) * np.eye(6)
+
+        # The equality contraints
+        Aeq = np.concatenate((J, np.eye(6)), axis=1)
+        beq = e.reshape((6,))
+
+        # The inequality constraints for joint limit avoidance
+        if self.kq > 0.0:
+            Ain = np.zeros((ets.n + 6, ets.n + 6))
+            bin = np.zeros(ets.n + 6)
+
+            # Form the joint limit velocity damper
+            Ain_l = np.zeros((ets.n, ets.n))
+            Bin_l = np.zeros(ets.n)
+
+            for i in range(ets.n):
+                ql0 = ets.qlim[0, i]
+                ql1 = ets.qlim[1, i]
+
+                if ql1 - q[i] <= self.pi[i]:
+                    Bin_l[i] = ((ql1 - q[i]) - self.ps) / (self.pi[i] - self.ps)
+                    Ain_l[i, i] = 1
+
+                if q[i] - ql0 <= self.pi[i]:
+                    Bin_l[i] = -(((ql0 - q[i]) + self.ps) / (self.pi[i] - self.ps))
+                    Ain_l[i, i] = -1
+
+            Ain[: ets.n, : ets.n] = Ain_l
+            bin[: ets.n] = (1.0 / self.kq) * Bin_l
+        else:
+            Ain = None
+            bin = None
+
+        # Manipulability maximisation
+        if self.km > 0.0:
+            Jm = ets.jacobm(q).reshape((ets.n,))
+            c = np.concatenate(((1.0 / self.km) * -Jm, np.zeros(6)))
+        else:
+            c = np.zeros(ets.n + 6)
+
+        xd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=None, ub=None, solver="quadprog")
+
+        if xd is None:  # pragma: nocover
+            raise np.linalg.LinAlgError("QP Unsolvable")
+
+        q += xd[: ets.n]
+
+        return E, q
+
+
+if __name__ == "__main__":  # pragma nocover
+    sol = IKSolution(
+        np.array([1, 2, 3]), success=True, iterations=10, searches=100, residual=0.1
+    )
+
+    a, b, c, d, e = sol
+
+    print(a, b, c, d, e)