ft[symbols]: Adds support for passing parameters as symbols to the forward kinematics.

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)

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,68 @@ 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)
+                # 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":
                 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}

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,61 +28,84 @@ 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()
 
     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():
             raise LinkNotInURDFError(
                 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
-

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 <m.spahn@tudelft.nl>"]
 license = "MIT"