Replacing pykdl with custom transformations library

__init__.py

@@ -1 +1,6 @@
 from .external.tf_function import transformations
+from .ambf_utilities.tf_utils.scripts.vector import Vector
+from .ambf_utilities.tf_utils.scripts.frame import Frame
+from .ambf_utilities.tf_utils.scripts.rotation import Rotation
+from .ambf_utilities.tf_utils.scripts.twist import Twist
+from .ambf_utilities.tf_utils.scripts.wrench import Wrench

ambf_utilities/__init__.py

@@ -0,0 +1,5 @@
+from .tf_utils.scripts.vector import Vector
+from .tf_utils.scripts.frame import Frame
+from .tf_utils.scripts.rotation import Rotation
+from .tf_utils.scripts.twist import Twist
+from .tf_utils.scripts.wrench import Wrench

ambf_utilities/tf_utils/__init__.py

@@ -0,0 +1,5 @@
+from .scripts.vector import Vector
+from .scripts.frame import Frame
+from .scripts.rotation import Rotation
+from .scripts.twist import Twist
+from .scripts.wrench import Wrench

ambf_utilities/tf_utils/scripts/__init__.py

@@ -0,0 +1,7 @@
+__all__ = ["vector", "frame", "rotation", "twist", "wrench"]
+
+from .vector import Vector
+from .frame import Frame
+from .rotation import Rotation
+from .twist import Twist
+from .wrench import Wrench

ambf_utilities/tf_utils/scripts/frame.py

@@ -0,0 +1,200 @@
+""" This class will generate a Frame in 3D space to mimic Frame in pyKDL"""
+
+import numpy as np
+from numpy import pi, sin, cos, tan, arctan2
+from vector import Vector
+from twist import Twist
+from wrench import Wrench
+from rotation import Rotation
+from warnings import warn
+import sys
+
+
+class Frame(object):
+
+  M = Rotation()
+  p = Vector()
+
+  @staticmethod
+  def make_Frame(HTM):
+    """
+    Return a frame from a 4x4 Homogenous transformaiton matrix
+
+    @param 4x4mat Homogenous TF Matrix
+
+    @return Frame
+    """
+
+    frame = Frame(HTM[:3, :3], HTM[0:3, 3])
+    return frame
+
+  @staticmethod
+  def make_HTM(f):
+    """
+    Return a 4x4 Homogenous Transformation Matrix from a Frame
+
+    @param Frame f
+
+    @return 4x4mat Homogenous TF Matrix
+    """
+
+    mat_44 = np.ndarray((4, 4))
+    mat_44[:3, :3] = f.M
+    mat_44[3:, :4] = np.array([0, 0, 0, 1])
+    mat_44[:3, 3] = f.p
+
+    return mat_44
+
+  @staticmethod
+  def Identity():
+    """
+        Constructs an identity frame
+
+        @return Frame
+        """
+
+    return Frame()
+
+  @staticmethod
+  def HD(a, alpha, d, theta):
+    """
+        Constructs a transformationmatrix T_link(i-1)_link(i) with the Denavit-Hartenberg 
+        convention as described in the original publictation: Denavit, J. and Hartenberg, 
+        R. S., A kinematic notation for lower-pair mechanisms based on matrices, 
+        ASME Journal of Applied Mechanics, 23:215-221, 1955.
+
+        @param float a
+        @param float alpha (radians)
+        @param float d
+        @param float theta (radians)
+
+        @return Frame
+        """
+
+    ct = cos(theta)
+    st = sin(theta)
+    sa = sin(alpha)
+    ca = cos(alpha)
+    return Frame(
+      Rotation(ct,
+               -st * ca,
+               st * sa,
+               st,
+               ct * ca,
+               -ct * sa,
+               0,
+               sa,
+               ca),
+      Vector(a * ct,
+             a * st,
+             d)
+    )
+
+  @staticmethod
+  def DH_Craig1989(a, alpha, d, theta):
+    """
+        Constructs a transformationmatrix T_link(i-1)_link(i) with the Denavit-Hartenberg 
+        convention as described in the Craigs book: Craig, J. J.,Introduction to Robotics: 
+        Mechanics and Control, Addison-Wesley, isbn:0-201-10326-5, 1986.
+
+        @param float a
+        @param float alpha (radians)
+        @param float d
+        @param float theta (radians)
+
+        @return Frame
+        """
+
+    ct = cos(theta)
+    st = sin(theta)
+    sa = sin(alpha)
+    ca = cos(alpha)
+    return Frame(
+      Rotation(ct,
+               -st,
+               0,
+               st * ca,
+               ct * ca,
+               -sa,
+               st * sa,
+               ct * sa,
+               ca),
+      Vector(a,
+             -sa * d,
+             ca * d)
+    )
+
+  @staticmethod
+  def AddDelta(f, t, d):
+    """
+        NOT IMPLEMENTED
+        Constructs a frame that is obtained by: starting from frame f, apply twist t, during time d
+
+        @param Frame f
+        @param Twist t
+        @param float d
+
+        @return Frame
+        """
+
+    return
+
+  def __init__(self, rot=None, pos=None):
+    """
+        Construct an an object of frame
+
+        @param Rotation rot
+        @param Vector pos
+        """
+    super(Frame, self).__init__()
+
+    if rot is not None:
+      self.M = Rotation(rot)
+
+    if pos is not None:
+      self.p = Vector(pos)
+
+    return
+
+  def Integrate(self, twist, frequency):
+    """
+        This frame is integrated into an updated frame with sample frequence, using first order integration
+        NOT IMPLEMENTED
+        """
+
+    return
+
+  def Inverse(self):
+    """
+        Returns the inverse of the frame
+
+        @return Frame
+        """
+
+    self.M.SetInverse()
+    self.p = -self.M * self.p
+
+    return self
+
+  def __mul__(self, f):
+
+    self.mat_44 = Frame.make_HTM(self)
+    f.mat_44 = Frame.make_HTM(f)
+
+    mul = np.matmul(self.mat_44, f.mat_44)
+
+    frame = Frame.make_Frame(mul)
+
+    return frame
+
+
+def test():
+  f1 = Frame()
+  f2 = Frame()
+  f = f1 * f2
+  print(f.p)
+  print(f.M)
+
+
+if __name__ == '__main__':
+  test()