Merge pull request #40 from maxspahn/ft-point-fk

ft[pointfk]: restores point fk and aligns planar fks with common fk.

forwardkinematics/fksCommon/fk.py

@@ -1,4 +1,6 @@
+from typing import Union
 import numpy as np
+import casadi as ca
 
 class ForwardKinematics():
 
@@ -15,4 +17,22 @@ def set_mount_transformation(self, mount_transformation: np.ndarray):
     def n(self) -> int:
         return self._n
 
+    def casadi(
+            self, q: ca.SX,
+            child_link: Union[int, str],
+            parent_link: Union[int, str, None] = None,
+            link_transformation=np.eye(4),
+            position_only: bool = False
+        ) -> ca.SX:
+        raise NotImplementedError
+
+    def numpy(
+            self, q: np.ndarray,
+            child_link: Union[int, str],
+            parent_link: Union[int, str, None] = None,
+            link_transformation=np.eye(4),
+            position_only: bool = False
+        ) -> np.ndarray:
+        raise NotImplementedError
+
 

forwardkinematics/planarFks/planarArmFk.py

@@ -3,60 +3,82 @@
 import casadi as ca
 from forwardkinematics.planarFks.planar_fk import ForwardKinematicsPlanar
 
+
 def get_rotation_matrix_casadi(angle: ca.SX) -> ca.SX:
     print(angle)
-    return ca.SX(ca.vcat([
-        ca.hcat([ca.cos(angle), -ca.sin(angle), 0]),
-        ca.hcat([ca.sin(angle), ca.cos(angle), 0]),
-        ca.hcat([0, 0, 1]),
-    ]))
+    return ca.SX(
+        ca.vcat(
+            [
+                ca.hcat([ca.cos(angle), -ca.sin(angle), 0]),
+                ca.hcat([ca.sin(angle), ca.cos(angle), 0]),
+                ca.hcat([0, 0, 1]),
+            ]
+        )
+    )
+
 
 def get_rotation_matrix_numpy(angle: ca.SX) -> np.ndarray:
-    return np.array([
-        [np.cos(angle), -np.sin(angle), 0],
-        [np.sin(angle), np.cos(angle), 0],
-        [0, 0, 1],
-    ])
+    return np.array(
+        [
+            [np.cos(angle), -np.sin(angle), 0],
+            [np.sin(angle), np.cos(angle), 0],
+            [0, 0, 1],
+        ]
+    )
 
 
 class PlanarArmFk(ForwardKinematicsPlanar):
     def __init__(self, n):
         super().__init__()
         self._n = n
 
-    def casadi(self, q: ca.SX, child_link: Union[int, str], parent_link: Union[int, str, None] = None, link_transformation=np.eye(3), position_only: bool = False):
+    def casadi(
+        self,
+        q: ca.SX,
+        child_link: Union[int, str],
+        parent_link: Union[int, str, None] = None,
+        link_transformation=np.eye(3),
+        position_only: bool = False,
+    ):
         child_link = self.ensure_int_link(child_link)
         if not parent_link:
             parent_link = 0
         parent_link = self.ensure_int_link(parent_link)
         if not parent_link:
             parent_link = 0
         fk = get_rotation_matrix_casadi(q[parent_link])
-        for i in range(parent_link+1, child_link + 1):
+        for i in range(parent_link + 1, child_link + 1):
             if i == self.n():
                 fk_i = get_rotation_matrix_casadi(0)
             else:
                 fk_i = get_rotation_matrix_casadi(q[i])
-            fk_i[0,2] = 1
+            fk_i[0, 2] = 1
             fk = ca.mtimes(fk, fk_i)
         fk = ca.mtimes(fk, link_transformation)
         if position_only:
             return fk[0:2, 2]
         else:
             return fk
 
