Source code for skbot.inverse_kinematics.targets

from numpy.typing import ArrayLike
from typing import Callable, List, Union
import numpy as np
from .. import transform as tf


class Target:
    """Abstract IK target.

    .. versionadded:: 0.10.0

    Parameters
    ----------
    static_frame : tf.Frame
        The frame in which the objective is constant.
    dynamic_frame : tf.Frame
        The frame in which the score is computed.
    atol : float
        The absolute tolerance for the score. If score is below this value
        the target is considered to be reached.
    """

    def __init__(
        self, static_frame: tf.Frame, dynamic_frame: tf.Frame, *, atol: float = 1e-3
    ) -> None:
        self.static_frame = static_frame
        self.dynamic_frame = dynamic_frame
        self._chain = self.static_frame.links_between(self.dynamic_frame)
        self.atol = atol

    def score(self):
        """The score of this target."""
        raise NotImplementedError

    def usage_count(self, joint: tf.Link) -> int:
        """Frequency of joint use in this target.

        This function counts the number of times that ``joint`` is used when
        computing the score for this target.

        Parameters
        ----------
        joint : tf.Link
            The link that has its frequency evaluated.

        Returns
        -------
        frequency : int
            The number of occurences of the link.

        """
        occurences = 0

        for link in self._chain:
            if link is joint:
                occurences += 1
            elif isinstance(link, tf.InvertLink) and link._forward_link is joint:
                occurences += 1

        return occurences

    def uses(self, joint: tf.Link) -> bool:
        """Check if target uses a joint.

        Parameters
        ----------
        joint : tf.Link
            The link to check.

        Returns
        -------
        is_used : bool
            True if joint is used when evaluating this target's score. False
            otherwise.
        """

        for link in self._chain:
            if link is joint:
                return True
            elif isinstance(link, tf.InvertLink) and link._forward_link is joint:
                return True
        return False


[docs]class PositionTarget(Target): """IK position target (nD). This target can be used to find an IK solution that positions a point (``static_position``) expressed in ``static_frame`` at a desired target position (``dynamic_position``) expressed in ``dynamic_frame``. To compute the current score, this target transforms ``static_position`` from ``static_frame`` into ``dynamic_frame`` and then measures the distance between the transformed point and ``dynamic_positon`` under the desired norm (default: L2). .. versionadded:: 0.10.0 Parameters ---------- static_position : ArrayLike The value of a position that moves in ``dynamic_frame`` expressed in ``static_frame``. dynamic_positon : ArrayLike The value of the target position expressed in ``dynamic_frame``. static_frame : tf.Frame The frame in which the moving position is expressed. dynamic_frame : tf.Frame The frame in which the target position is expressed. norm : Callable A function of the form ``norm(ArrayLike) -> float`` that computes the norm of the distance between ``target_position`` and the transformed ``static_position`` in ``dynamic_frame``. If None defaults to L2. """
[docs] def __init__( self, static_position: ArrayLike, dynamic_position: ArrayLike, static_frame: tf.Frame, dynamic_frame: tf.Frame, norm: Callable[[np.ndarray], float] = None, *, atol: float = 1e-3, ) -> None: super().__init__(static_frame, dynamic_frame, atol=atol) self.static_position = np.asarray(static_position) self.dynamic_position = np.asarray(dynamic_position) if norm is None: self.norm = np.linalg.norm else: self.norm = norm
def score(self): current_pos = self.static_position for link in self._chain: current_pos = link.transform(current_pos) return self.norm(self.dynamic_position - current_pos)
[docs]class RotationTarget(Target): """IK rotation target (2D/3D). This target can be used to find an IK solution such that ``dynamic_frame`` has rotation ``desired_rotation`` when the rotation is expressed relative to ``static_frame``. The score function computes the distance in radians between the current rotation and desired rotation. .. versionadded:: 0.10.0 Parameters ---------- desired_rotation : Union[tf.Link, List[tf.Link]] A link or list of links that expresses the rotation of ``dynamic_frame`` relative to ``static_frame``. static_frame : tf.Frame The frame in which the rotation is expressed. dynamic_frame : tf.Frame The frame that should be rotated by ``desired_rotation`` relative to ``static_frame``. """
[docs] def __init__( self, desired_rotation: Union[tf.Link, List[tf.Link]], static_frame: tf.Frame, dynamic_frame: tf.Frame, *, atol: float = 1e-3, ) -> None: parent_dim = static_frame.ndim child_dim = dynamic_frame.ndim if parent_dim != child_dim: raise NotImplementedError("Projected Targets are not supported yet.") if parent_dim not in [2, 3]: raise NotImplementedError("Only 2D and 3D is currently supported.") super().__init__(static_frame, dynamic_frame, atol=atol) if isinstance(desired_rotation, tf.Link): self.desired_rotation = [desired_rotation] else: self.desired_rotation = desired_rotation self.desired_rotation = tf.simplify_links(self.desired_rotation) self.desired_rotation = [ x for x in self.desired_rotation if not isinstance(x, tf.Translation) ]
def score(self): basis = np.eye(self.static_frame.ndim) desired_basis = basis for link in self.desired_rotation: desired_basis = link.transform(desired_basis) reduced = tf.simplify_links(self._chain) reduced = [x for x in reduced if not isinstance(x, tf.Translation)] actual_basis = basis for link in reduced: actual_basis = link.transform(actual_basis) trace = np.trace(desired_basis @ actual_basis.T) if self.static_frame.ndim == 3: value = np.clip((trace - 1) / 2, -1, 1) theta = np.arccos(value) elif self.static_frame.ndim == 2: value = np.clip(trace / 2, -1, 1) theta = np.arccos(value) else: raise NotImplementedError("Only 2D and 3D is currently supported.") return theta