From 662b776b3b2107ecaeec8ff0c9e929ef6ed5a00d Mon Sep 17 00:00:00 2001 From: Max Spahn Date: Thu, 3 Oct 2024 21:39:32 +0200 Subject: [PATCH 1/3] ft[symbols]: Adds support for passing parameters as symbols to the forward kinematics. --- .../urdfFks/casadiConversion/urdfparser.py | 65 +++++++++------ forwardkinematics/urdfFks/urdfFk.py | 79 +++++++++++++------ pyproject.toml | 2 +- tests/test_pandaFk.py | 75 +++++++++++++----- tests/test_urdfparser.py | 48 +++++++++++ 5 files changed, 203 insertions(+), 66 deletions(-) create mode 100644 tests/test_urdfparser.py diff --git a/forwardkinematics/urdfFks/casadiConversion/urdfparser.py b/forwardkinematics/urdfFks/casadiConversion/urdfparser.py index 910273c..9b6ea1f 100644 --- a/forwardkinematics/urdfFks/casadiConversion/urdfparser.py +++ b/forwardkinematics/urdfFks/casadiConversion/urdfparser.py @@ -2,17 +2,21 @@ Changes are in get_forward_kinematics as it allows to pass the variable as an argument. """ +from typing import Dict + import casadi as ca import numpy as np from urdf_parser_py.urdf import URDF + import forwardkinematics.urdfFks.casadiConversion.geometry.transformation_matrix as T class URDFparser(object): """Class that turns a chain from URDF to casadi functions.""" + actuated_types = ["prismatic", "revolute", "continuous"] - - def __init__(self, root_link: str="base_link", end_links: list = None): + + def __init__(self, root_link: str = "base_link", end_links: list = None): self._root_link = root_link if isinstance(end_links, str): self._end_links = [end_links] @@ -89,17 +93,16 @@ def set_active_joints(self) -> None: self._active_joints.add(parent_joint) if parent_link == self._root_link: break - + def active_joints(self) -> set: return self._active_joints - def get_joint_info(self, root, tip) -> list: """Using an URDF to extract joint information, i.e list of joints, actuated names and upper and lower limits.""" chain = self.robot_desc.get_chain(root, tip) if self.robot_desc is None: - raise ValueError('Robot description not loaded from urdf') + raise ValueError("Robot description not loaded from urdf") joint_list = [] @@ -120,46 +123,64 @@ def detect_link_names(self): if link.name in self.robot_desc.parent_map: self._link_names.append(link.name) else: - print(f"Link with name {link.name} does not has a parent. Link name is skipped.") + print( + f"Link with name {link.name} does not has a parent. Link name is skipped." + ) return self._link_names - def get_forward_kinematics(self, root, tip, q, link_transformation=np.eye(4)): + def get_forward_kinematics( + self, + root, + tip, + q, + link_transformation=np.eye(4), + symbolic_parameters: Dict[str, Dict[str, ca.SX]] = None, + ) -> Dict[str, ca.SX]: + if symbolic_parameters is None: + symbolic_parameters = {} """Returns the forward kinematics as a casadi function.""" if self.robot_desc is None: - raise ValueError('Robot description not loaded from urdf') + raise ValueError("Robot description not loaded from urdf") joint_list = self.get_joint_info(self._absolute_root_link, tip) T_fk = ca.SX.eye(4) for joint in joint_list: + # For xyz and rpy, check if they are in the symbolic parameters + xyzrpy = joint.origin.xyz + joint.origin.rpy + if joint.name in symbolic_parameters: + for index, parameter in enumerate( + ["x", "y", "z", "roll", "pitch", "yaw"] + ): + if parameter in symbolic_parameters[joint.name]: + xyzrpy[index] = symbolic_parameters[joint.name][parameter] + + # xyz = joint.origin.xyz + xyz = xyzrpy[:3] + rpy = xyzrpy[3:] if joint.type == "fixed": - xyz = joint.origin.xyz - rpy = joint.origin.rpy joint_frame = T.numpy_rpy(xyz, *rpy) T_fk = ca.mtimes(T_fk, joint_frame) elif joint.type == "prismatic": if joint.axis is None: - axis = ca.np.array([1., 0., 0.]) + axis = ca.np.array([1.0, 0.0, 0.0]) else: axis = ca.np.array(joint.axis) - joint_frame = T.prismatic(joint.origin.xyz, - joint.origin.rpy, - joint.axis, q[self._joint_map[joint.name]]) + joint_frame = T.prismatic( + xyz, rpy, joint.axis, q[self._joint_map[joint.name]] + ) T_fk = ca.mtimes(T_fk, joint_frame) elif joint.type in ["revolute", "continuous"]: if joint.axis is None: - axis = ca.np.array([1., 0., 0.]) + axis = ca.np.array([1.0, 0.0, 0.0]) else: axis = ca.np.array(joint.axis) - axis = (1./ca.np.linalg.norm(axis))*axis + axis = (1.0 / ca.np.linalg.norm(axis)) * axis joint_frame = T.revolute( - joint.origin.xyz, - joint.origin.rpy, - joint.axis, q[self._joint_map[joint.name]]) + xyz, rpy, joint.axis, q[self._joint_map[joint.name]] + ) T_fk = ca.mtimes(T_fk, joint_frame) T_fk = ca.mtimes(T_fk, link_transformation) - return { - "T_fk": T_fk - } + return {"T_fk": T_fk} diff --git a/forwardkinematics/urdfFks/urdfFk.py b/forwardkinematics/urdfFks/urdfFk.py index d436789..519f346 100644 --- a/forwardkinematics/urdfFks/urdfFk.py +++ b/forwardkinematics/urdfFks/urdfFk.py @@ -1,8 +1,9 @@ -from typing import Union, List -import numpy as np +from typing import List, Union + import casadi as ca -import forwardkinematics.urdfFks.casadiConversion.urdfparser as u2c +import numpy as np +import forwardkinematics.urdfFks.casadiConversion.urdfparser as u2c from forwardkinematics.fksCommon.fk import ForwardKinematics @@ -11,7 +12,13 @@ class LinkNotInURDFError(Exception): class URDFForwardKinematics(ForwardKinematics): - def __init__(self, urdf: str, root_link: str, end_links: List[str], base_type: str = 'holonomic'): + def __init__( + self, + urdf: str, + root_link: str, + end_links: List[str], + base_type: str = "holonomic", + ): super().__init__() self._urdf = urdf self._root_link = root_link @@ -21,17 +28,19 @@ def __init__(self, urdf: str, root_link: str, end_links: List[str], base_type: s self._n = self.robot.degrees_of_freedom() self._q_ca = ca.SX.sym("q", self._n) self._mount_transformation = np.identity(4) - if base_type in ['diffdrive']: + if base_type in ["diffdrive"]: self._q_base = ca.SX.sym("q_base", 3) self.generate_functions() def n(self) -> int: - if self._base_type == 'diffdrive': + if self._base_type == "diffdrive": return self._n + 3 return self._n def read_urdf(self): - self.robot = u2c.URDFparser(root_link=self._root_link, end_links=self._end_links) + self.robot = u2c.URDFparser( + root_link=self._root_link, end_links=self._end_links + ) self.robot.from_string(self._urdf) self.robot.detect_link_names() self.robot.set_joint_variable_map() @@ -39,16 +48,24 @@ def read_urdf(self): def generate_functions(self): self._fks = {} for link in self.robot.link_names(): - if self._base_type in ['diffdrive']: + if self._base_type in ["diffdrive"]: q = ca.vcat([self._q_base, self._q_ca]) else: q = self._q_ca - ca_fun = ca.Function( - "fk" + link, [q], [self.casadi(q, link)] - ) + ca_fun = ca.Function("fk" + link, [q], [self.casadi(q, link)]) self._fks[link] = ca_fun - def casadi(self, q: ca.SX, child_link: str, parent_link: Union[str, None] = None, link_transformation=np.eye(4), position_only=False): + def casadi( + self, + q: ca.SX, + child_link: str, + parent_link: Union[str, None] = None, + link_transformation=np.eye(4), + position_only=False, + symbolic_parameters=None, + ): + if symbolic_parameters is None: + symbolic_parameters = {} if parent_link is None: parent_link = self._root_link if child_link not in self.robot.link_names(): @@ -56,26 +73,39 @@ def casadi(self, q: ca.SX, child_link: str, parent_link: Union[str, None] = None f"""The link you have requested, {child_link}, is not in the urdf. Possible links are {self.robot.link_names()}""" ) - if self._base_type in ['diffdrive']: - fk = self.robot.get_forward_kinematics(parent_link, child_link, q[2:], link_transformation)["T_fk"] + if self._base_type in ["diffdrive"]: + fk = self.robot.get_forward_kinematics( + parent_link, child_link, q[2:], link_transformation + )["T_fk"] c = ca.cos(q[2]) s = ca.sin(q[2]) - T_base = ca.vcat([ - ca.hcat([c, -s, 0, q[0]]), - ca.hcat([s, c, 0, q[1]]), - ca.hcat([0, 0, 1, 0]), - ca.hcat([0, 0, 0, 1]), - ]) + T_base = ca.vcat( + [ + ca.hcat([c, -s, 0, q[0]]), + ca.hcat([s, c, 0, q[1]]), + ca.hcat([0, 0, 1, 0]), + ca.hcat([0, 0, 0, 1]), + ] + ) fk = ca.mtimes(T_base, fk) else: - fk = self.robot.get_forward_kinematics(parent_link, child_link, q, link_transformation)["T_fk"] + fk = self.robot.get_forward_kinematics( + parent_link, child_link, q, link_transformation, symbolic_parameters + )["T_fk"] fk = ca.mtimes(self._mount_transformation, fk) if position_only: fk = fk[0:3, 3] return fk - def numpy(self, q: ca.SX, child_link: str, parent_link: Union[str, None] = None, link_transformation=np.eye(4), position_only=False): + def numpy( + self, + q: ca.SX, + child_link: str, + parent_link: Union[str, None] = None, + link_transformation=np.eye(4), + position_only=False, + ): if child_link not in self._fks and child_link != self._root_link: raise LinkNotInURDFError( f"""The link you have requested, {child_link}, is not in the urdf. @@ -97,9 +127,10 @@ def numpy(self, q: ca.SX, child_link: str, parent_link: Union[str, None] = None, else: fk_child = self._fks[child_link](q) tf_parent_child = np.dot(np.linalg.inv(fk_parent), fk_child) - tf_parent_child = np.dot(link_transformation, tf_parent_child) #ToDo check if correct + tf_parent_child = np.dot( + link_transformation, tf_parent_child + ) # ToDo check if correct if position_only: return tf_parent_child[0:3, 3] else: return tf_parent_child - diff --git a/pyproject.toml b/pyproject.toml index a89098c..e7e3c9c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [tool.poetry] name = "forwardkinematics" -version = "1.2.3" +version = "1.2.4" description = "\"Light-weight implementation of forward kinematics using casadi.\"" authors = ["Max Spahn "] license = "MIT" diff --git a/tests/test_pandaFk.py b/tests/test_pandaFk.py index cc890d4..e005d8d 100644 --- a/tests/test_pandaFk.py +++ b/tests/test_pandaFk.py @@ -1,12 +1,13 @@ import os + import casadi as ca import numpy as np import pytest -from forwardkinematics import GenericURDFFk -from forwardkinematics import GenericXMLFk +from forwardkinematics import GenericURDFFk, GenericXMLFk from forwardkinematics.urdfFks.urdfFk import LinkNotInURDFError + @pytest.fixture def fk() -> GenericURDFFk: urdf_file = os.path.dirname(os.path.abspath(__file__)) + "/panda.urdf" @@ -14,11 +15,12 @@ def fk() -> GenericURDFFk: urdf = file.read() fk_panda = GenericURDFFk( urdf, - root_link = 'panda_link0', + root_link="panda_link0", end_links="panda_leftfinger", ) return fk_panda + @pytest.fixture def fk_xml() -> GenericXMLFk: xml_file = os.path.dirname(os.path.abspath(__file__)) + "/panda.xml" @@ -26,24 +28,27 @@ def fk_xml() -> GenericXMLFk: xml = file.read() fk_panda = GenericXMLFk( xml, - root_link = 'panda_link0', + root_link="panda_link0", end_links="panda_leftfinger", ) return fk_panda + def test_pandaFk(fk): q_ca = ca.SX.sym("q", 7) q_np = np.random.random(7) - fkCasadi = fk.casadi(q_ca, 'panda_link9', position_only=False) - fkNumpy = fk.numpy(q_np, 'panda_link9', position_only=False) + fkCasadi = fk.casadi(q_ca, "panda_link9", position_only=False) + fkNumpy = fk.numpy(q_np, "panda_link9", position_only=False) assert isinstance(fkCasadi, ca.SX) assert isinstance(fkNumpy, np.ndarray) + def test_xmlFk(fk_xml): q_ca = ca.SX.sym("q", 9) - fk_casadi = fk_xml.casadi(q_ca, 'link7', position_only=False) + fk_casadi = fk_xml.casadi(q_ca, "link7", position_only=False) assert isinstance(fk_casadi, ca.SX) + def test_compare_xml_urdf(fk, fk_xml): """ Casadi expressions cannot be compared directly, as the expressions might @@ -52,44 +57,76 @@ def test_compare_xml_urdf(fk, fk_xml): """ q_ca = ca.SX.sym("q", 9) for i in range(8): - fk_casadi_urdf = fk.casadi(q_ca, f'panda_link{i}', position_only=False) - fk_casadi_xml = fk_xml.casadi(q_ca, f'link{i}_c', position_only=False) + fk_casadi_urdf = fk.casadi(q_ca, f"panda_link{i}", position_only=False) + fk_casadi_xml = fk_xml.casadi(q_ca, f"link{i}_c", position_only=False) print(fk_casadi_xml) assert fk_casadi_urdf.shape == fk_casadi_xml.shape - fk_xml_function = ca.Function('fk_xml', [q_ca], [fk_casadi_xml]) - fk_urdf_function = ca.Function('fk_urdf', [q_ca], [fk_casadi_urdf]) + fk_xml_function = ca.Function("fk_xml", [q_ca], [fk_casadi_xml]) + fk_urdf_function = ca.Function("fk_urdf", [q_ca], [fk_casadi_urdf]) q_np = np.random.random(9) fk_np_xml = fk_xml_function(q_np) fk_np_urdf = fk_urdf_function(q_np) assert np.allclose(fk_np_xml, fk_np_urdf, atol=1e-4) - fk_np_direct_xml = fk_xml.numpy(q_np, f'link{i}_c', position_only=False) + fk_np_direct_xml = fk_xml.numpy(q_np, f"link{i}_c", position_only=False) assert np.allclose(fk_np_xml, fk_np_direct_xml, atol=1e-4) + def test_pandaFkByName(fk): - q_ca = ca.SX.sym('q', 7) + q_ca = ca.SX.sym("q", 7) q_np = np.random.random(7) - fkCasadi = fk.casadi(q_ca, 'panda_link3', position_only=False) + fkCasadi = fk.casadi(q_ca, "panda_link3", position_only=False) assert isinstance(fkCasadi, ca.SX) + def test_pandafkByWrongName(fk): - q_ca = ca.SX.sym('q', 7) + q_ca = ca.SX.sym("q", 7) with pytest.raises(LinkNotInURDFError): - fkCasadi = fk.casadi(q_ca, 'panda_link10', position_only=False) + fkCasadi = fk.casadi(q_ca, "panda_link10", position_only=False) + def test_simpleFk(fk: GenericURDFFk): q_np = np.array([0.0000, 1.0323, 0.0000, 0.8247, 0.0000, 0.2076, 0.0000]) - fkNumpy = fk.numpy(q_np, 'panda_link0', position_only=True) + fkNumpy = fk.numpy(q_np, "panda_link0", position_only=True) x_ee = np.array([0.0, 0.0, 0.0]) assert fkNumpy[0] == pytest.approx(x_ee[0], abs=1e-4) assert fkNumpy[1] == pytest.approx(x_ee[1], abs=1e-4) assert fkNumpy[2] == pytest.approx(x_ee[2], abs=1e-4) - fkNumpy = fk.numpy(q_np, 'panda_link3', position_only=True) + fkNumpy = fk.numpy(q_np, "panda_link3", position_only=True) x_ee = np.array([0.2713, 0.0, 0.4950]) assert fkNumpy[0] == pytest.approx(x_ee[0], abs=1e-4) assert fkNumpy[1] == pytest.approx(x_ee[1], abs=1e-4) assert fkNumpy[2] == pytest.approx(x_ee[2], abs=1e-4) - fkNumpy = fk.numpy(q_np, 'panda_link9', position_only=True) + fkNumpy = fk.numpy(q_np, "panda_link9", position_only=True) x_ee = np.array([0.4, 0.0, 0.71]) assert fkNumpy[0] == pytest.approx(x_ee[0], abs=1e-4) assert fkNumpy[1] == pytest.approx(x_ee[1], abs=1e-4) assert fkNumpy[2] == pytest.approx(x_ee[2], abs=1e-4) + + +def test_symbolic_fk(fk: GenericURDFFk): + q_ca = ca.SX.sym("q", 7) + panda_joint1_z = ca.SX.sym("panda_joint1_z") + panda_joint3_roll = ca.SX.sym("panda_joint3_roll") + symbolic_parameters = { + "panda_joint1": { + "z": panda_joint1_z, + }, + "panda_joint3": {"roll": panda_joint3_roll}, + } + + link_name = "panda_link5" + fkCasadi = fk.casadi( + q_ca, link_name, position_only=False, symbolic_parameters=symbolic_parameters + ) + assert isinstance(fkCasadi, ca.SX) + fk_casadi_function = ca.Function( + "fk", [q_ca, panda_joint1_z, panda_joint3_roll], [fkCasadi] + ) + q_np = np.random.random(7) + z = 0.333 + roll = 1.57079632679 + fk_np = fk.numpy(q_np, link_name, position_only=False) + fk_symbolic_np = fk_casadi_function(q_np, z, roll) + print(fk_np) + print(fk_symbolic_np) + assert np.allclose(fk_np, fk_symbolic_np, atol=1e-4) diff --git a/tests/test_urdfparser.py b/tests/test_urdfparser.py new file mode 100644 index 0000000..cf13923 --- /dev/null +++ b/tests/test_urdfparser.py @@ -0,0 +1,48 @@ +import os + +import casadi as ca +import numpy as np +import pytest + +import forwardkinematics.urdfFks.casadiConversion.urdfparser as u2c + + +@pytest.fixture +def urdfparser() -> u2c.URDFparser: + urdf_file = os.path.dirname(os.path.abspath(__file__)) + "/panda.urdf" + with open(urdf_file, "r") as file: + urdf = file.read() + robot = u2c.URDFparser(root_link="panda_link0", end_links=["panda_leftfinger"]) + robot.from_string(urdf) + robot.detect_link_names() + robot.set_joint_variable_map() + return robot + + +def test_simple_link(urdfparser): + assert isinstance(urdfparser, u2c.URDFparser) + panda_joint1_x = ca.SX.sym("panda_joint1_x") + panda_joint1_roll = ca.SX.sym("panda_joint1_roll") + panda_joint1_yaw = ca.SX.sym("panda_joint1_yaw") + symbolic_parameters = { + "panda_joint1": { + "x": panda_joint1_x, + "roll": panda_joint1_roll, + "yaw": panda_joint1_yaw, + } + } + T = urdfparser.get_forward_kinematics( + "panda_link0", + "panda_link1", + ca.vertcat([0, 0, 0, 0, 0, 0, 0]), + symbolic_parameters=symbolic_parameters, + ) + assert isinstance(T, dict) + T_fk = T["T_fk"] + print(T_fk) + variable_names = [variable.name() for variable in ca.symvar(T_fk)] + assert isinstance(T["T_fk"], ca.SX) + assert "panda_joint1_x" in variable_names + assert "panda_joint1_roll" in variable_names + assert "panda_joint1_yaw" in variable_names + assert T_fk[0, 3] == panda_joint1_x From 1fe8c0040efbae94a4a8b331a8f6f334fdd4a338 Mon Sep 17 00:00:00 2001 From: Max Spahn Date: Fri, 4 Oct 2024 16:14:36 +0200 Subject: [PATCH 2/3] ft[symbols]: Adds posibility to have symbols in fixed joints. --- .../geometry/transformation_matrix.py | 38 +++++++++++++++++++ .../urdfFks/casadiConversion/urdfparser.py | 6 ++- 2 files changed, 43 insertions(+), 1 deletion(-) diff --git a/forwardkinematics/urdfFks/casadiConversion/geometry/transformation_matrix.py b/forwardkinematics/urdfFks/casadiConversion/geometry/transformation_matrix.py index fd9ca97..1a8b94d 100644 --- a/forwardkinematics/urdfFks/casadiConversion/geometry/transformation_matrix.py +++ b/forwardkinematics/urdfFks/casadiConversion/geometry/transformation_matrix.py @@ -3,6 +3,44 @@ import casadi as cs import numpy as np +def fixed(xyz, rpy) -> cs.SX: + """Returns a transformation matrix for a fixed joint.""" + T = cs.SX.zeros(4, 4) + + # Origin rotation from RPY ZYX convention + cr = cs.cos(rpy[0]) + sr = cs.sin(rpy[0]) + cp = cs.cos(rpy[1]) + sp = cs.sin(rpy[1]) + cy = cs.cos(rpy[2]) + sy = cs.sin(rpy[2]) + r00 = cy*cp + r01 = cy*sp*sr - sy*cr + r02 = cy*sp*cr + sy*sr + r10 = sy*cp + r11 = sy*sp*sr + cy*cr + r12 = sy*sp*cr - cy*sr + r20 = -sp + r21 = cp*sr + r22 = cp*cr + + # Homogeneous transformation matrix + T[0, 0] = r00 + T[0, 1] = r01 + T[0, 2] = r02 + T[1, 0] = r10 + T[1, 1] = r11 + T[1, 2] = r12 + T[2, 0] = r20 + T[2, 1] = r21 + T[2, 2] = r22 + T[0, 3] = xyz[0] + T[1, 3] = xyz[1] + T[2, 3] = xyz[2] + T[3, 3] = 1.0 + return T + + def prismatic(xyz, rpy, axis, qi): T = cs.SX.zeros(4, 4) diff --git a/forwardkinematics/urdfFks/casadiConversion/urdfparser.py b/forwardkinematics/urdfFks/casadiConversion/urdfparser.py index 9b6ea1f..d49c6e0 100644 --- a/forwardkinematics/urdfFks/casadiConversion/urdfparser.py +++ b/forwardkinematics/urdfFks/casadiConversion/urdfparser.py @@ -157,7 +157,11 @@ def get_forward_kinematics( xyz = xyzrpy[:3] rpy = xyzrpy[3:] if joint.type == "fixed": - joint_frame = T.numpy_rpy(xyz, *rpy) + # check if xyz contains a ca.SX + if any(isinstance(i, ca.SX) for i in xyz) or any(isinstance(i, ca.SX) for i in rpy): + joint_frame = T.fixed(xyz, rpy) + else: + joint_frame = T.numpy_rpy(xyz, *rpy) T_fk = ca.mtimes(T_fk, joint_frame) elif joint.type == "prismatic": From b19cdf86351c249f8904f985485a638faa560265 Mon Sep 17 00:00:00 2001 From: Max Spahn Date: Thu, 19 Dec 2024 16:38:23 +0100 Subject: [PATCH 3/3] ft[test]: Adds symbolic variable to check if fixed joints work with casadi. --- tests/test_pandaFk.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/tests/test_pandaFk.py b/tests/test_pandaFk.py index e005d8d..774b85f 100644 --- a/tests/test_pandaFk.py +++ b/tests/test_pandaFk.py @@ -107,26 +107,29 @@ def test_symbolic_fk(fk: GenericURDFFk): q_ca = ca.SX.sym("q", 7) panda_joint1_z = ca.SX.sym("panda_joint1_z") panda_joint3_roll = ca.SX.sym("panda_joint3_roll") + panda_joint8_z = ca.SX.sym("panda_joint8_z") symbolic_parameters = { "panda_joint1": { "z": panda_joint1_z, }, "panda_joint3": {"roll": panda_joint3_roll}, + "panda_joint8": {"z": panda_joint8_z}, } - link_name = "panda_link5" + link_name = "panda_link8" fkCasadi = fk.casadi( q_ca, link_name, position_only=False, symbolic_parameters=symbolic_parameters ) assert isinstance(fkCasadi, ca.SX) fk_casadi_function = ca.Function( - "fk", [q_ca, panda_joint1_z, panda_joint3_roll], [fkCasadi] + "fk", [q_ca, panda_joint1_z, panda_joint3_roll, panda_joint8_z], [fkCasadi] ) q_np = np.random.random(7) z = 0.333 roll = 1.57079632679 + z_last = 0.107 fk_np = fk.numpy(q_np, link_name, position_only=False) - fk_symbolic_np = fk_casadi_function(q_np, z, roll) + fk_symbolic_np = fk_casadi_function(q_np, z, roll, z_last) print(fk_np) print(fk_symbolic_np) assert np.allclose(fk_np, fk_symbolic_np, atol=1e-4)