-    def numpy(self, q: ca.SX, child_link: Union[int, str], parent_link: Union[int, str, None] = None, link_transformation=np.eye(3), position_only: bool = False):
+    def numpy(
+        self,
+        q: np.ndarray,
+        child_link: Union[int, str],
+        parent_link: Union[int, str, None] = None,
+        link_transformation=np.eye(3),
+        position_only: bool = False,
+    ):
         child_link = self.ensure_int_link(child_link)
         if not parent_link:
             parent_link = 0
         parent_link = self.ensure_int_link(parent_link)
         fk = get_rotation_matrix_numpy(q[parent_link])
-        for i in range(parent_link+1, child_link + 1):
+        for i in range(parent_link + 1, child_link + 1):
             if i == self.n():
                 fk_i = get_rotation_matrix_numpy(0)
             else:
                 fk_i = get_rotation_matrix_numpy(q[i])
-            fk_i[0,2] = 1
+            fk_i[0, 2] = 1
             fk = np.dot(fk, fk_i)
         fk = np.dot(fk, link_transformation)
         if position_only:

forwardkinematics/planarFks/planar_fk.py

@@ -1,6 +1,8 @@
 from typing import Union
 import re
+
 import numpy as np
+import casadi as ca
 
 from forwardkinematics.fksCommon.fk import ForwardKinematics
 
@@ -13,7 +15,7 @@ def __init__(self):
         super().__init__()
         self._mount_transformation = np.identity(3)
 
-    def ensure_int_link(self, link: Union[str, int]):
+    def ensure_int_link(self, link: Union[str, int]) -> int:
         if isinstance(link, int):
             return link
         regex_match = re.match(r'\D*(\d*)', link)
@@ -22,3 +24,20 @@ def ensure_int_link(self, link: Union[str, int]):
         except Exception as _:
             raise NoLinkIndexFoundInLinkNameError(f"Link name {link} could not be resolved")
 
+    def casadi(
+            self, q: ca.SX,
+            child_link: Union[int, str],
+            parent_link: Union[int, str, None] = None,
+            link_transformation=np.eye(3),
+            position_only: bool = False
+        ) -> ca.SX:
+        raise NotImplementedError
+
+    def numpy(
+            self, q: np.ndarray,
+            child_link: Union[int, str],
+            parent_link: Union[int, str, None] = None,
+            link_transformation=np.eye(3),
+            position_only: bool = False
+        ) -> np.ndarray:
+        raise NotImplementedError

forwardkinematics/planarFks/point_fk.py

@@ -0,0 +1,58 @@
+from typing import Union
+
+import numpy as np
+import casadi as ca
+
+from forwardkinematics.planarFks.planar_fk import ForwardKinematicsPlanar
+
+
+class PointFk(ForwardKinematicsPlanar):
+    def __init__(self):
+        super().__init__()
+        self._n = 2
+
+    def casadi(
+        self,
+        q: ca.SX,
+        child_link: Union[int, str],
+        parent_link: Union[int, str, None] = None,
+        link_transformation=np.eye(3),
+        position_only: bool = False,
+    ):
+        fk = ca.SX.eye(3)
+        child_link = self.ensure_int_link(child_link)
+        if not parent_link:
+            parent_link = 0
+        parent_link = self.ensure_int_link(parent_link)
+        if not parent_link:
+            parent_link = 0
+        if parent_link == 0 and child_link > 0:
+            fk[0:2, 2] = q
+        fk = ca.mtimes(fk, link_transformation)
+        if position_only:
+            return fk[0:2, 2]
+        else:
+            return fk
+
+    def numpy(
+        self,
+        q: np.ndarray,
+        child_link: Union[int, str],
+        parent_link: Union[int, str, None] = None,
+        link_transformation=np.eye(3),
+        position_only: bool = False,
+    ):
+        fk = np.eye(3)
+        child_link = self.ensure_int_link(child_link)
+        if not parent_link:
+            parent_link = 0
+        parent_link = self.ensure_int_link(parent_link)
+        if not parent_link:
+            parent_link = 0
+        if parent_link == 0 and child_link > 0:
+            fk[0:2, 2] = q
+        fk = np.dot(fk, link_transformation)
+        if position_only:
+            return fk[0:2, 2]
+        else:
+            return fk

