From 217e787223272e7f247f2f6cd85bb59910573b0b Mon Sep 17 00:00:00 2001 From: Max Spahn Date: Mon, 15 Apr 2024 15:38:17 +0200 Subject: [PATCH 1/2] ft[pointfk]: restores point fk and aligns planar fks with common fk. --- forwardkinematics/fksCommon/fk.py | 20 ++++++++ forwardkinematics/planarFks/planarArmFk.py | 54 ++++++++++++++------ forwardkinematics/planarFks/planar_fk.py | 21 +++++++- forwardkinematics/planarFks/point_fk.py | 58 ++++++++++++++++++++++ tests/test_not_implemented_errors.py | 28 +++++++++++ tests/test_planarFk.py | 23 ++++++++- 6 files changed, 186 insertions(+), 18 deletions(-) create mode 100644 forwardkinematics/planarFks/point_fk.py create mode 100644 tests/test_not_implemented_errors.py diff --git a/forwardkinematics/fksCommon/fk.py b/forwardkinematics/fksCommon/fk.py index 13811ff..ca50243 100644 --- a/forwardkinematics/fksCommon/fk.py +++ b/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 + diff --git a/forwardkinematics/planarFks/planarArmFk.py b/forwardkinematics/planarFks/planarArmFk.py index 117603b..e81bd76 100644 --- a/forwardkinematics/planarFks/planarArmFk.py +++ b/forwardkinematics/planarFks/planarArmFk.py @@ -3,20 +3,28 @@ 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): @@ -24,7 +32,14 @@ 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 @@ -32,12 +47,12 @@ def casadi(self, q: ca.SX, child_link: Union[int, str], parent_link: Union[int, 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: @@ -45,18 +60,25 @@ def casadi(self, q: ca.SX, child_link: Union[int, str], parent_link: Union[int, 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: diff --git a/forwardkinematics/planarFks/planar_fk.py b/forwardkinematics/planarFks/planar_fk.py index dbeab15..4ed4acf 100644 --- a/forwardkinematics/planarFks/planar_fk.py +++ b/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 diff --git a/forwardkinematics/planarFks/point_fk.py b/forwardkinematics/planarFks/point_fk.py new file mode 100644 index 0000000..2af24fe --- /dev/null +++ b/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 diff --git a/tests/test_not_implemented_errors.py b/tests/test_not_implemented_errors.py new file mode 100644 index 0000000..30d92c2 --- /dev/null +++ b/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) + diff --git a/tests/test_planarFk.py b/tests/test_planarFk.py index 208ec7c..e85d30a 100644 --- a/tests/test_planarFk.py +++ b/tests/test_planarFk.py @@ -2,6 +2,8 @@ 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(): @@ -9,7 +11,7 @@ def test_planarFk(): 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 + From 8557aed6a67b26d4ddcb21d00a1d5455ca17d54a Mon Sep 17 00:00:00 2001 From: Max Spahn Date: Mon, 15 Apr 2024 15:39:08 +0200 Subject: [PATCH 2/2] Bumps version. --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index ed5b6e8..b07ff33 100644 --- a/pyproject.toml +++ b/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 "] license = "MIT"