text: """
  Utility functions of matrix and vector transformations.
  
  NOTE: convention for quaternions is (x, y, z, w)
  """
  
  import math
  import numpy as np
  
  
  PI = np.pi
  EPS = np.finfo(float).eps * 4.
  
  # axis sequences for Euler angles
  _NEXT_AXIS = [1, 2, 0, 1]
  
  # map axes strings to/from tuples of inner axis, parity, repetition, frame
  _AXES2TUPLE = {
      "sxyz": (0, 0, 0, 0),
      "sxyx": (0, 0, 1, 0),
      "sxzy": (0, 1, 0, 0),
      "sxzx": (0, 1, 1, 0),
      "syzx": (1, 0, 0, 0),
      "syzy": (1, 0, 1, 0),
      "syxz": (1, 1, 0, 0),
      "syxy": (1, 1, 1, 0),
      "szxy": (2, 0, 0, 0),
      "szxz": (2, 0, 1, 0),
      "szyx": (2, 1, 0, 0),
      "szyz": (2, 1, 1, 0),
      "rzyx": (0, 0, 0, 1),
      "rxyx": (0, 0, 1, 1),
      "ryzx": (0, 1, 0, 1),
      "rxzx": (0, 1, 1, 1),
      "rxzy": (1, 0, 0, 1),
      "ryzy": (1, 0, 1, 1),
      "rzxy": (1, 1, 0, 1),
      "ryxy": (1, 1, 1, 1),
      "ryxz": (2, 0, 0, 1),
      "rzxz": (2, 0, 1, 1),
      "rxyz": (2, 1, 0, 1),
      "rzyz": (2, 1, 1, 1),
  }
  
  _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
  
  
  def convert_quat(q, to="xyzw"):
      """
      Converts quaternion from one convention to another.
      The convention to convert TO is specified as an optional argument.
      If to == 'xyzw', then the input is in 'wxyz' format, and vice-versa.
  
      Args:
          q: a 4-dim numpy array corresponding to a quaternion
          to: a string, either 'xyzw' or 'wxyz', determining
              which convention to convert to.
      """
      if to == "xyzw":
          return q[[1, 2, 3, 0]]
      if to == "wxyz":
          return q[[3, 0, 1, 2]]
      raise Exception("convert_quat: choose a valid `to` argument (xyzw or wxyz)")
  
  
  def quat_multiply(quaternion1, quaternion0):
      """Return multiplication of two quaternions.
      >>> q = quat_multiply([1, -2, 3, 4], [-5, 6, 7, 8])
      >>> np.allclose(q, [-44, -14, 48, 28])
      True
      """
      x0, y0, z0, w0 = quaternion0
      x1, y1, z1, w1 = quaternion1
      return np.array(
          (
              x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
              -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
              x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
              -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
          ),
          dtype=np.float32,
      )
  
  
  def quat_conjugate(quaternion):
      """Return conjugate of quaternion.
      >>> q0 = random_quaternion()
      >>> q1 = quat_conjugate(q0)
      >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3])
      True
      """
      return np.array(
          (-quaternion[0], -quaternion[1], -quaternion[2], quaternion[3]),
          dtype=np.float32,
      )
  
  
  def quat_inverse(quaternion):
      """Return inverse of quaternion.
      >>> q0 = random_quaternion()
      >>> q1 = quat_inverse(q0)
      >>> np.allclose(quat_multiply(q0, q1), [0, 0, 0, 1])
      True
      """
      return quat_conjugate(quaternion) / np.dot(quaternion, quaternion)
  
  
  def quat_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
      """Return spherical linear interpolation between two quaternions.
      >>> q0 = random_quat()
      >>> q1 = random_quat()
      >>> q = quat_slerp(q0, q1, 0.0)
      >>> np.allclose(q, q0)
      True
      >>> q = quat_slerp(q0, q1, 1.0, 1)
      >>> np.allclose(q, q1)
      True
      >>> q = quat_slerp(q0, q1, 0.5)
      >>> angle = math.acos(np.dot(q0, q))
      >>> np.allclose(2.0, math.acos(np.dot(q0, q1)) / angle) or \
          np.allclose(2.0, math.acos(-np.dot(q0, q1)) / angle)
      True
      """
      q0 = unit_vector(quat0[:4])
      q1 = unit_vector(quat1[:4])
      if fraction == 0.0:
          return q0
      elif fraction == 1.0:
          return q1
      d = np.dot(q0, q1)
      if abs(abs(d) - 1.0) < _EPS:
          return q0
      if shortestpath and d < 0.0:
          # invert rotation
          d = -d
          q1 *= -1.0
      angle = math.acos(d) + spin * math.pi
      if abs(angle) < _EPS:
          return q0
      isin = 1.0 / math.sin(angle)
      q0 *= math.sin((1.0 - fraction) * angle) * isin
      q1 *= math.sin(fraction * angle) * isin
      q0 += q1
      return q0
  
  
  def random_quat(rand=None):
      """Return uniform random unit quaternion.
      rand: array like or None
          Three independent random variables that are uniformly distributed
          between 0 and 1.
      >>> q = random_quat()
      >>> np.allclose(1.0, vector_norm(q))
      True
      >>> q = random_quat(np.random.random(3))
      >>> q.shape
      (4,)
      """
      if rand is None:
          rand = np.random.rand(3)
      else:
          assert len(rand) == 3
      r1 = np.sqrt(1.0 - rand[0])
      r2 = np.sqrt(rand[0])
      pi2 = math.pi * 2.0
      t1 = pi2 * rand[1]
      t2 = pi2 * rand[2]
      return np.array(
          (np.sin(t1) * r1, np.cos(t1) * r1, np.sin(t2) * r2, np.cos(t2) * r2),
          dtype=np.float32,
      )
  
  
  def vec(values):
      """
      Converts value tuple into a numpy vector.
  
      Args:
          values: a tuple of numbers
  
      Returns:
          a numpy vector of given values
      """
      return np.array(values, dtype=np.float32)
  
  
  def mat4(array):
      """
      Converts an array to 4x4 matrix.
  
      Args:
          array: the array in form of vec, list, or tuple
  
      Returns:
          a 4x4 numpy matrix
      """
      return np.array(array, dtype=np.float32).reshape((4, 4))
  
  
  def mat2pose(hmat):
      """
      Converts a homogeneous 4x4 matrix into pose.
  
      Args:
          hmat: a 4x4 homogeneous matrix
  
      Returns:
          (pos, orn) tuple where pos is vec3 float in cartesian,
              orn is vec4 float quaternion
      """
      pos = hmat[:3, 3]
      orn = mat2quat(hmat[:3, :3])
      return pos, orn
  
  
  def mat2quat(rmat, precise=False):
      """
      Converts given rotation matrix to quaternion.
  
      Args:
          rmat: 3x3 rotation matrix
          precise: If isprecise is True, the input matrix is assumed to be a precise
               rotation matrix and a faster algorithm is used.
  
      Returns:
          vec4 float quaternion angles
      """
      M = np.array(rmat, dtype=np.float32, copy=False)[:3, :3]
      if precise:
          q = np.empty((4,))
          t = np.trace(M)
          if t > M[3, 3]:
              q[0] = t
              q[3] = M[1, 0] - M[0, 1]
              q[2] = M[0, 2] - M[2, 0]
              q[1] = M[2, 1] - M[1, 2]
          else:
              i, j, k = 0, 1, 2
              if M[1, 1] > M[0, 0]:
                  i, j, k = 1, 2, 0
              if M[2, 2] > M[i, i]:
                  i, j, k = 2, 0, 1
              t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
              q[i] = t
              q[j] = M[i, j] + M[j, i]
              q[k] = M[k, i] + M[i, k]
              q[3] = M[k, j] - M[j, k]
              q = q[[3, 0, 1, 2]]
          q *= 0.5 / math.sqrt(t * M[3, 3])
      else:
          m00 = M[0, 0]
          m01 = M[0, 1]
          m02 = M[0, 2]
          m10 = M[1, 0]
          m11 = M[1, 1]
          m12 = M[1, 2]
          m20 = M[2, 0]
          m21 = M[2, 1]
          m22 = M[2, 2]
          # symmetric matrix K
          K = np.array(
              [
                  [m00 - m11 - m22, 0.0, 0.0, 0.0],
                  [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
                  [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
                  [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22],
              ]
          )
          K /= 3.0
          # quaternion is Eigen vector of K that corresponds to largest eigenvalue
          w, V = np.linalg.eigh(K)
          q = V[[3, 0, 1, 2], np.argmax(w)]
      if q[0] < 0.0:
          np.negative(q, q)
      return q[[1, 2, 3, 0]]
  
  
  def mat2euler(rmat, axes="sxyz"):
      """
      Converts given rotation matrix to euler angles in radian.
  
      Args:
          rmat: 3x3 rotation matrix
          axes: One of 24 axis sequences as string or encoded tuple
  
      Returns:
          converted euler angles in radian vec3 float
      """
      try:
          firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
      except (AttributeError, KeyError):
          firstaxis, parity, repetition, frame = axes
  
      i = firstaxis
      j = _NEXT_AXIS[i + parity]
      k = _NEXT_AXIS[i - parity + 1]
  
      M = np.array(rmat, dtype=np.float32, copy=False)[:3, :3]
      if repetition:
          sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
          if sy > EPS:
              ax = math.atan2(M[i, j], M[i, k])
              ay = math.atan2(sy, M[i, i])
              az = math.atan2(M[j, i], -M[k, i])
          else:
              ax = math.atan2(-M[j, k], M[j, j])
              ay = math.atan2(sy, M[i, i])
              az = 0.0
      else:
          cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
          if cy > EPS:
              ax = math.atan2(M[k, j], M[k, k])
              ay = math.atan2(-M[k, i], cy)
              az = math.atan2(M[j, i], M[i, i])
          else:
              ax = math.atan2(-M[j, k], M[j, j])
              ay = math.atan2(-M[k, i], cy)
              az = 0.0
  
      if parity:
          ax, ay, az = -ax, -ay, -az
      if frame:
          ax, az = az, ax
      return vec((ax, ay, az))
  
  
  def pose2mat(pose):
      """
      Converts pose to homogeneous matrix.
  
      Args:
          pose: a (pos, orn) tuple where pos is vec3 float cartesian, and
              orn is vec4 float quaternion.
  
      Returns:
          4x4 homogeneous matrix
      """
      homo_pose_mat = np.zeros((4, 4), dtype=np.float32)
      homo_pose_mat[:3, :3] = quat2mat(pose[1])
      homo_pose_mat[:3, 3] = np.array(pose[0], dtype=np.float32)
      homo_pose_mat[3, 3] = 1.
      return homo_pose_mat
  
  
  def quat2mat(quaternion):
      """
      Converts given quaternion (x, y, z, w) to matrix.
  
      Args:
          quaternion: vec4 float angles
  
      Returns:
          3x3 rotation matrix
      """
      q = np.array(quaternion, dtype=np.float32, copy=True)[[3, 0, 1, 2]]
      n = np.dot(q, q)
      if n < EPS:
          return np.identity(3)
      q *= math.sqrt(2.0 / n)
      q = np.outer(q, q)
      return np.array(
          [
              [1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0]],
              [q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0]],
              [q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2]],
          ]
      )
  
  
  def pose_in_A_to_pose_in_B(pose_A, pose_A_in_B):
      """
      Converts a homogenous matrix corresponding to a point C in frame A
      to a homogenous matrix corresponding to the same point C in frame B.
  
      Args:
          pose_A: numpy array of shape (4,4) corresponding to the pose of C in frame A
          pose_A_in_B: numpy array of shape (4,4) corresponding to the pose of A in frame B
  
      Returns:
          numpy array of shape (4,4) corresponding to the pose of C in frame B
      """
  
      # pose of A in B takes a point in A and transforms it to a point in C.
  
      # pose of C in B = pose of A in B * pose of C in A
      # take a point in C, transform it to A, then to B
      # T_B^C = T_A^C * T_B^A
      return pose_A_in_B.dot(pose_A)
  
  
  def pose_inv(pose):
      """
      Computes the inverse of a homogenous matrix corresponding to the pose of some
      frame B in frame A. The inverse is the pose of frame A in frame B.
  
      Args:
          pose: numpy array of shape (4,4) for the pose to inverse
  
      Returns:
          numpy array of shape (4,4) for the inverse pose
      """
  
      # Note, the inverse of a pose matrix is the following
      # [R t; 0 1]^-1 = [R.T -R.T*t; 0 1]
  
      # Intuitively, this makes sense.
      # The original pose matrix translates by t, then rotates by R.
      # We just invert the rotation by applying R-1 = R.T, and also translate back.
      # Since we apply translation first before rotation, we need to translate by
      # -t in the original frame, which is -R-1*t in the new frame, and then rotate back by
      # R-1 to align the axis again.
  
      pose_inv = np.zeros((4, 4))
      pose_inv[:3, :3] = pose[:3, :3].T
      pose_inv[:3, 3] = -pose_inv[:3, :3].dot(pose[:3, 3])
      pose_inv[3, 3] = 1.0
      return pose_inv
  
  
  def _skew_symmetric_translation(pos_A_in_B):
      """
      Helper function to get a skew symmetric translation matrix for converting quantities
      between frames.
      """
      return np.array(
          [
              0.,
              -pos_A_in_B[2],
              pos_A_in_B[1],
              pos_A_in_B[2],
              0.,
              -pos_A_in_B[0],
              -pos_A_in_B[1],
              pos_A_in_B[0],
              0.,
          ]
      ).reshape((3, 3))
  
  
  def vel_in_A_to_vel_in_B(vel_A, ang_vel_A, pose_A_in_B):
      """
      Converts linear and angular velocity of a point in frame A to the equivalent in frame B.
  
      Args:
          vel_A: 3-dim iterable for linear velocity in A
          ang_vel_A: 3-dim iterable for angular velocity in A
          pose_A_in_B: numpy array of shape (4,4) corresponding to the pose of A in frame B
  
      Returns:
          vel_B, ang_vel_B: two numpy arrays of shape (3,) for the velocities in B
      """
      pos_A_in_B = pose_A_in_B[:3, 3]
      rot_A_in_B = pose_A_in_B[:3, :3]
      skew_symm = _skew_symmetric_translation(pos_A_in_B)
      vel_B = rot_A_in_B.dot(vel_A) + skew_symm.dot(rot_A_in_B.dot(ang_vel_A))
      ang_vel_B = rot_A_in_B.dot(ang_vel_A)
      return vel_B, ang_vel_B
  
  
  def force_in_A_to_force_in_B(force_A, torque_A, pose_A_in_B):
      """
      Converts linear and rotational force at a point in frame A to the equivalent in frame B.
  
      Args:
          force_A: 3-dim iterable for linear force in A
          torque_A: 3-dim iterable for rotational force (moment) in A
          pose_A_in_B: numpy array of shape (4,4) corresponding to the pose of A in frame B
  
      Returns:
          force_B, torque_B: two numpy arrays of shape (3,) for the forces in B
      """
      pos_A_in_B = pose_A_in_B[:3, 3]
      rot_A_in_B = pose_A_in_B[:3, :3]
      skew_symm = _skew_symmetric_translation(pos_A_in_B)
      force_B = rot_A_in_B.T.dot(force_A)
      torque_B = -rot_A_in_B.T.dot(skew_symm.dot(force_A)) + rot_A_in_B.T.dot(torque_A)
      return force_B, torque_B
  
  
  def rotation_matrix(angle, direction, point=None):
      """
      Returns matrix to rotate about axis defined by point and direction.
  
      Examples:
  
          >>> angle = (random.random() - 0.5) * (2*math.pi)
          >>> direc = numpy.random.random(3) - 0.5
          >>> point = numpy.random.random(3) - 0.5
          >>> R0 = rotation_matrix(angle, direc, point)
          >>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
          >>> is_same_transform(R0, R1)
          True
          >>> R0 = rotation_matrix(angle, direc, point)
          >>> R1 = rotation_matrix(-angle, -direc, point)
          >>> is_same_transform(R0, R1)
          True
          >>> I = numpy.identity(4, numpy.float32)
          >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc))
          True
          >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2,
          ...                                                direc, point)))
          True
  
      """
      sina = math.sin(angle)
      cosa = math.cos(angle)
      direction = unit_vector(direction[:3])
      # rotation matrix around unit vector
      R = np.array(
          ((cosa, 0.0, 0.0), (0.0, cosa, 0.0), (0.0, 0.0, cosa)), dtype=np.float32
      )
      R += np.outer(direction, direction) * (1.0 - cosa)
      direction *= sina
      R += np.array(
          (
              (0.0, -direction[2], direction[1]),
              (direction[2], 0.0, -direction[0]),
              (-direction[1], direction[0], 0.0),
          ),
          dtype=np.float32,
      )
      M = np.identity(4)
      M[:3, :3] = R
      if point is not None:
          # rotation not around origin
          point = np.array(point[:3], dtype=np.float32, copy=False)
          M[:3, 3] = point - np.dot(R, point)
      return M
  
  
  def make_pose(translation, rotation):
      """
      Makes a homogenous pose matrix from a translation vector and a rotation matrix.
  
      Args:
          translation: a 3-dim iterable
          rotation: a 3x3 matrix
  
      Returns:
          pose: a 4x4 homogenous matrix
      """
      pose = np.zeros((4, 4))
      pose[:3, :3] = rotation
      pose[:3, 3] = translation
      pose[3, 3] = 1.0
      return pose
  
  
  def unit_vector(data, axis=None, out=None):
      """
      Returns ndarray normalized by length, i.e. eucledian norm, along axis.
  
      Examples:
  
          >>> v0 = numpy.random.random(3)
          >>> v1 = unit_vector(v0)
          >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0))
          True
          >>> v0 = numpy.random.rand(5, 4, 3)
          >>> v1 = unit_vector(v0, axis=-1)
          >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2)
          >>> numpy.allclose(v1, v2)
          True
          >>> v1 = unit_vector(v0, axis=1)
          >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1)
          >>> numpy.allclose(v1, v2)
          True
          >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float32)
          >>> unit_vector(v0, axis=1, out=v1)
          >>> numpy.allclose(v1, v2)
          True
          >>> list(unit_vector([]))
          []
          >>> list(unit_vector([1.0]))
          [1.0]
  
      """
      if out is None:
          data = np.array(data, dtype=np.float32, copy=True)
          if data.ndim == 1:
              data /= math.sqrt(np.dot(data, data))
              return data
      else:
          if out is not data:
              out[:] = np.array(data, copy=False)
          data = out
      length = np.atleast_1d(np.sum(data * data, axis))
      np.sqrt(length, length)
      if axis is not None:
          length = np.expand_dims(length, axis)
      data /= length
      if out is None:
          return data
  
  
  def get_orientation_error(target_orn, current_orn):
      """
      Returns the difference between two quaternion orientations as a 3 DOF numpy array.
      For use in an impedance controller / task-space PD controller.
  
      Args:
          target_orn: 4-dim iterable, desired orientation as a (x, y, z, w) quaternion
          current_orn: 4-dim iterable, current orientation as a (x, y, z, w) quaternion
  
      Returns:
          orn_error: 3-dim numpy array for current orientation error, corresponds to
              (target_orn - current_orn)
      """
      current_orn = np.array(
          [current_orn[3], current_orn[0], current_orn[1], current_orn[2]]
      )
      target_orn = np.array([target_orn[3], target_orn[0], target_orn[1], target_orn[2]])
  
      pinv = np.zeros((3, 4))
      pinv[0, :] = [-current_orn[1], current_orn[0], -current_orn[3], current_orn[2]]
      pinv[1, :] = [-current_orn[2], current_orn[3], current_orn[0], -current_orn[1]]
      pinv[2, :] = [-current_orn[3], -current_orn[2], current_orn[1], current_orn[0]]
      orn_error = 2.0 * pinv.dot(np.array(target_orn))
      return orn_error
  
  
  def get_pose_error(target_pose, current_pose):
      """
      Computes the error corresponding to target pose - current pose as a 6-dim vector.
      The first 3 components correspond to translational error while the last 3 components
      correspond to the rotational error.
  
      Args:
          target_pose: a 4x4 homogenous matrix for the target pose
          current_pose: a 4x4 homogenous matrix for the current pose
  
      Returns:
          A 6-dim numpy array for the pose error.
      """
      error = np.zeros(6)
  
      # compute translational error
      target_pos = target_pose[:3, 3]
      current_pos = current_pose[:3, 3]
      pos_err = target_pos - current_pos
  
      # compute rotational error
      r1 = current_pose[:3, 0]
      r2 = current_pose[:3, 1]
      r3 = current_pose[:3, 2]
      r1d = target_pose[:3, 0]
      r2d = target_pose[:3, 1]
      r3d = target_pose[:3, 2]
      rot_err = 0.5 * (np.cross(r1, r1d) + np.cross(r2, r2d) + np.cross(r3, r3d))
  
      error[:3] = pos_err
      error[3:] = rot_err
      return error
  