text: # Copyright 2015-2021 - RoboDK Inc. - https://robodk.com/
  # Licensed under the Apache License, Version 2.0 (the "License")
  # you may not use this file except in compliance with the License.
  # You may obtain a copy of the License at
  # http://www.apache.org/licenses/LICENSE-2.0
  # Unless required by applicable law or agreed to in writing, software
  # distributed under the License is distributed on an "AS IS" BASIS,
  # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  # See the License for the specific language governing permissions and
  # limitations under the License.
  
  # --------------------------------------------
  # --------------- DESCRIPTION ----------------
  #
  # This is a robotics toolbox for RoboDK robot post processors and RoboDK API for Python
  # This toolbox includes a simple matrix class for pose transofmrations (Mat)
  # This toolbox has been inspired from Peter Corke's Robotics Toolbox:
  # http://petercorke.com/wordpress/toolboxes/robotics-toolbox
  #
  # In this document: pose = transformation matrix = homogeneous matrix = 4x4 matrix
  #
  # More information about the RoboDK API for Python here:
  #     https://robodk.com/doc/en/RoboDK-API.html
  #     https://robodk.com/doc/en/PythonAPI/index.html
  #
  # More information about RoboDK post processors here:
  #     https://robodk.com/help#PostProcessor
  #
  # Visit the Matrix and Quaternions FAQ for more information about pose/homogeneous transformations
  #     http://www.j3d.org/matrix_faq/matrfaq_latest.html
  # --------------------------------------------
  
  import math
  import operator
  import sys
  import unittest
  import time
  
  #----------------------------------------------------
  #--------      Generic file usage     ---------------
  
  import os.path
  import time
  
  
  def searchfiles(pattern='C:\\RoboDK\\Library\\*.rdk'):
      """List the files in a directory with a given extension"""
      import glob
      return glob.glob(pattern)
  
  
  #def CurrentFile(file = __file__):
  #    """Returns the current Python file being executed"""
  #    return os.path.realpath(file)
  
  
  def getFileDir(filepath):
      """Returns the directory of a file path"""
      return os.path.dirname(filepath)
  
  
  def getBaseName(filepath):
      """Returns the file name and extension of a file path"""
      return os.path.basename(filepath)
  
  
  def getFileName(filepath):
      """Returns the file name (with no extension) of a file path"""
      return os.path.splitext(os.path.basename(filepath))[0]
  
  
  def DateModified(filepath, stringformat=False):
      """Returns the time that a file was modified"""
      time_in_s = os.path.getmtime(filepath)
      if stringformat:
          return time.ctime(time_in_s)
      else:
          return time_in_s
  
  
  def DateCreated(filepath, stringformat=False):
      """Returns the time that a file was modified"""
      time_in_s = os.path.getctime(filepath)
      if stringformat:
          return time.ctime(time_in_s)
      else:
          return time_in_s
  
  
  def DirExists(folder):
      """Returns true if the folder exists"""
      return os.path.isdir(folder)
  
  
  def FileExists(file):
      """Returns true if the file exists"""
      return os.path.exists(file)
  
  
  def FilterName(namefilter, safechar='P', reserved_names=None):
      """Get a safe program or variable name that can be used for robot programming"""
      # remove non accepted characters
      for c in r' -[]/\;,><&*:%=+@!#^|?^':
          namefilter = namefilter.replace(c, '')
  
      # remove non english characters
      char_list = (c for c in namefilter if 0 < ord(c) < 127)
      namefilter = ''.join(char_list)
  
      # Make sure we have a non empty string
      if len(namefilter) <= 0:
          namefilter = safechar
  
      # Make sure we don't start with a number
      if namefilter[0].isdigit():
          print(namefilter)
          namefilter = safechar + namefilter
  
      # Make sure we are not using a reserved name
      if reserved_names is not None:
          while namefilter.lower() in reserved_names:
              namefilter = safechar + namefilter
  
          # Add the name to reserved names
          reserved_names.append(namefilter)
  
      return namefilter
  
  
  #----------------------------------------------------
  #--------      Generic math usage     ---------------
  
  pi = math.pi
  
  
  def pause(seconds):
      """Pause in seconds
  
      :param pause: time in seconds
      :type pause: float"""
      time.sleep(seconds)
  
  
  def atan2(y, x):
      """Returns angle of a 2D coordinate in the XY plane"""
      return math.atan2(y, x)
  
  
  def sqrt(value):
      """Returns the square root of a value"""
      return math.sqrt(value)
  
  
  def sin(value):
      """Returns the sinus of an angle in radians"""
      return math.sin(value)
  
  
  def cos(value):
      """Returns the cosinus of an angle in radians"""
      return math.cos(value)
  
  
  def asin(value):
      """Returns the arc sinus in radians"""
      return math.asin(value)
  
  
  def acos(value):
      """Returns the arc cosinus in radians"""
      return math.acos(value)
  
  
  def name_2_id(str_name_id):
      """Returns the number of a numbered object. For example: "Frame 3", "Frame3", "Fram3 3" returns 3."""
      import re
      numbers = re.findall(r'[0-9]+', str_name_id)
      if len(numbers) > 0:
          return float(numbers[-1])
      return -1
  
  
  #----------------------------------------------------
  #--------     Generic matrix usage    ---------------
  
  
  def rotx(rx):
      r"""Returns a rotation matrix around the X axis (radians)
  
      .. math::
  
          R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 & 0 \\
          0 & c_\theta & -s_\theta & 0 \\
          0 & s_\theta & c_\theta & 0 \\
          0 & 0 & 0 & 1
          \end{bmatrix}
  
      :param float rx: rotation around X axis in radians
  
      .. seealso:: :func:`~robodk.transl`, :func:`~robodk.roty`, :func:`~robodk.roty`
      """
      ct = math.cos(rx)
      st = math.sin(rx)
      return Mat([[1, 0, 0, 0], [0, ct, -st, 0], [0, st, ct, 0], [0, 0, 0, 1]])
  
  
  def roty(ry):
      r"""Returns a rotation matrix around the Y axis (radians)
  
      .. math::
  
          R_y(\theta) = \begin{bmatrix} c_\theta & 0 & s_\theta & 0 \\
          0 & 1 & 0 & 0 \\
          -s_\theta & 0 & c_\theta & 0 \\
          0 & 0 & 0 & 1
          \end{bmatrix}
  
      :param float ry: rotation around Y axis in radians
  
      .. seealso:: :func:`~robodk.transl`, :func:`~robodk.rotx`, :func:`~robodk.rotz`
      """
      ct = math.cos(ry)
      st = math.sin(ry)
      return Mat([[ct, 0, st, 0], [0, 1, 0, 0], [-st, 0, ct, 0], [0, 0, 0, 1]])
  
  
  def rotz(rz):
      r"""Returns a rotation matrix around the Z axis (radians)
  
      .. math::
  
          R_x(\theta) = \begin{bmatrix} c_\theta & -s_\theta & 0 & 0 \\
          s_\theta & c_\theta & 0 & 0 \\
          0 & 0 & 1 & 0 \\
          0 & 0 & 0 & 1
          \end{bmatrix}
  
      :param float ry: rotation around Y axis in radians
  
      .. seealso:: :func:`~robodk.transl`, :func:`~robodk.rotx`, :func:`~robodk.roty`
      """
      ct = math.cos(rz)
      st = math.sin(rz)
      return Mat([[ct, -st, 0, 0], [st, ct, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
  
  
  def transl(tx, ty=None, tz=None):
      r"""Returns a translation matrix (mm)
  
      .. math::
  
          T(t_x, t_y, t_z) = \begin{bmatrix} 1 & 0 & 0 & t_x \\
          0 & 1 & 0 & t_y \\
          0 & 0 & 1 & t_z \\
          0 & 0 & 0 & 1
          \end{bmatrix}
  
      :param float tx: translation along the X axis
      :param float ty: translation along the Y axis
      :param float tz: translation along the Z axis
  
      .. seealso:: :func:`~robodk.rotx`, :func:`~robodk.roty`, :func:`~robodk.rotz`
      """
      if ty is None:
          xx = tx[0]
          yy = tx[1]
          zz = tx[2]
      else:
          xx = tx
          yy = ty
          zz = tz
      return Mat([[1, 0, 0, xx], [0, 1, 0, yy], [0, 0, 1, zz], [0, 0, 0, 1]])
  
  
  def RelTool(target_pose, x, y, z, rx=0, ry=0, rz=0):
      """Calculates a relative target with respect to the tool coordinates. This procedure has exactly the same behavior as ABB's RelTool instruction.
      X,Y,Z are in mm, W,P,R are in degrees.
  
      :param float x: translation along the Tool X axis (mm)
      :param float y: translation along the Tool Y axis (mm)
      :param float z: translation along the Tool Z axis (mm)
      :param float rx: rotation around the Tool X axis (deg) (optional)
      :param float ry: rotation around the Tool Y axis (deg) (optional)
      :param float rz: rotation around the Tool Z axis (deg) (optional)
  
      .. seealso:: :func:`~robodk.Offset`, :func:`~robodk.transl`, :func:`~robodk.rotx`, :func:`~robodk.roty`, :func:`~robodk.rotz`
      """
      if type(target_pose) != Mat:
          target_pose = target_pose.Pose()
      new_target = target_pose * transl(x, y, z) * rotx(rx * pi / 180) * roty(ry * pi / 180) * rotz(rz * pi / 180)
      return new_target
  
  
  def Offset(target_pose, x, y, z, rx=0, ry=0, rz=0):
      """Calculates a relative target with respect to the reference frame coordinates.
      X,Y,Z are in mm, RX,RY,RZ are in degrees.
  
      :param float x: translation along the Reference X axis (mm)
      :param float y: translation along the Reference Y axis (mm)
      :param float z: translation along the Reference Z axis (mm)
      :param float rx: rotation around the Reference X axis (deg) (optional)
      :param float ry: rotation around the Reference Y axis (deg) (optional)
      :param float rz: rotation around the Reference Z axis (deg) (optional)
  
      .. seealso:: :func:`~robodk.RelTool`, :func:`~robodk.transl`, :func:`~robodk.rotx`, :func:`~robodk.roty`, :func:`~robodk.rotz`
      """
      if type(target_pose) != Mat:
          # item object assumed:
          target_pose = target_pose.Pose()
      if not target_pose.isHomogeneous():
          raise Exception(MatrixError, "Pose matrix is not homogeneous!")
      new_target = transl(x, y, z) * rotx(rx * pi / 180.0) * roty(ry * pi / 180.0) * rotz(rz * pi / 180.0) * target_pose
      return new_target
  
  
  def point_Zaxis_2_pose(point, zaxis, yaxis_hint1=[0, 0, 1], yaxis_hint2=[0, 1, 1]):
      """Returns a pose given the origin as a point, a Z axis and a preferred orientation for the Y axis"""
      pose = eye(4)
      pose.setPos(point)
      pose.setVZ(zaxis)
      yaprox = yaxis_hint1
      if angle3(zaxis, yaprox) < 2 * pi / 180:
          yaprox = yaxis_hint2
      xaxis = normalize3(cross(yaprox, zaxis))
      yaxis = cross(zaxis, xaxis)
      pose.setVX(xaxis)
      pose.setVY(yaxis)
      return pose
  
  
  def eye(size=4):
      r"""Returns the identity matrix
  
      .. math::
  
          T(t_x, t_y, t_z) = \begin{bmatrix} 1 & 0 & 0 & 0 \\
          0 & 1 & 0 & 0 \\
          0 & 0 & 1 & 0 \\
          0 & 0 & 0 & 1
          \end{bmatrix}
  
      :param int size: square matrix size (4x4 Identity matrix by default, otherwise it is initialized to 0)
  
      .. seealso:: :func:`~robodk.transl`, :func:`~robodk.rotx`, :func:`~robodk.roty`, :func:`~robodk.rotz`
      """
      if size == 4:
          return Mat([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
      else:
          newmat = Mat(size, size)
          for i in range(size):
              newmat[i, i] = 1
          return newmat
  
  
  def size(matrix, dim=None):
      """Returns the size of a matrix (m,n).
      Dim can be set to 0 to return m (rows) or 1 to return n (columns)
  
      :param matrix: pose
      :type matrix: :class:`.Mat`
      :param dim: dimension
      :type dim: int
      """
      return matrix.size(dim)
  
  
  def tr(matrix):
      """Returns the transpose of the matrix
  
      :param matrix: pose
      :type matrix: :class:`.Mat`"""
      return matrix.tr()
  
  
  def invH(matrix):
      """Returns the inverse of a homogeneous matrix
  
      :param matrix: pose
      :type matrix: :class:`.Mat`
  
      .. seealso:: :func:`~robodk.transl`, :func:`~robodk.rotx`, :func:`~robodk.roty`, :func:`~robodk.rotz`
      """
      return matrix.invH()
  
  
  def catV(mat1, mat2):
      """Concatenate 2 matrices (vertical concatenation)"""
      return mat1.catV(mat2)
  
  
  def catH(mat1, mat2):
      """Concatenate 2 matrices (horizontal concatenation)"""
      return mat1.catH(mat2)
  
  
  def tic():
      """Start a stopwatch timer"""
      import time
      global TICTOC_START_TIME
      TICTOC_START_TIME = time.time()
  
  
  def toc():
      """Read the stopwatch timer"""
      import time
      if 'TICTOC_START_TIME' in globals():
          elapsed = time.time() - TICTOC_START_TIME
          #print("Elapsed time is " + str(elapsed) + " seconds.")
          return elapsed
      else:
          print("Toc: start time not set")
          return -1
  
  
  def LoadList(strfile, separator=',', codec='utf-8'):
      """Load data from a CSV file or a TXT file to a Python list (list of list of numbers)
  
      .. seealso:: :func:`~robodk.SaveList`, :func:`~robodk.LoadMat`
  
      Example:
  
          .. code-block:: python
  
              csvdata = LoadList(strfile, ',')
              values = []
              for i in range(len(csvdata)):
                  print(csvdata[i])
                  values.append(csvdata[i])
  
              # We can also save the list back to a CSV file
              # SaveList(csvdata, strfile, ',')
  
      """
      def todecimal(value):
          try:
              return float(value)
          except:
              return value
  
      import csv
      import codecs
      # Read all CSV data:
      csvdata = []
      #with open(strfile) as csvfile:
      with codecs.open(strfile, "r", codec) as csvfile:
          csvread = csv.reader(csvfile, delimiter=separator, quotechar='|')
          for row in csvread:
              row_nums = [todecimal(i) for i in row]
              csvdata.append(row_nums)
      return csvdata
  
  
  def SaveList(list_variable, strfile, separator=','):
      """Save a list or a list of lists as a CSV or TXT file.
  
      .. seealso:: :func:`~robodk.LoadList`, :func:`~robodk.LoadMat`"""
  
      Mat(list_variable).tr().SaveMat(strfile, separator)
  
  
  def LoadMat(strfile, separator=','):
      """Load data from a CSV file or a TXT file to a :class:`.Mat` Matrix
  
      .. seealso:: :func:`~robodk.LoadList`
  
      """
      return Mat(LoadList(strfile, separator))
  
  
  #----------------------------------------------------
  #------ Pose to xyzrpw and xyzrpw to pose------------
  def Pose(x,y,z,r,p,w):
      """Create a pose from XYZRPW coordinates. The pose format is the one used by KUKA (XYZABC coordinates). This is function is the same as KUKA_2_Pose (with the difference that the input values are not a list). This function is used as "p" by the intermediate file when generating a robot program.
      
      .. seealso:: :func:`~robodk.KUKA_2_Pose`, :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      a = r*math.pi/180.0
      b = p*math.pi/180.0
      c = w*math.pi/180.0
      ca = math.cos(a)
      sa = math.sin(a)
      cb = math.cos(b)
      sb = math.sin(b)
      cc = math.cos(c)
      sc = math.sin(c)
      return Mat([[cb*ca,ca*sc*sb-cc*sa,sc*sa+cc*ca*sb,x],[cb*sa,cc*ca+sc*sb*sa,cc*sb*sa-ca*sc,y],[-sb,cb*sc,cc*cb,z],[0.0,0.0,0.0,1.0]])
      
  def pose_2_xyzrpw(H):
      """Calculates the equivalent position (mm) and Euler angles (deg) as an [x,y,z,r,p,w] array, given a pose.
      It returns the values that correspond to the following operation:
      transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180)
  
      :param H: pose
      :type H: :class:`.Mat`
      :return: [x,y,z,w,p,r] in mm and deg
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      x = H[0, 3]
      y = H[1, 3]
      z = H[2, 3]
      if (H[2, 0] > (1.0 - 1e-10)):
          p = -pi / 2
          r = 0
          w = math.atan2(-H[1, 2], H[1, 1])
      elif H[2, 0] < -1.0 + 1e-10:
          p = pi / 2
          r = 0
          w = math.atan2(H[1, 2], H[1, 1])
      else:
          p = math.atan2(-H[2, 0], sqrt(H[0, 0] * H[0, 0] + H[1, 0] * H[1, 0]))
          w = math.atan2(H[1, 0], H[0, 0])
          r = math.atan2(H[2, 1], H[2, 2])
      return [x, y, z, r * 180 / pi, p * 180 / pi, w * 180 / pi]
  
  
  def xyzrpw_2_pose(xyzrpw):
      """Calculates the pose from the position (mm) and Euler angles (deg), given a [x,y,z,r,p,w] array.
      The result is the same as calling: H = transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180)
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      [x, y, z, r, p, w] = xyzrpw
      a = r * pi / 180
      b = p * pi / 180
      c = w * pi / 180
      ca = math.cos(a)
      sa = math.sin(a)
      cb = math.cos(b)
      sb = math.sin(b)
      cc = math.cos(c)
      sc = math.sin(c)
      H = Mat([[cb * cc, cc * sa * sb - ca * sc, sa * sc + ca * cc * sb, x], [cb * sc, ca * cc + sa * sb * sc, ca * sb * sc - cc * sa, y], [-sb, cb * sa, ca * cb, z], [0, 0, 0, 1]])
      return H
  
  
  def Pose(tx, ty, tz, rx, ry, rz):
      """Returns the pose (:class:`.Mat`) given the position (mm) and Euler angles (deg) as an array [x,y,z,rx,ry,rz].
      The result is the same as calling: H = transl(x,y,z)*rotx(rx*pi/180)*roty(ry*pi/180)*rotz(rz*pi/180)
      This pose format is printed for homogeneous poses automatically. This Pose is the same representation used by Mecademic or Staubli robot controllers.
  
      :param float tx: position (X coordinate)
      :param float ty: position (Y coordinate)
      :param float tz: position (Z coordinate)
      :param float rx: first rotation (X coordinate)
      :param float ry: first rotation (Y coordinate)
      :param float rz: first rotation (Z coordinate)
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`
      """
      return TxyzRxyz_2_Pose([tx, ty, tz, rx * pi / 180, ry * pi / 180, rz * pi / 180])
  
  
  def TxyzRxyz_2_Pose(xyzrpw):
      """Returns the pose given the position (mm) and Euler angles (rad) as an array [x,y,z,rx,ry,rz].
      The result is the same as calling: H = transl(x,y,z)*rotx(rx)*roty(ry)*rotz(rz)
  
      :param xyzrpw: [x,y,z,rx,ry,rz] in mm and radians
      :type xyzrpw: list of float
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      [x, y, z, rx, ry, rz] = xyzrpw
      srx = math.sin(rx)
      crx = math.cos(rx)
      sry = math.sin(ry)
      cry = math.cos(ry)
      srz = math.sin(rz)
      crz = math.cos(rz)
      H = Mat([[cry * crz, -cry * srz, sry, x], [crx * srz + crz * srx * sry, crx * crz - srx * sry * srz, -cry * srx, y], [srx * srz - crx * crz * sry, crz * srx + crx * sry * srz, crx * cry, z], [0, 0, 0, 1]])
      return H
  
  
  def Pose_2_TxyzRxyz(H):
      """Retrieve the position (mm) and Euler angles (rad) as an array [x,y,z,rx,ry,rz] given a pose.
      It returns the values that correspond to the following operation:
      H = transl(x,y,z)*rotx(rx)*roty(ry)*rotz(rz).
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      x = H[0, 3]
      y = H[1, 3]
      z = H[2, 3]
      a = H[0, 0]
      b = H[0, 1]
      c = H[0, 2]
      d = H[1, 2]
      e = H[2, 2]
      if c > (1.0 - 1e-10):
          ry1 = pi / 2
          rx1 = 0
          rz1 = atan2(H[1, 0], H[1, 1])
      elif c < (-1.0 + 1e-10):
          ry1 = -pi / 2
          rx1 = 0
          rz1 = atan2(H[1, 0], H[1, 1])
      else:
          sy = c
          cy1 = +sqrt(1 - sy * sy)
          sx1 = -d / cy1
          cx1 = e / cy1
          sz1 = -b / cy1
          cz1 = a / cy1
          rx1 = atan2(sx1, cx1)
          ry1 = atan2(sy, cy1)
          rz1 = atan2(sz1, cz1)
      return [x, y, z, rx1, ry1, rz1]
  
  
  def Pose_2_Staubli(H):
      """Converts a pose (4x4 matrix) to a Staubli XYZWPR target
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      xyzwpr = Pose_2_TxyzRxyz(H)
      xyzwpr[3] = xyzwpr[3] * 180.0 / pi
      xyzwpr[4] = xyzwpr[4] * 180.0 / pi
      xyzwpr[5] = xyzwpr[5] * 180.0 / pi
      return xyzwpr
  
  
  def Pose_2_Motoman(H):
      """Converts a pose (4x4 matrix) to a Motoman XYZWPR target (mm and deg)
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      xyzwpr = pose_2_xyzrpw(H)
      return xyzwpr
  
  
  def Pose_2_Fanuc(H):
      """Converts a pose (4x4 matrix) to a Fanuc XYZWPR target (mm and deg)
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      xyzwpr = pose_2_xyzrpw(H)
      return xyzwpr
  
  
  def Pose_2_Techman(H):
      """Converts a pose (4x4 matrix) to a Techman XYZWPR target (mm and deg)
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      xyzwpr = pose_2_xyzrpw(H)
      return xyzwpr
  
  
  def Motoman_2_Pose(xyzwpr):
      """Converts a Motoman target to a pose (4x4 matrix)
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      return xyzrpw_2_pose(xyzwpr)
  
  
  def Fanuc_2_Pose(xyzwpr):
      """Converts a Motoman target to a pose (4x4 matrix)
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      return xyzrpw_2_pose(xyzwpr)
  
  
  def Techman_2_Pose(xyzwpr):
      """Converts a Techman target to a pose (4x4 matrix)
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      return xyzrpw_2_pose(xyzwpr)
  
  
  def Pose_2_KUKA(H):
      """Converts a pose (4x4 matrix) to an XYZABC KUKA target (Euler angles), required by KUKA KRC controllers.
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      x = H[0, 3]
      y = H[1, 3]
      z = H[2, 3]
      if (H[2, 0]) > (1.0 - 1e-10):
          p = -pi / 2
          r = 0
          w = atan2(-H[1, 2], H[1, 1])
      elif (H[2, 0]) < (-1.0 + 1e-10):
          p = pi / 2
          r = 0
          w = atan2(H[1, 2], H[1, 1])
      else:
          p = atan2(-H[2, 0], sqrt(H[0, 0] * H[0, 0] + H[1, 0] * H[1, 0]))
          w = atan2(H[1, 0], H[0, 0])
          r = atan2(H[2, 1], H[2, 2])
      return [x, y, z, w * 180 / pi, p * 180 / pi, r * 180 / pi]
  
  
  def KUKA_2_Pose(xyzrpw):
      """Converts a KUKA XYZABC target to a pose (4x4 matrix), required by KUKA KRC controllers.
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      [x, y, z, r, p, w] = xyzrpw
      a = r * math.pi / 180.0
      b = p * math.pi / 180.0
      c = w * math.pi / 180.0
      ca = math.cos(a)
      sa = math.sin(a)
      cb = math.cos(b)
      sb = math.sin(b)
      cc = math.cos(c)
      sc = math.sin(c)
      return Mat([[cb * ca, ca * sc * sb - cc * sa, sc * sa + cc * ca * sb, x], [cb * sa, cc * ca + sc * sb * sa, cc * sb * sa - ca * sc, y], [-sb, cb * sc, cc * cb, z], [0.0, 0.0, 0.0, 1.0]])
  
  
  def Adept_2_Pose(xyzrpw):
      """Converts an Adept XYZRPW target to a pose (4x4 matrix)
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      [x, y, z, r, p, w] = xyzrpw
      a = r * math.pi / 180.0
      b = p * math.pi / 180.0
      c = w * math.pi / 180.0
      ca = math.cos(a)
      sa = math.sin(a)
      cb = math.cos(b)
      sb = math.sin(b)
      cc = math.cos(c)
      sc = math.sin(c)
      return Mat([[ca * cb * cc - sa * sc, -cc * sa - ca * cb * sc, ca * sb, x], [ca * sc + cb * cc * sa, ca * cc - cb * sa * sc, sa * sb, y], [-cc * sb, sb * sc, cb, z], [0.0, 0.0, 0.0, 1.0]])
  
  
  def Pose_2_Adept(H):
      """Converts a pose to an Adept target
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      x = H[0, 3]
      y = H[1, 3]
      z = H[2, 3]
      if H[2, 2] > (1.0 - 1e-10):
          r = 0
          p = 0
          w = atan2(H[1, 0], H[0, 0])
      elif H[2, 2] < (-1.0 + 1e-10):
          r = 0
          p = pi
          w = atan2(H[1, 0], H[1, 1])
      else:
          cb = H[2, 2]
          sb = +sqrt(1 - cb * cb)
          sc = H[2, 1] / sb
          cc = -H[2, 0] / sb
          sa = H[1, 2] / sb
          ca = H[0, 2] / sb
          r = atan2(sa, ca)
          p = atan2(sb, cb)
          w = atan2(sc, cc)
      return [x, y, z, r * 180 / pi, p * 180 / pi, w * 180 / pi]
  
  
  def Comau_2_Pose(xyzrpw):
      """Converts a Comau XYZRPW target to a pose (4x4 matrix), the same representation required by PDL Comau programs.
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      return Adept_2_Pose(xyzrpw)
  
  
  def Pose_2_Comau(H):
      """Converts a pose to a Comau target, the same representation required by PDL Comau programs.
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`"""
      return Pose_2_Adept(H)
  
  
  def Pose_2_Nachi(pose):
      """Converts a pose to a Nachi XYZRPW target
  
      :param pose: pose
      :type pose: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      [x, y, z, r, p, w] = pose_2_xyzrpw(pose)
      return [x, y, z, w, p, r]
  
  
  def Nachi_2_Pose(xyzwpr):
      """Converts a Nachi XYZRPW target to a pose (4x4 matrix)
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      return xyzrpw_2_pose(xyzwpr)
  
  
  def pose_2_quaternion(Ti):
      """Returns the quaternion orientation vector of a pose (4x4 matrix)
  
      :param Ti: pose
      :type Ti: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      a = (Ti[0, 0])
      b = (Ti[1, 1])
      c = (Ti[2, 2])
      sign2 = 1
      sign3 = 1
      sign4 = 1
      if (Ti[2, 1] - Ti[1, 2]) < 0:
          sign2 = -1
      if (Ti[0, 2] - Ti[2, 0]) < 0:
          sign3 = -1
      if (Ti[1, 0] - Ti[0, 1]) < 0:
          sign4 = -1
      q1 = sqrt(max(a + b + c + 1, 0)) / 2
      q2 = sign2 * sqrt(max(a - b - c + 1, 0)) / 2
      q3 = sign3 * sqrt(max(-a + b - c + 1, 0)) / 2
      q4 = sign4 * sqrt(max(-a - b + c + 1, 0)) / 2
      return [q1, q2, q3, q4]
  
  
  def Pose_Split(pose1, pose2, delta_mm=1.0):
      """Create a sequence of poses that transitions from pose1 to pose2 by steps of delta_mm in mm (the first and last pose are not included in the list)"""
      pose_delta = invH(pose1) * pose2
      distance = norm(pose_delta.Pos())
      if distance <= delta_mm:
          return [pose2]
  
      pose_list = []
  
      x, y, z, w, p, r = Pose_2_UR(pose_delta)
  
      steps = max(1, int(distance / delta_mm))
  
      xd = x / steps
      yd = y / steps
      zd = z / steps
      wd = w / steps
      pd = p / steps
      rd = r / steps
      for i in range(steps - 1):
          factor = i + 1
          pose_list.append(pose1 * UR_2_Pose([xd * factor, yd * factor, zd * factor, wd * factor, pd * factor, rd * factor]))
  
      return pose_list
  
  
  def quaternion_2_pose(qin):
      """Returns the pose orientation matrix (4x4 matrix) given a quaternion orientation vector
  
      :param list qin: quaternions as 4 float values
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      qnorm = sqrt(qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3])
      q = qin
      q[0] = q[0] / qnorm
      q[1] = q[1] / qnorm
      q[2] = q[2] / qnorm
      q[3] = q[3] / qnorm
      pose = Mat([[1 - 2*q[2]*q[2] - 2*q[3]*q[3], 2*q[1]*q[2] - 2*q[3]*q[0],     2*q[1]*q[3] + 2*q[2]*q[0],     0],
                  [2*q[1]*q[2] + 2*q[3]*q[0],     1 - 2*q[1]*q[1] - 2*q[3]*q[3], 2*q[2]*q[3] - 2*q[1]*q[0],     0],
                  [2*q[1]*q[3] - 2*q[2]*q[0],     2*q[2]*q[3] + 2*q[1]*q[0],     1 - 2*q[1]*q[1] - 2*q[2]*q[2], 0],
                  [0,                             0,                             0,                             1]])
      return pose
  
  
  def Pose_2_ABB(H):
      """Converts a pose to an ABB target (using quaternion representation).
  
      :param H: pose
      :type H: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      q = pose_2_quaternion(H)
      return [H[0, 3], H[1, 3], H[2, 3], q[0], q[1], q[2], q[3]]
  
  
  def print_pose_ABB(pose):
      """Displays an ABB RAPID target (the same way it is displayed in IRC5 controllers).
  
      :param pose: pose
      :type pose: :class:`.Mat`
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      q = pose_2_quaternion(pose)
      print('[[%.3f,%.3f,%.3f],[%.6f,%.6f,%.6f,%.6f]]' % (pose[0, 3], pose[1, 3], pose[2, 3], q[0], q[1], q[2], q[3]))
  
  
  def Pose_2_UR(pose):
      """Calculate the p[x,y,z,u,v,w] position with rotation vector for a pose target. This is the same format required by Universal Robot controllers.
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
      """
      NUMERIC_TOLERANCE = 1e-8
  
      def saturate_1(value):
          return min(max(value, -1.0), 1.0)
  
      angle = acos(saturate_1((pose[0, 0] + pose[1, 1] + pose[2, 2] - 1) * 0.5))
      rxyz = [pose[2, 1] - pose[1, 2], pose[0, 2] - pose[2, 0], pose[1, 0] - pose[0, 1]]
      if angle < NUMERIC_TOLERANCE:
          rxyz = [0, 0, 0]
      else:
          sin_angle = sin(angle)
          if abs(sin_angle) < NUMERIC_TOLERANCE or norm(rxyz) < NUMERIC_TOLERANCE:
              d3 = [pose[0, 0], pose[1, 1], pose[2, 2]]
              mx = max(d3)
              mx_id = d3.index(mx)
              if mx_id == 0:
                  rxyz = [pose[0, 0] + 1, pose[1, 0], pose[2, 0]]
              elif mx_id == 1:
                  rxyz = [pose[0, 1], pose[1, 1] + 1, pose[2, 1]]
              else:
                  rxyz = [pose[0, 2], pose[1, 2], pose[2, 2] + 1]
  
              rxyz = mult3(rxyz, angle / (sqrt(max(0, 2 * (1 + mx)))))
          else:
              rxyz = normalize3(rxyz)
              rxyz = mult3(rxyz, angle)
      return [pose[0, 3], pose[1, 3], pose[2, 3], rxyz[0], rxyz[1], rxyz[2]]
  
  
  def UR_2_Pose(xyzwpr):
      """Calculate the pose target given a p[x,y,z,u,v,w] cartesian target with rotation vector. This is the same format required by Universal Robot controllers.
  
      .. seealso:: :class:`.Mat`, :func:`~robodk.TxyzRxyz_2_Pose`
      """
      x, y, z, w, p, r = xyzwpr
      wpr = [w, p, r]
      angle = norm(wpr)
      cosang = cos(0.5 * angle)
  
      if angle == 0.0:
          q234 = [0.0, 0.0, 0.0]
      else:
          ratio = sin(0.5 * angle) / angle
          q234 = mult3(wpr, ratio)
  
      q1234 = [cosang, q234[0], q234[1], q234[2]]
      pose = quaternion_2_pose(q1234)
      pose.setPos([x, y, z])
      return pose
  
  
  #----------------------------------------------------
  #-------- ROBOT MODEL (D-H and D-H M) ---------------
  
  
  def dh(rz, tx=None, tz=None, rx=None):
      """Returns the Denavit-Hartenberg 4x4 matrix for a robot link.
      calling dh(rz,tx,tz,rx) is the same as using rotz(rz)*transl(tx,0,tz)*rotx(rx)
      calling dh(rz,tx,tz,rx) is the same as calling dh([rz,tx,tz,rx])
      """
      if tx is None: [rz, tx, tz, rx] = rz
  
      crx = math.cos(rx)
      srx = math.sin(rx)
      crz = math.cos(rz)
      srz = math.sin(rz)
      return Mat( [[crz, -srz*crx,  srz*srx, tx*crz],
                   [srz,  crz*crx, -crz*srx, tx*srz],
                   [  0,      srx,      crx,     tz],
                   [  0,        0,        0,      1]])
  
  def dhm(rx, tx=None, tz=None, rz=None):
      """Returns the Denavit-Hartenberg Modified 4x4 matrix for a robot link (Craig 1986).
  
      calling dhm(rx,tx,tz,rz) is the same as using rotx(rx)*transl(tx,0,tz)*rotz(rz)
  
      calling dhm(rx,tx,tz,rz) is the same as calling dhm([rx,tx,tz,rz])
      """
      if tx is None: [rx, tx, tz, rz] = rx
  
      crx = math.cos(rx)
      srx = math.sin(rx)
      crz = math.cos(rz)
      srz = math.sin(rz)
      return Mat([[crz,        -srz,    0,      tx],
                  [crx*srz, crx*crz, -srx, -tz*srx],
                  [srx*srz, crz*srx,  crx,  tz*crx],
                  [      0,       0,    0,       1]])
  
  def joints_2_angles(jin, type):
      """Converts the robot encoders into angles between links depending on the type of the robot."""
      jout = jin
      if type == 2:
          jout[2] = -jin[1] - jin[2]
          jout[3] = -jin[3]
          jout[4] = -jin[4]
          jout[5] = -jin[5]
      elif type == 3:
          jout[2] = -jin[2]
          jout[3] = -jin[3]
          jout[4] = -jin[4]
          jout[5] = -jin[5]
      elif type == 4:
          jout[2] = +jin[1] + jin[2]
      elif type == 11:
          jout[2] = -jin[1] - jin[2]
          jout[0] = -jin[0]
          jout[3] = -jin[3]
          jout[5] = -jin[5]
      return jout
  
  
  def angles_2_joints(jin, type):
      """Converts the angles between links into the robot motor space depending on the type of the robot."""
      jout = jin
      if type == 2:
          jout[2] = -jin[1] - jin[2]
          jout[3] = -jin[3]
          jout[4] = -jin[4]
          jout[5] = -jin[5]
      elif type == 3:
          jout[2] = -jin[2]
          jout[3] = -jin[3]
          jout[4] = -jin[4]
          jout[5] = -jin[5]
      elif type == 11:
          jout[2] = -jin[1] - jin[2]
          jout[0] = -jin[0]
          jout[3] = -jin[3]
          jout[5] = -jin[5]
      return jout
  
  
  #----------------------------------------------------
  #-------- Useful geometric tools ---------------
  
  
  def norm(p):
      """Returns the norm of a 3D vector"""
      return sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2])
  
  
  def normalize3(a):
      """Returns the unitary vector"""
      norminv = 1.0 / norm(a)
      return [a[0] * norminv, a[1] * norminv, a[2] * norminv]
  
  
  def cross(a, b):
      """Returns the cross product of two 3D vectors"""
      c = [a[1]*b[2] - a[2]*b[1],
           a[2]*b[0] - a[0]*b[2],
           a[0]*b[1] - a[1]*b[0]]
      return c
  
  
  def dot(a, b):
      """Returns the dot product of two 3D vectors"""
      return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
  
  
  def angle3(a, b):
      """Returns the angle in radians of two 3D vectors"""
      cos_angle = dot(normalize3(a), normalize3(b))
      cos_angle = min(1.0, max(-1.0, cos_angle))
      return acos(cos_angle)
  
  
  def pose_angle(pose):
      """Returns the angle in radians of a 4x4 matrix pose
  
      :param pose: pose
      :type pose: :class:`.Mat`"""
      cos_ang = (pose[0, 0] + pose[1, 1] + pose[2, 2] - 1) / 2
      cos_ang = min(max(cos_ang, -1), 1)
      return acos(cos_ang)
  
  
  def pose_angle_between(pose1, pose2):
      """Returns the angle in radians between two poses (4x4 matrix pose)"""
      return pose_angle(invH(pose1) * pose2)
  
  
  def mult3(v, d):
      """Multiplies a 3D vector to a scalar"""
      return [v[0] * d, v[1] * d, v[2] * d]
  
  
  def subs3(a, b):
      """Subtracts two 3D vectors c=a-b"""
      return [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
  
  
  def add3(a, b):
      """Adds two 3D vectors c=a+b"""
      return [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
  
  
  def distance(a, b):
      """Calculates the distance between two points"""
      return norm(subs3(a, b))
  
  
  def pose_is_similar(a, b, tolerance=0.1):
      """Check if the pose is similar. Returns True if both poses are less than 0.1 mm or 0.1 deg appart. Optionally provide the tolerance in mm+deg"""
      if distance(a.Pos(), b.Pos()) + pose_angle_between(a, b) * 180 / pi < tolerance:
          return True
      return False
  
  
  def intersect_line_2_plane(pline, vline, pplane, vplane):
      """Calculates the intersection betweeen a line and a plane"""
      D = -dot(vplane, pplane)
      k = -(D + dot(vplane, pline)) / dot(vplane, vline)
      p = add3(pline, mult3(vline, k))
      return p
  
  
  def proj_pt_2_plane(point, planepoint, planeABC):
      """Projects a point to a plane"""
      return intersect_line_2_plane(point, planeABC, planepoint, planeABC)
  
  
  def proj_pt_2_line(point, paxe, vaxe):
      """Projects a point to a line"""
      vpaxe2point = subs3(point, paxe)
      dist = dot(vaxe, vpaxe2point) / dot(vaxe, vaxe)
      return add3(paxe, mult3(vaxe, dist))
  
  
  def fitPlane(points):
      """Best fits a plane to a cloud of points"""
      import numpy as np
      XYZ = np.array(points)
      [rows, cols] = XYZ.shape
      # Set up constraint equations of the form  AB = 0,
      # where B is a column vector of the plane coefficients
      # in the form b(1)*X + b(2)*Y +b(3)*Z + b(4) = 0.
      p = (np.ones((rows, 1)))
      AB = np.hstack([XYZ, p])
      [u, d, v] = np.linalg.svd(AB, 0)
      B = v[3, :]  # Solution is last column of v.
      nn = np.linalg.norm(B[0:3])
      B = B / nn
      pplane = [0, 0, -(B[3] / B[2])]
      vplane = B[0:3].tolist()
      return pplane, vplane
  
  
  #----------------------------------------------------
  #--------       Mat matrix class      ---------------
  
  
  class MatrixError(Exception):
      """ An exception class for Matrix """
      pass
  
  
  class Mat(object):
      """Mat is a matrix object. The main purpose of this object is to represent a pose in the 3D space (position and orientation).
  
      A pose is a 4x4 matrix that represents the position and orientation of one reference frame with respect to another one, in the 3D space.
  
      Poses are commonly used in robotics to place objects, reference frames and targets with respect to each other.
  
      .. seealso:: :func:`~robodk.TxyzRxyz_2_Pose`, :func:`~robodk.Pose_2_TxyzRxyz`, :func:`~robodk.Pose_2_ABB`, :func:`~robodk.Pose_2_Adept`, :func:`~robodk.Adept_2_Pose`, :func:`~robodk.Pose_2_Comau`, :func:`~robodk.Pose_2_Fanuc`, :func:`~robodk.Pose_2_KUKA`, :func:`~robodk.KUKA_2_Pose`, :func:`~robodk.Pose_2_Motoman`, :func:`~robodk.Pose_2_Nachi`, :func:`~robodk.Pose_2_Staubli`, :func:`~robodk.Pose_2_UR`, :func:`~robodk.quaternion_2_pose`
  
      Example:
  
          .. code-block:: python
  
              from robolink import *                  # import the robolink library
              from robodk import *                    # import the robodk library
  
              RDK = Robolink()                        # connect to the RoboDK API
              robot  = RDK.Item('', ITEM_TYPE_ROBOT)  # Retrieve a robot available in RoboDK
              #target  = RDK.Item('Target 1')         # Retrieve a target (example)
  
  
              pose = robot.Pose()                     # retrieve the current robot position as a pose (position of the active tool with respect to the active reference frame)
              # target = target.Pose()                # the same can be applied to targets (taught position)
  
              # Read the 4x4 pose matrix as [X,Y,Z , A,B,C] Euler representation (mm and deg): same representation as KUKA robots
              XYZABC = Pose_2_KUKA(pose)
              print(XYZABC)
  
              # Read the 4x4 pose matrix as [X,Y,Z, q1,q2,q3,q4] quaternion representation (position in mm and orientation in quaternion): same representation as ABB robots (RAPID programming)
              xyzq1234 = Pose_2_ABB(pose)
              print(xyzq1234)
  
              # Read the 4x4 pose matrix as [X,Y,Z, u,v,w] representation (position in mm and orientation vector in radians): same representation as Universal Robots
              xyzuvw = Pose_2_UR(pose)
              print(xyzuvw)
  
              x,y,z,a,b,c = XYZABC                    # Use the KUKA representation (for example) and calculate a new pose based on the previous pose
              XYZABC2 = [x,y,z+50,a,b,c+45]
              pose2 = KUKA_2_Pose(XYZABC2)            # Convert the XYZABC array to a pose (4x4 matrix)
  
              robot.MoveJ(pose2)                      # Make a joint move to the new position
              # target.setPose(pose2)                  # We can also update the pose to targets, tools, reference frames, objects, ...
      """
      def __init__(self, rows=None, ncols=None):
          if ncols is None:
              if rows is None:
                  m = 4
                  n = 4
                  self.rows = [[0] * n for x in range(m)]
              else:
                  if isinstance(rows, Mat):
                      rows = rows.copy().rows
                  m = len(rows)
                  transpose = 0
                  if isinstance(rows, list) and len(rows) == 0:
                      # Check empty matrix
                      self.rows = [[]]
                      n = 0
                      return
  
                  if not isinstance(rows[0], list):
                      rows = [rows]
                      transpose = 1
  
                  n = len(rows[0])
                  if any([len(row) != n for row in rows[1:]]):  # Validity check
                      #raise Exception(MatrixError, "inconsistent row length")
                      # make the size uniform (fill blanks with zeros)
                      n = max([len(i) for i in rows])
                      for row in rows:
                          row += [0] * (n - len(row))
  
                  self.rows = rows
                  if transpose:
                      self.rows = [list(item) for item in zip(*self.rows)]
          else:
              m = max(rows, 0)
              n = max(ncols, 0)
              if m == 0:
                  m = 1
                  n = 0
  
              self.rows = [[0] * n for x in range(m)]
  
      def __iter__(self):
          if self.size(0) == 0 or self.size(1) == 0:
              return iter([])
          return iter(self.tr().rows)
  
      def copy(self):
          sz = self.size()
          newmat = Mat(sz[0], sz[1])
          for i in range(sz[0]):
              for j in range(sz[1]):
                  newmat[i, j] = self[i, j]
          return newmat
  
      def __len__(self):
          """Return the number of columns"""
          return len(self.rows[0])
  
      def ColsCount(self):
          """Return the number of coumns. Same as len().
  
          .. seealso:: :func:`~Mat.Cols`, :func:`~Mat.Rows`, :func:`~Mat.RowsCount`
          """
          return len(self.rows[0])
  
      def RowsCount(self):
          """Return the number of rows
  
          .. seealso:: :func:`~Mat.Cols`, :func:`~Mat.Rows`, :func:`~Mat.ColsCount`
  
          """
          return len(self.rows[0])
  
      def Cols(self):
          """Retrieve the matrix as a list of columns (list of list of float).
  
          .. seealso:: :func:`~Mat.Rows`, :func:`~Mat.ColsCount`, :func:`~Mat.RowsCount`
  
          Example:
  
              .. code-block:: python
  
                  >>> transl(10,20,30).Rows()
                  [[1, 0, 0, 10], [0, 1, 0, 20], [0, 0, 1, 30], [0, 0, 0, 1]]
  
                  >>> transl(10,20,30).Cols()
                  [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [10, 20, 30, 1]]
          """
          return self.tr().rows
  
      def Rows(self):
          """Get the matrix as a list of lists
  
          .. seealso:: :func:`~Mat.Cols`, :func:`~Mat.ColsCount`, :func:`~Mat.RowsCount`
          """
          return self.rows
  
      def __getitem__(self, idx):
          if isinstance(idx, int):  #integer A[1]
              return tr(Mat(self.rows[idx]))
          elif isinstance(idx, slice):  #one slice: A[1:3]
              return Mat(self.rows[idx])
          else:  #two slices: A[1:3,1:3]
              idx1 = idx[0]
              idx2 = idx[1]
              if isinstance(idx1, int) and isinstance(idx2, int):
                  return self.rows[idx1][idx2]
              matsize = self.size()
              if isinstance(idx1, slice):
                  indices1 = idx1.indices(matsize[0])
                  rg1 = range(*indices1)
              else:  #is int
                  rg1 = range(idx1, idx1 + 1)
              if isinstance(idx2, slice):
                  indices2 = idx2.indices(matsize[1])
                  rg2 = range(*indices2)
              else:  #is int
                  rg2 = range(idx2, idx2 + 1)
              #newm = int(abs((rg1.stop-rg1.start)/rg1.step))
              #newn = int(abs((rg2.stop-rg2.start)/rg2.step))
              newm = rg1
              newn = rg2
              newmat = Mat(len(newm), len(newn))
              cm = 0
              for i in rg1:
                  cn = 0
                  for j in rg2:
                      newmat.rows[cm][cn] = self.rows[i][j]
                      cn = cn + 1
                  cm = cm + 1
              return newmat
  
      def __setitem__(self, idx, item):
          if isinstance(item, float) or isinstance(item, int):
              item = Mat([[item]])
          elif isinstance(item, list):
              item = Mat(item)
  
          matsize = self.size()
          if isinstance(idx, int):  #integer A[1]
              idx1 = idx
              idx2 = 0
              #raise Exception(MatrixError, "Cannot set item. Use [i,:] instead.")
              #self.rows[idx] = item
          elif isinstance(idx, slice):  #one slice: A[1:3]
              # raise Exception(MatrixError, "Cannot set item. Use [a:b,:] instead.")
              idx1 = idx
              idx2 = 0
          else:
              idx1 = idx[0]
              idx2 = idx[1]
  
          # at this point we have two slices: example A[1:3,1:3]
          if isinstance(idx1, slice):
              indices1 = idx1.indices(matsize[0])
              rg1 = range(*indices1)
          else:  #is int
              rg1 = range(idx1, idx1 + 1)
          if isinstance(idx2, slice):
              indices2 = idx2.indices(matsize[1])
              rg2 = range(*indices2)
          else:  #is int
              rg2 = range(idx2, idx2 + 1)
          #newm = int(abs((rg1.stop-rg1.start)/rg1.step))
          #newn = int(abs((rg2.stop-rg2.start)/rg2.step))
          newm = rg1
          newn = rg2
          itmsz = item.size()
          if len(newm) != itmsz[0] or len(newn) != itmsz[1]:
              raise Exception(MatrixError, "Submatrix indices does not match the new matrix sizes", itmsz[0], "x", itmsz[1], "<-", newm, "x", newn)
          #newmat = Mat(newm,newn)
          cm = 0
          for i in rg1:
              cn = 0
              for j in rg2:
                  self.rows[i][j] = item.rows[cm][cn]
                  cn = cn + 1
              cm = cm + 1
  
      def __str__(self):
          #s='\n [ '.join([(', '.join([str(item) for item in row])+' ],') for row in self.rows])
          str_add = ''
          if self.isHomogeneous():
              x, y, z, rx, ry, rz = Pose_2_TxyzRxyz(self)
              str_add = 'Pose(%.3f, %.3f, %.3f,  %.3f, %.3f, %.3f):\n' % (x, y, z, rx * 180 / pi, ry * 180 / pi, rz * 180 / pi)
  
          s = '\n [ '.join([(', '.join([('%.3f' % item if type(item) == float else str(item)) for item in row]) + ' ],') for row in self.rows])
          return str_add + '[[ ' + s[:-1] + ']\n'
  
      def __repr__(self):
          s = str(self)
          rank = str(self.size())
          rep = "Matrix: %s\n%s" % (rank, s)
          return rep
  
      def tr(self):
          """Returns the transpose of the matrix"""
          if self.size(0) == 0 or self.size(1) == 0:
              return Mat(0, 0)
          mat = Mat([list(item) for item in zip(*self.rows)])
          return mat
  
      def size(self, dim=None):
          """Returns the size of a matrix (m,n).
          Dim can be set to 0 to return m (rows) or 1 to return n (columns)"""
          m = len(self.rows)
          if m > 0:
              n = len(self.rows[0])
          else:
              n = 0
  
          if dim is None:
              return (m, n)
          elif dim == 0:
              return m
          elif dim == 1:
              return n
          else:
              raise Exception(MatrixError, "Invalid dimension!")
  
      def catV(self, mat2):
          """Concatenate with another matrix (vertical concatenation)"""
          if not isinstance(mat2, Mat):
              #raise Exception(MatrixError, "Concatenation must be performed with 2 matrices")
              return self.catH(Mat(mat2).tr())
  
          sz1 = self.size()
          sz2 = mat2.size()
          if sz1[1] != sz2[1]:
              raise Exception(MatrixError, "Horizontal size of matrices does not match")
  
          newmat = Mat(sz1[0] + sz2[0], sz1[1])
          newmat[0:sz1[0], :] = self
          newmat[sz1[0]:, :] = mat2
          return newmat
  
      def catH(self, mat2):
          """Concatenate with another matrix (horizontal concatenation)"""
          if not isinstance(mat2, Mat):
              #raise Exception(MatrixError, "Concatenation must be performed with 2 matrices")
              return self.catH(Mat(mat2))
  
          sz1 = self.size()
          sz2 = mat2.size()
          if sz1[0] != sz2[0]:
              raise Exception(MatrixError, "Horizontal size of matrices does not match")
  
          newmat = Mat(sz1[0], sz1[1] + sz2[1])
          newmat[:, :sz1[1]] = self
          newmat[:, sz1[1]:] = mat2
          return newmat
  
      def __eq__(self, other):
          """Test equality"""
          if other is None:
              return False
          #return (other.rows == self.rows)
          return pose_is_similar(other, self)
  
      def __ne__(self, other):
          return not (self == other)
  
      def __add__(self, mat):
          """Add a matrix to this matrix and
          return the new matrix. It doesn't modify
          the current matrix"""
          if isinstance(mat, int) or isinstance(mat, float):
              m, n = self.size()
              result = Mat(m, n)
              for x in range(m):
                  for y in range(n):
                      result.rows[x][y] = self.rows[x][y] + mat
              return result
          sz = self.size()
          m = sz[0]
          n = sz[1]
          ret = Mat(m, n)
          if sz != mat.size():
              raise Exception(MatrixError, "Can not add matrices of sifferent sizes!")
          for x in range(m):
              row = [sum(item) for item in zip(self.rows[x], mat.rows[x])]
              ret.rows[x] = row
          return ret
  
      def __sub__(self, mat):
          """Subtract a matrix from this matrix and
          return the new matrix. It doesn't modify
          the current matrix"""
          if isinstance(mat, int) or isinstance(mat, float):
              m, n = self.size()
              result = Mat(m, n)
              for x in range(m):
                  for y in range(n):
                      result.rows[x][y] = self.rows[x][y] - mat
              return result
          sz = self.size()
          m = sz[0]
          n = sz[1]
          ret = Mat(m, n)
          if sz != mat.size():
              raise Exception(MatrixError, "Can not subtract matrices of sifferent sizes!")
          for x in range(m):
              row = [item[0] - item[1] for item in zip(self.rows[x], mat.rows[x])]
              ret.rows[x] = row
          return ret
  
      def __mul__(self, mat):
          """Multiply a matrix with this matrix and
          return the new matrix. It doesn't modify
          the current matrix"""
          if isinstance(mat, int) or isinstance(mat, float):
              m, n = self.size()
              mulmat = Mat(m, n)
              for x in range(m):
                  for y in range(n):
                      mulmat.rows[x][y] = self.rows[x][y] * mat
              return mulmat
          if isinstance(mat, list):  #case of a matrix times a vector
              szvect = len(mat)
              m = self.size(0)
              matvect = Mat(mat)
              if szvect + 1 == m:
                  vectok = catV(matvect, Mat([[1]]))
                  result = self * vectok
                  return (result[:-1, :]).tr().rows[0]
              elif szvect == m:
                  result = self * Mat(matvect)
                  return result.tr().rows[0]
              else:
                  raise Exception(MatrixError, "Invalid product")
          else:
              matm, matn = mat.size()
              m, n = self.size()
              if (n != matm):
                  raise Exception(MatrixError, "Matrices cannot be multipled (unexpected size)!")
              mat_t = mat.tr()
              mulmat = Mat(m, matn)
              for x in range(m):
                  for y in range(mat_t.size(0)):
                      mulmat.rows[x][y] = sum([item[0] * item[1] for item in zip(self.rows[x], mat_t.rows[y])])
              return mulmat
  
      def eye(self, m=4):
          """Make identity matrix of size (mxm)"""
          rows = [[0] * m for x in range(m)]
          idx = 0
          for row in rows:
              row[idx] = 1
              idx += 1
          return Mat(rows)
  
      def isHomogeneous(self):
          """returns 1 if it is a Homogeneous matrix"""
          m, n = self.size()
          if m != 4 or n != 4:
              return False
          #if self[3,:] != Mat([[0.0,0.0,0.0,1.0]]):
          #    return False
          test = self[0:3, 0:3]
          test = test * test.tr()
          test[0, 0] = test[0, 0] - 1.0
          test[1, 1] = test[1, 1] - 1.0
          test[2, 2] = test[2, 2] - 1.0
          zero = 0.0
          for x in range(3):
              for y in range(3):
                  zero = zero + abs(test[x, y])
          if zero > 1e-4:
              return False
          return True
  
      def RelTool(self, x, y, z, rx=0, ry=0, rz=0):
          """Calculates a target relative with respect to the tool coordinates.
          X,Y,Z are in mm, W,P,R are in degrees. The behavior of this function is the same as ABB's RAPID RelTool command."""
          return RelTool(self, x, y, z, rx, ry, rz)
  
      def Offset(self, x, y, z, rx=0, ry=0, rz=0):
          """Calculates a relative target with respect to the reference frame coordinates.
          X,Y,Z are in mm, W,P,R are in degrees."""
          return Offset(self, x, y, z, rx, ry, rz)
  
      def invH(self):
          """Returns the inverse of this pose (homogeneous matrix assumed)"""
          if not self.isHomogeneous():
              raise Exception(MatrixError, "Pose matrix is not homogeneous. invH() can only compute the inverse of a homogeneous matrix")
          Hout = self.tr()
          Hout[3, 0:3] = Mat([[0, 0, 0]])
          Hout[0:3, 3] = (Hout[0:3, 0:3] * self[0:3, 3]) * (-1)
          return Hout
  
      def inv(self):
          """Returns the inverse of this pose (homogeneous matrix assumed)"""
          return self.invH()
  
      def tolist(self):
          """Returns the first column of the matrix as a list"""
          return tr(self).rows[0]
  
      def list(self):
          """Returns the first column of the matrix as a list"""
          return tr(self).rows[0]
  
      def list2(self):
          """Returns the matrix as list of lists (one list per column)"""
          return tr(self).rows
  
      def Pos(self):
          """Returns the position of a pose (assumes that a 4x4 homogeneous matrix is being used)"""
          return self[0:3, 3].tolist()
  
      def VX(self):
          """Returns the X vector of a pose (assumes that a 4x4 homogeneous matrix is being used)"""
          return self[0:3, 0].tolist()
  
      def VY(self):
          """Returns the Y vector of a pose (assumes that a 4x4 homogeneous matrix is being used)"""
          return self[0:3, 1].tolist()
  
      def VZ(self):
          """Returns the Z vector of a pose (assumes that a 4x4 homogeneous matrix is being used)"""
          return self[0:3, 2].tolist()
  
      def Rot33(self):
          """Returns the sub 3x3 rotation matrix"""
          return self[0:3, 0:3]
  
      def setPos(self, newpos):
          """Sets the XYZ position of a pose (assumes that a 4x4 homogeneous matrix is being used)"""
          self[0, 3] = newpos[0]
          self[1, 3] = newpos[1]
          self[2, 3] = newpos[2]
          return self
  
      def setVX(self, v_xyz):
          """Sets the VX vector of a pose, which is the first column of a homogeneous matrix (assumes that a 4x4 homogeneous matrix is being used)"""
          v_xyz = normalize3(v_xyz)
          self[0, 0] = v_xyz[0]
          self[1, 0] = v_xyz[1]
          self[2, 0] = v_xyz[2]
          return self
  
      def setVY(self, v_xyz):
          """Sets the VY vector of a pose, which is the first column of a homogeneous matrix (assumes that a 4x4 homogeneous matrix is being used)"""
          v_xyz = normalize3(v_xyz)
          self[0, 1] = v_xyz[0]
          self[1, 1] = v_xyz[1]
          self[2, 1] = v_xyz[2]
          return self
  
      def setVZ(self, v_xyz):
          """Sets the VZ vector of a pose, which is the first column of a homogeneous matrix (assumes that a 4x4 homogeneous matrix is being used)"""
          v_xyz = normalize3(v_xyz)
          self[0, 2] = v_xyz[0]
          self[1, 2] = v_xyz[1]
          self[2, 2] = v_xyz[2]
          return self
  
      def translationPose(self):
          """Return the translation pose of this matrix. The rotation returned is set to identity (assumes that a 4x4 homogeneous matrix is being used)"""
          return transl(self.Pos())
  
      def rotationPose(self):
          """Return the rotation pose of this matrix. The position returned is set to [0,0,0] (assumes that a 4x4 homogeneous matrix is being used)"""
          mat_rotation = Mat(self)
          mat_rotation.setPos([0, 0, 0])
          return mat_rotation
  
      def SaveCSV(self, strfile):
          """Save the :class:`.Mat` Matrix to a CSV (Comma Separated Values) file. The file can be easily opened as a spreadsheet such as Excel.
  
          .. seealso:: :func:`~Mat.SaveMat`, :func:`~robodk.SaveList`, :func:`~robodk.LoadList`, :func:`~robodk.LoadMat`
          """
          self.tr().SaveMat(strfile)
  
      def SaveMat(self, strfile, separator=','):
          """Save the :class:`.Mat` Matrix to a CSV or TXT file
  
          .. seealso:: :func:`~Mat.SaveCSV`, :func:`~robodk.SaveList`, :func:`~robodk.LoadList`, :func:`~robodk.LoadMat`
          """
          sz = self.size()
          m = sz[0]
          n = sz[1]
          file = open(strfile, 'w')
          for j in range(n):
              for i in range(m):
                  file.write(('%.6f' + separator) % self.rows[i][j])
              file.write('\n')
          file.close()
  
  
  #-------------------------------------------------------
  # FTP TRANSFER Tools
  def RemoveFileFTP(ftp, filepath):
      """Delete a file on a remote server."""
      import ftplib
      try:
          ftp.delete(filepath)
      except ftplib.all_errors as e:
          import sys
          print('POPUP: Could not remove file {0}: {1}'.format(filepath, e))
          sys.stdout.flush()
  
  
  def RemoveDirFTP(ftp, path):
      """Recursively delete a directory tree on a remote server."""
      import ftplib
      wd = ftp.pwd()
      try:
          names = ftp.nlst(path)
      except ftplib.all_errors as e:
          # some FTP servers complain when you try and list non-existent paths
          print('RemoveDirFTP: Could not remove folder {0}: {1}'.format(path, e))
          return
  
      for name in names:
          if os.path.split(name)[1] in ('.', '..'): continue
          print('RemoveDirFTP: Checking {0}'.format(name))
          try:
              ftp.cwd(path + '/' + name)  # if we can cwd to it, it's a folder
              ftp.cwd(wd)  # don't try a nuke a folder we're in
              RemoveDirFTP(ftp, path + '/' + name)
          except ftplib.all_errors:
              ftp.delete(path + '/' + name)
              #RemoveFileFTP(ftp, name)
  
      try:
          ftp.rmd(path)
      except ftplib.all_errors as e:
          print('RemoveDirFTP: Could not remove {0}: {1}'.format(path, e))
  
  
  def UploadDirFTP(localpath, server_ip, remote_path, username, password):
      """Upload a folder to a robot through FTP recursively"""
      import ftplib
      import os
      import sys
      main_folder = os.path.basename(os.path.normpath(localpath))
      print("POPUP: <p>Connecting to <strong>%s</strong> using user name <strong>%s</strong> and password ****</p><p>Please wait...</p>" % (server_ip, username))
      sys.stdout.flush()
      try:
          myFTP = ftplib.FTP(server_ip, username, password)
          print('Connection established')
      except:
          error_str = sys.exc_info()[1]
          print("POPUP: <font color=\"red\">Connection to %s failed: <p>%s</p></font>" % (server_ip, error_str))
          sys.stdout.flush()
          pause(4)
          return False
  
      remote_path_prog = remote_path + '/' + main_folder
      myPath = r'%s' % localpath
      print("POPUP: Connected. Deleting existing files on %s..." % remote_path_prog)
      sys.stdout.flush()
      RemoveDirFTP(myFTP, remote_path_prog)
      print("POPUP: Connected. Uploading program to %s..." % server_ip)
      sys.stdout.flush()
      try:
          myFTP.cwd(remote_path)
          myFTP.mkd(main_folder)
          myFTP.cwd(remote_path_prog)
      except:
          error_str = sys.exc_info()[1]
          print("POPUP: <font color=\"red\">Remote path not found or can't be created: %s</font>" % (remote_path))
          sys.stdout.flush()
          pause(4)
          #contin = mbox("Remote path\n%s\nnot found or can't create folder.\n\nChange path and permissions and retry." % remote_path)
          return False
  
      def uploadThis(path):
          files = os.listdir(path)
          os.chdir(path)
          for f in files:
              if os.path.isfile(path + r'\{}'.format(f)):
                  print('  Sending file: %s' % f)
                  print("POPUP: Sending file: %s" % f)
                  sys.stdout.flush()
                  fh = open(f, 'rb')
                  myFTP.storbinary('STOR %s' % f, fh)
                  fh.close()
              elif os.path.isdir(path + r'\{}'.format(f)):
                  print('  Sending folder: %s' % f)
                  myFTP.mkd(f)
                  myFTP.cwd(f)
                  uploadThis(path + r'\{}'.format(f))
          myFTP.cwd('..')
          os.chdir('..')
  
      uploadThis(myPath)  # now call the recursive function
      myFTP.close()
      print("POPUP: Folder trasfer completed: <font color=\"blue\">%s</font>" % remote_path)
      sys.stdout.flush()
      return True
  
  
  def UploadFileFTP(file_path_name, server_ip, remote_path, username, password):
      """Upload a file to a robot through FTP"""
      filepath = getFileDir(file_path_name)
      filename = getBaseName(file_path_name)
      import ftplib
      import os
      import sys
      print("POPUP: <p>Connecting to <strong>%s</strong> using user name <strong>%s</strong> and password ****</p><p>Please wait...</p>" % (server_ip, username))
      sys.stdout.flush()
      try:
          myFTP = ftplib.FTP(server_ip, username, password)
      except:
          error_str = sys.exc_info()[1]
          print("POPUP: <font color=\"red\">Connection to %s failed: <p>%s</p></font>" % (server_ip, error_str))
          sys.stdout.flush()
          pause(4)
          return False
  
      remote_path_prog = remote_path + '/' + filename
      print("POPUP: Connected. Deleting remote file %s..." % remote_path_prog)
      sys.stdout.flush()
      RemoveFileFTP(myFTP, remote_path_prog)
      print("POPUP: Connected. Uploading program to %s..." % server_ip)
      sys.stdout.flush()
      try:
          myFTP.cwd(remote_path)
      except:
          error_str = sys.exc_info()[1]
          print("POPUP: <font color=\"red\">Remote path not found or can't be created: %s</font>" % (remote_path))
          sys.stdout.flush()
          pause(4)
          #contin = mbox("Remote path\n%s\nnot found or can't create folder.\n\nChange path and permissions and retry." % remote_path)
          return False
  
      def uploadThis(localfile, filename):
          print('  Sending file: %s' % localfile)
          print("POPUP: Sending file: %s" % filename)
          sys.stdout.flush()
          fh = open(localfile, 'rb')
          myFTP.storbinary('STOR %s' % filename, fh)
          fh.close()
  
      uploadThis(file_path_name, filename)
      myFTP.close()
      print("POPUP: File trasfer completed: <font color=\"blue\">%s</font>" % remote_path_prog)
      sys.stdout.flush()
      return True
  
  
  def UploadFTP(program, robot_ip, remote_path, ftp_user, ftp_pass, pause_sec=2):
      """Upload a program or a list of programs to the robot through FTP provided the connection parameters"""
      # Iterate through program list if it is a list of files
      if isinstance(program, list):
          if len(program) == 0:
              print('POPUP: Nothing to transfer')
              sys.stdout.flush()
              pause(pause_sec)
              return
  
          for prog in program:
              UploadFTP(prog, robot_ip, remote_path, ftp_user, ftp_pass, 0)
  
          print("POPUP: <font color=\"blue\">Done: %i files and folders successfully transferred</font>" % len(program))
          sys.stdout.flush()
          pause(pause_sec)
          print("POPUP: Done")
          sys.stdout.flush()
          return
  
      import os
      if os.path.isfile(program):
          print('Sending program file %s...' % program)
          UploadFileFTP(program, robot_ip, remote_path, ftp_user, ftp_pass)
      else:
          print('Sending program folder %s...' % program)
          UploadDirFTP(program, robot_ip, remote_path, ftp_user, ftp_pass)
  
      pause(pause_sec)
      print("POPUP: Done")
      sys.stdout.flush()
  
  
  #------------------------------------------------------
  #--------       TKinter dependencies    ---------------
  _tkinter_available = True
  if sys.version_info[0] < 3:
      # Python 2.X only:
      try:
          import Tkinter as tkinter
          import tkFileDialog as filedialog
          import tkMessageBox as messagebox
      except:
          _tkinter_available = False
  else:
      # Python 3.x only
      try:
          import tkinter
          from tkinter import filedialog
          from tkinter import messagebox
      except ModuleNotFoundError:
          _tkinter_available = False
  
  #------------------
  
  #------------------
  if _tkinter_available:
  
      def getOpenFile(path_preference="C:/RoboDK/Library/", strfile='', strtitle='Open file ...', defaultextension='.txt', filetypes=[('All files', '.*'), ('Text files', '.txt')]):
          """Pop up a file dialog window to select a file to open. Returns a file object opened in read-only mode. Use returned value.name to retrieve the file path."""
          options = {}
          options['initialdir'] = path_preference
          options['title'] = strtitle
          options['defaultextension'] = defaultextension  #'.txt'
          options['filetypes'] = filetypes  # [('all files', '.*'), ('text files', '.txt')]
          options['initialfile'] = strfile
          root = tkinter.Tk()
          root.withdraw()
          root.attributes("-topmost", True)
          file_path = filedialog.askopenfilename(**options)
          # same as: file_path = tkinter.filedialog.askopenfilename()
          return file_path
  
      def getSaveFile(path_preference="C:/RoboDK/Library/", strfile='file.txt', strtitle='Save file as ...', defaultextension='.txt', filetypes=[('All files', '.*'), ('Text files', '.txt')]):
          """Pop up a file dialog window to select a file to save. Returns a file object opened in write-only mode. Use returned value.name to retrieve the file path."""
          options = {}
          options['initialdir'] = path_preference
          options['title'] = strtitle
          options['defaultextension'] = defaultextension  #'.txt'
          options['filetypes'] = filetypes  # [('all files', '.*'), ('text files', '.txt')]
          options['initialfile'] = strfile
          #options['parent'] = root
          root = tkinter.Tk()
          root.withdraw()
          root.attributes("-topmost", True)
          file_path = filedialog.asksaveasfile(**options)
          #same as: file_path = tkinter.filedialog.asksaveasfile(**options)
          return file_path
  
      def getOpenFileName(path_preference="C:/RoboDK/Library/", strfile='', strtitle='Open file ...', defaultextension='.txt', filetypes=[('All files', '.*'), ('Text files', '.txt')]):
          """Pop up a file dialog window to select a file to open. Returns the file path as a string."""
          options = {}
          options['initialdir'] = path_preference
          options['title'] = strtitle
          options['defaultextension'] = defaultextension  #'.txt'
          options['filetypes'] = filetypes  # [('all files', '.*'), ('text files', '.txt')]
          options['initialfile'] = strfile
          root = tkinter.Tk()
          root.withdraw()
          root.attributes("-topmost", True)
          file_path = filedialog.askopenfilename(**options)
          # same as: file_path = tkinter.filedialog.askopenfilename()
          return file_path
  
      def getSaveFileName(path_preference="C:/RoboDK/Library/", strfile='file.txt', strtitle='Save file as ...', defaultextension='.txt', filetypes=[('All files', '.*'), ('Text files', '.txt')]):
          """Pop up a file dialog window to select a file to save. Returns the file path as a string."""
          options = {}
          options['initialdir'] = path_preference
          options['title'] = strtitle
          options['defaultextension'] = defaultextension  #'.txt'
          options['filetypes'] = filetypes  # [('all files', '.*'), ('text files', '.txt')]
          options['initialfile'] = strfile
          #options['parent'] = root
          root = tkinter.Tk()
          root.withdraw()
          root.attributes("-topmost", True)
          file_path = filedialog.asksaveasfilename(**options)
          #same as: file_path = tkinter.filedialog.asksaveasfile(**options)
          return file_path
  
      def getSaveFolder(path_programs='/', popup_msg='Select a directory to save your program'):
          """Ask the user to select a folder to save a program or other file. Returns the path of the folder as a string."""
          root = tkinter.Tk()
          root.withdraw()
          root.attributes("-topmost", True)
          dirname = filedialog.askdirectory(initialdir=path_programs, title=popup_msg)
          if len(dirname) < 1:
              dirname = None
  
          return dirname
  
      def getOpenFolder(path_preference="C:/RoboDK/Library/", strtitle='Open folder ...'):
          """Pop up a folder dialog window to select a folder to open. Returns the path of the folder as a string."""
          options = {}
          options['title'] = strtitle
          options['initialdir'] = path_preference
          root = tkinter.Tk()
          root.withdraw()
          root.attributes("-topmost", True)
          file_path = filedialog.askdirectory(**options)
          return file_path
  
      def ShowMessage(msg, title=None):
          """Show a blocking message"""
          print(msg)
          if title is None:
              title = msg
  
          root = tkinter.Tk()
          root.overrideredirect(1)
          root.withdraw()
          root.attributes("-topmost", True)
          result = messagebox.showinfo(title, msg)  #, icon='warning')#, parent=texto)
          root.destroy()
          return result
  
      def ShowMessageYesNo(msg, title=None):
          """Show a blocking message and let the user answer Yes or No"""
          print(msg)
          if title is None:
              title = msg
  
          root = tkinter.Tk()
          root.overrideredirect(1)
          root.withdraw()
          root.attributes("-topmost", True)
          result = messagebox.askyesno(title, msg)  #, icon='warning')#, parent=texto)
          root.destroy()
          return result
  
      def ShowMessageYesNoCancel(msg, title=None):
          """Show a blocking message and let the user answer Yes, No or Cancel"""
          print(msg)
          if title is None:
              title = msg
  
          root = tkinter.Tk()
          root.overrideredirect(1)
          root.withdraw()
          root.attributes("-topmost", True)
          result = messagebox.askyesnocancel(title, msg)  #, icon='warning')#, parent=texto)
          root.destroy()
          return result
  
      class MessageBox(object):
          def __init__(self, msg, b1, b2, frame, t, entry):
  
              root = self.root = tkinter.Tk()
              root.title('Input')
              self.msg = str(msg)
              # ctrl+c to copy self.msg
              root.bind('<Control-c>', func=self.to_clip)
              # remove the outer frame if frame=False
              if not frame: root.overrideredirect(True)
              # default values for the buttons to return
              self.b1_return = True
              self.b2_return = False
              # if b1 or b2 is a tuple unpack into the button text & return value
              if isinstance(b1, tuple): b1, self.b1_return = b1
              if isinstance(b2, tuple): b2, self.b2_return = b2
              # main frame
              frm_1 = tkinter.Frame(root)
              frm_1.pack(ipadx=2, ipady=2)
              # the message
              message = tkinter.Label(frm_1, text=self.msg)
              message.pack(padx=8, pady=8)
              # if entry=True create and set focus
              if entry is not None:
                  if entry == True:
                      entry = ''
                  self.entry = tkinter.Entry(frm_1)
                  self.entry.pack()
                  self.entry.insert(0, entry)
                  self.entry.focus_set()
              # button frame
              frm_2 = tkinter.Frame(frm_1)
              frm_2.pack(padx=4, pady=4)
              # buttons
              btn_1 = tkinter.Button(frm_2, width=8, text=b1)
              btn_1['command'] = self.b1_action
              btn_1.pack(side='left')
              if not entry: btn_1.focus_set()
              btn_2 = tkinter.Button(frm_2, width=8, text=b2)
              btn_2['command'] = self.b2_action
              btn_2.pack(side='left')
              # the enter button will trigger the focused button's action
              btn_1.bind('<KeyPress-Return>', func=self.b1_action)
              btn_2.bind('<KeyPress-Return>', func=self.b2_action)
              # roughly center the box on screen
              # for accuracy see: http://stackoverflow.com/a/10018670/1217270
              root.update_idletasks()
              xp = (root.winfo_screenwidth() // 2) - (root.winfo_width() // 2)
              yp = (root.winfo_screenheight() // 2) - (root.winfo_height() // 2)
              geom = (root.winfo_width(), root.winfo_height(), xp, yp)
              root.geometry('{0}x{1}+{2}+{3}'.format(*geom))
              # call self.close_mod when the close button is pressed
              root.protocol("WM_DELETE_WINDOW", self.close_mod)
              # a trick to activate the window (on windows 7)
              #root.deiconify()
              # if t is specified: call time_out after t seconds
              if t: root.after(int(t * 1000), func=self.time_out)
  
          def b1_action(self, event=None):
              try:
                  x = self.entry.get()
              except AttributeError:
                  self.returning = self.b1_return
                  self.root.quit()
              else:
                  if x:
                      self.returning = x
                      self.root.quit()
  
          def b2_action(self, event=None):
              self.returning = self.b2_return
              self.root.quit()
  
          # remove this function and the call to protocol
          # then the close button will act normally
          def close_mod(self):
              pass
  
          def time_out(self):
              try:
                  x = self.entry.get()
              except AttributeError:
                  self.returning = None
              else:
                  self.returning = x
              finally:
                  self.root.quit()
  
          def to_clip(self, event=None):
              self.root.clipboard_clear()
              self.root.clipboard_append(self.msg)
  
      def mbox(msg, b1='OK', b2='Cancel', frame=True, t=False, entry=None):
          """Create an instance of MessageBox, and get data back from the user.
  
          :param msg: string to be displayed
          :type msg: str
          :param b1: left button text, or a tuple (<text for button>, <to return on press>)
          :type b1: str, tuple
          :param b2: right button text, or a tuple (<text for button>, <to return on press>)
          :type b2: str, tuple
          :param frame: include a standard outerframe: True or False
          :type frame: bool
          :param t: time in seconds (int or float) until the msgbox automatically closes
          :type t: int, float
          :param entry: include an entry widget that will provide its contents returned. Provide text to fill the box
          :type entry: None, bool, str
  
          Example:
  
              .. code-block:: python
  
                  name = mbox('Enter your name', entry=True)
                  name = mbox('Enter your name', entry='default')
                  if name:
                      print("Value: " + name)
  
                  value = mbox('Male or female?', ('male', 'm'), ('female', 'f'))
                  mbox('Process done')
  
          """
          msgbox = MessageBox(msg, b1, b2, frame, t, entry)
  
          try:
              from robolink import getPathIcon
              iconpath = getPathIcon()
              msgbox.root.iconbitmap(iconpath)
          except:
              print("RoboDK's Robolink module not found")
  
          msgbox.root.attributes("-topmost", True)
          msgbox.root.mainloop()
          # the function pauses here until the mainloop is quit
          msgbox.root.destroy()
          return msgbox.returning
  