pyproject.toml

@@ -1,6 +1,6 @@
 [tool.poetry]
 name = "forwardkinematics"
-version = "1.2.1"
+version = "1.2.2"
 description = "\"Light-weight implementation of forward kinematics using casadi.\""
 authors = ["Max Spahn <[email protected]>"]
 license = "MIT"

tests/test_not_implemented_errors.py

@@ -0,0 +1,28 @@
+import pytest
+import casadi as ca
+import numpy as np
+
+from forwardkinematics.fksCommon.fk import ForwardKinematics
+from forwardkinematics.planarFks.planar_fk import ForwardKinematicsPlanar
+
+class myFK(ForwardKinematics):
+    pass
+
+class myPlanarFK(ForwardKinematicsPlanar):
+    pass
+
+
+def test_myFK():
+    fk = myFK()
+    with pytest.raises(NotImplementedError):
+        fk.casadi(ca.SX.sym('q', 6), 3)
+    with pytest.raises(NotImplementedError):
+        fk.numpy(np.ones(5), 5)
+
+def test_myPlanarFk():
+    fk = myPlanarFK()
+    with pytest.raises(NotImplementedError):
+        fk.casadi(ca.SX.sym('q', 6), 3)
+    with pytest.raises(NotImplementedError):
+        fk.numpy(np.ones(5), 5)
+

tests/test_planarFk.py

@@ -2,14 +2,16 @@
 import numpy as np
 import pytest
 from forwardkinematics.planarFks.planarArmFk import PlanarArmFk
+from forwardkinematics.planarFks.planar_fk import NoLinkIndexFoundInLinkNameError
+from forwardkinematics.planarFks.point_fk import PointFk
 
 
 def test_planarFk():
     q_ca = ca.SX.sym("q", 2)
     fkPlanar = PlanarArmFk(n=2)
     q_np = np.array([1.0, 0.0])
     fkCasadi = fkPlanar.casadi(q_ca, 2, position_only=False)
-    fkNumpy = fkPlanar.casadi(q_np, 2, position_only=False)
+    fkNumpy = fkPlanar.numpy(q_np, 2, position_only=False)
     assert fkNumpy[0,2] == np.cos(1.0) * 1.0 + np.cos(1.0) * 1.0
     assert fkNumpy[1,2] == np.sin(1.0) * 1.0 + np.sin(1.0) * 1.0
     assert fkNumpy[0,0] == np.cos(1.0)
@@ -33,3 +35,22 @@ def test_planarFk_by_name():
     assert test_eval[0,2] == pytest.approx(np.cos(1.0) * 1.0 + np.cos(1.0) * 1.0)
     assert test_eval[1,2] == pytest.approx(np.sin(1.0) * 1.0 + np.sin(1.0) * 1.0)
     assert test_eval[0,0] == np.cos(1.0)
+
+    with pytest.raises(NoLinkIndexFoundInLinkNameError):
+        fkPlanar.casadi(q_ca, "peter_link", position_only=False)
+
+def test_pointFk():
+    q_ca = ca.SX.sym("q", 2)
+    fk_point = PointFk()
+    q_np = np.array([1.0, 0.0])
+    fkCasadi = fk_point.casadi(q_ca, 2, position_only=False)
+    fkNumpy = fk_point.numpy(q_np, 2, position_only=False)
+    assert fkNumpy[0,2] == 1.0
+    assert fkNumpy[1,2] == 0.0
+    assert fkNumpy[0,0] == 1.0
+    fkCasadi_fun = ca.Function('test_fun', [q_ca], [fkCasadi])
+    test_eval = np.array(fkCasadi_fun(q_np))
+    assert test_eval[0,2] == 1.0
+    assert test_eval[1,2] == 0.0
+    assert test_eval[0,0] == 1.0
+