Adjust for str parent

jhavl · jhavl · commit 95ffb01d8e0e · 2021-04-13T09:07:53.000+10:00
diff --git a/roboticstoolbox/robot/ELink.py b/roboticstoolbox/robot/ELink.py
@@ -267,7 +267,7 @@ def ets(self):
         :rtype: ETS or ETS2 instance
 
         The sequence:
-        
+
             - has at least one element
             - may include zero or more constant transforms
             - no more than one variable transform, which if present will
@@ -453,6 +453,10 @@ def _get_fknm(self):
         return isflip, axis, jindex, parent, shape_base, shape_wT, shape_sT
 
     def _init_fknm(self):
+        if isinstance(self.parent, str):
+            # Initialise later
+            return
+
         isflip, axis, jindex, parent, \
             shape_base, shape_wT, shape_sT = self._get_fknm()
 
@@ -467,6 +471,7 @@ def _update_fknm(self):
         # Check if not initialized yet
         try:
             if self._fknm is None:
+                self._init_fknm()
                 return
         except AttributeError:
             return
@@ -751,5 +756,5 @@ def A(self, q=0.0, **kwargs):
             return SE2(T, check=False)
 
 
-l0 = ELink(qlim=[-1, 1])
-print(l0.qlim)
+# l0 = ELink(qlim=[-1, 1])
+# print(l0.qlim)
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -146,6 +146,9 @@ def __init__(
         for link in links:
             if isinstance(link.parent, str):
                 link._parent = self._linkdict[link.parent]
+                # Update the fast kinematics object
+                if isinstance(self, ERobot):
+                    link._init_fknm()
 
         if all([link.parent is None for link in links]):
             # no parent links were given, assume they are sequential
@@ -1039,6 +1042,8 @@ def URDF(cls, file_path, gripper=None):
 
         return cls(links, name=name, gripper_links=gripper)
 
+# --------------------------------------------------------------------- #
+
     def _reset_cache(self):
         self._path_cache = {}
         self._path_cache_fknm = {}
@@ -1080,9 +1085,9 @@ def _reset_cache(self):
     #     path.reverse()
     #     return path
 
-    def to_dict(self, show_robot=True, show_collision=False):
+    def _to_dict(self, show_robot=True, show_collision=False):
 
-        self.fkine_links(self.q)
+        self._set_link_fk(self.q)
 
         ob = []
 
@@ -1108,7 +1113,7 @@ def to_dict(self, show_robot=True, show_collision=False):
 
         return ob
 
-    def fk_dict(self, show_robot=True, show_collision=False):
+    def _fk_dict(self, show_robot=True, show_collision=False):
         ob = []
 
         # Do the robot
@@ -1133,6 +1138,47 @@ def fk_dict(self, show_robot=True, show_collision=False):
 
         return ob
 
+    def _set_link_fk(self, q):
+        '''
+        robot._set_link_fk(q) evaluates fkine for each link within a
+        robot and stores that pose in a private variable within the link.
+
+        This method is not for general use.
+
+        :param q: The joint angles/configuration of the robot
+        :type q: float ndarray(n)
+
+        .. note::
+
+            - The robot's base transform, if present, are incorporated
+              into the result.
+
+        :references:
+            - Kinematic Derivatives using the Elementary Transform
+              Sequence, J. Haviland and P. Corke
+
+        '''
+
+        if self._base is None:
+            base = self._eye_fknm
+        else:
+            base = self._base.A
+
+        fknm.fkine_all(
+            self._cache_m,
+            self._cache_links_fknm,
+            q,
+            base)
+
+        for i in range(len(self._cache_grippers)):
+            fknm.fkine_all(
+                len(self._cache_grippers[i]),
+                self._cache_grippers[i],
+                self.grippers[i].q,
+                base)
+
+# --------------------------------------------------------------------- #
+
     @staticmethod
     def URDF_read(file_path, tld=None):
         """
@@ -1279,38 +1325,6 @@ def fkine(
 
         return T
 
-    # def fkine_links(self, q):
-    #     '''
-    #     robot.fkine_links(q) evaluates fkine for each link within a
-    #     robot and stores that pose within the link.
-
-    #     :param q: The joint angles/configuration of the robot
-    #     :type q: float ndarray(n)
-
-    #     .. note::
-
-    #         - The robot's base transform, if present, are incorporated
-    #           into the result.
-
-    #     :references:
-    #         - Kinematic Derivatives using the Elementary Transform
-    #           Sequence, J. Haviland and P. Corke
-
-    #     '''
-
-    #     fknm.fkine_all(
-    #         self._cache_m,
-    #         self._cache_links_fknm,
-    #         q,
-    #         self._base.A)
-
-    #     for i in range(len(self._cache_grippers)):
-    #         fknm.fkine_all(
-    #             len(self._cache_grippers[i]),
-    #             self._cache_grippers[i],
-    #             self.grippers[i].q,
-    #             self._base.A)
-
     def get_path(self, end=None, start=None, _fknm=False):
         """
         Find a path from start to end. The end must come after
