Merge branch 'master' of github.com:petercorke/robotics-toolbox-python

jhavl · jhavl · commit 5c071d093a89 · 2021-04-08T13:55:34.000+10:00
diff --git a/roboticstoolbox/robot/Dynamics.py b/roboticstoolbox/robot/Dynamics.py
@@ -49,11 +49,12 @@ def gravity(self, gravity_new):
         self.dynchanged()
 
 # --------------------------------------------------------------------- #
-    def dyntable(self):
+    def dynamics(self):
         """
         Pretty print the dynamic parameters (Robot superclass)
 
-        The dynamic parameters are printed in a table, with one row per link.
+        The dynamic parameters (inertial and friction) are printed in a table,
+        with one row per link.
 
         Example:
 
@@ -78,9 +79,9 @@ def dyntable(self):
             table.row(link.name, *link._dyn2list())
         print(table)
 
-    def printdyn(self):
+    def dynamics_list(self):
         """
-        Print dynamic parameters
+        Print dynamic parameters (Robot superclass)
 
         Display the kinematic and dynamic parameters to the console in
         reable format
diff --git a/roboticstoolbox/robot/ELink.py b/roboticstoolbox/robot/ELink.py
@@ -13,16 +13,24 @@
 
 class BaseELink(Link):
 
-    def __init__(self, name=None, joint_name=None):
+    def __init__(self, name=None, parent=None, joint_name=None):
 
         super().__init__()
 
         self._name = name
+
+        if parent is not None:
+            if isinstance(parent, (str, BaseELink)):
+                self._parent = parent
+            else:
+                raise ValueError('parent must be BaseELink subclass or str')
+        else:
+            self._parent = None
+
         self._joint_name = joint_name
 
         self._jindex = None
         self._children = []
-        self._parent = None
 
     def __repr__(self):
         name = self.__class__.__name__
@@ -327,7 +335,6 @@ def __init__(
             ets=ETS(),
             v=None,
             jindex=None,
-            parent=None,
             **kwargs):
 
         # process common options
@@ -347,14 +354,6 @@ def __init__(
             elif jindex is None and v.jindex is not None:
                 jindex = v.jindex
 
-        # TODO simplify this logic, can be ELink class or None
-        # if isinstance(parent, list):
-        #     raise TypeError(
-        #         'Only one parent link can be present')
-        if not isinstance(parent, (ELink, str)) and parent is not None:
-            raise TypeError(
-                'Parent must be of type ELink, str or None')
-
         # Initialise the static transform representing the constant
         # component of the ETS
         self._init_Ts()
@@ -624,7 +623,6 @@ def __init__(
             ets=ETS2(),
             v=None,
             jindex=None,
-            parent=None,
             **kwargs):
 
         # process common options
@@ -644,14 +642,6 @@ def __init__(
             elif jindex is None and v.jindex is not None:
                 jindex = v.jindex
 
-        # TODO simplify this logic, can be ELink class or None
-        # if isinstance(parent, list):
-        #     raise TypeError(
-        #         'Only one parent link can be present')
-        if not isinstance(parent, (ELink2, str)) and parent is not None:
-            raise TypeError(
-                'Parent must be of type ELink, str or None')
-
         # Initialise the static transform representing the constant
         # component of the ETS
         self._init_Ts()
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -311,8 +311,11 @@ def __str__(self):
                 parent_name,
                 f"{{{link.name}}} = {{{parent_name}}}{s}"
             )
-
-        s = f"{self.__class__.__name__}: {self.name}"
+        if isinstance(self, ERobot):
+            classname = 'ERobot'
+        elif isinstance(self, ERobot2):
+            classname = 'ERobot2'
+        s = f"{classname}: {self.name}"
         if self.manufacturer is not None and len(self.manufacturer) > 0:
             s += f" (by {self.manufacturer})"
         s += f", {self.n} joints ({self.structure})"
@@ -872,22 +875,13 @@ def __init__(self, arg, **kwargs):
             # we're passed an ETS string
             ets = arg
             links = []
-
             # chop it up into segments, a link frame after every joint
-            start = 0
-            for j, k in enumerate(ets.joints()):
-                ets_j = ets[start:k + 1]
-                start = k + 1
-                if j == 0:
-                    parent = None
-                else:
-                    parent = links[-1]
+            parent = None
+            for j, ets_j in enumerate(arg.split()):
                 elink = ELink(ets_j, parent=parent, name=f"link{j:d}")
+                parent = elink
                 links.append(elink)
-            tail = arg[start:]
-            if len(tail) > 0:
-                elink = ELink(tail, parent=links[-1], name=f"link{j+1:d}")
-                links.append(elink)
+            
         elif islistof(arg, ELink):
             links = arg
         else:
@@ -1063,7 +1057,7 @@ def URDF_read(file_path, tld=None):
 # --------------------------------------------------------------------- #
 
     def fkine(
-            self, q, end=None, start=None, tool=None,
+            self, q, unit='rad', end=None, start=None, tool=None,
             include_base=True, fast=False):
         '''
         Forward kinematics
@@ -1132,7 +1126,7 @@ def fkine(
         T = SE3.Empty()
 
         for k, qk in enumerate(q):
-
+            qk = self.toradians(qk)
             link = end  # start with last link
 
             # add tool if provided
@@ -1980,22 +1974,13 @@ def __init__(self, arg, **kwargs):
             # we're passed an ETS string
             ets = arg
             links = []
-
             # chop it up into segments, a link frame after every joint
-            start = 0
-            for j, k in enumerate(ets.joints()):
-                ets_j = ets[start:k + 1]
-                start = k + 1
-                if j == 0:
-                    parent = None
-                else:
-                    parent = links[-1]
+            parent = None
+            for j, ets_j in enumerate(arg.split()):
                 elink = ELink2(ets_j, parent=parent, name=f"link{j:d}")
+                parent = elink
                 links.append(elink)
-            tail = arg[start:]
-            if len(tail) > 0:
-                elink = ELink2(tail, parent=links[-1], name=f"link{j+1:d}")
-                links.append(elink)
+
         elif islistof(arg, ELink2):
             links = arg
         else:
@@ -2013,8 +1998,9 @@ def jacob0(self, q):
     def jacobe(self, q):
         return self.ets().jacobe(q)
 
-    def fkine(self, q):
-        return self.ets().eval(q)
+    def fkine(self, q, unit='rad'):
+
+        return self.ets().eval(q, unit=unit)
 # --------------------------------------------------------------------- #
 
     def plot(
@@ -2108,6 +2094,7 @@ def teach(
             fellipse=False,
             eeframe=True,
             name=False,
+            unit='rad',
             backend='pyplot2'):
         """
         2D Graphical teach pendant
@@ -2131,6 +2118,9 @@ def teach(
         :type eeframe: bool
         :param name: (Plot Option) Plot the name of the robot near its base
         :type name: bool
+        :param unit: angular units: 'rad' [default], or 'deg'
+        :type unit: str
+
         :return: A reference to the PyPlot object which controls the
             matplotlib figure
         :rtype: PyPlot
@@ -2158,6 +2148,11 @@ def teach(
 
         if q is None:
             q = np.zeros((self.n,))
+        else:
+            q = getvector(q, self.n)
+
+        if unit == 'deg':
+            q = self.toradians(q)
 
         # Make an empty 3D figure
         env = self._get_graphical_backend(backend)
diff --git a/roboticstoolbox/robot/ETS.py b/roboticstoolbox/robot/ETS.py
@@ -13,15 +13,14 @@
     issymbol, tr2jac, transl2, trot2, removesmall, trinv, \
     verifymatrix, iseye, tr2jac2
 
-
 class BaseETS(UserList, ABC):
 
     # T is a NumPy array (4,4) or None
     # ets_tuple = namedtuple('ETS3', 'eta axis_func axis joint T jindex flip')
 
     def __init__(
             self, axis=None, eta=None, axis_func=None,
-            unit='rad', j=None, flip=False):
+            unit='rad', j=None, flip=False, qlim=None):
         """
         Elementary transform sequence (superclass)
 
@@ -148,7 +147,7 @@ def __init__(
         # Save all the params in a named tuple
         e = SimpleNamespace(
             eta=eta, axis_func=axis_func,
-            axis=axis, joint=joint, T=T, jindex=j, flip=flip)
+            axis=axis, joint=joint, T=T, jindex=j, flip=flip, qlim=qlim)
 
         # And make it the only value of this instance
         self.data = [e]
@@ -397,6 +396,10 @@ def structure(self):
         return ''.join(
             ['R' if self.isrevolute else 'P' for i in self.joints()])
 
+    @property
+    def qlim(self):
+        return self.data[0].qlim
+
     def joints(self):
         """
         Get index of joint transforms
@@ -527,6 +530,25 @@ def eval(self, q=None, unit='rad'):
 
         return T
 
+    def split(self):
+        """
+        Split ETS into link segments
+
+        Returns a list of ETS, each one, apart from the last,
+        ends with a variable ET.
+        """
+        segments = []
+        start = 0
+        for j, k in enumerate(self.joints()):
+            ets_j = self[start:k + 1]
+            start = k + 1
+            segments.append(ets_j)
+        tail = self[start:]
+        if len(tail) > 0:
+            segments.append(tail)
+        
+        return segments
+
     def compile(self):
         """
         Compile an ETS
@@ -907,6 +929,25 @@ def inv(self):
             inv *= et
         return inv
 
+    def plot(self, *args, **kwargs):
+        from roboticstoolbox.robot.ERobot import ERobot, ERobot2
+
+        if isinstance(self, ETS):
+            robot = ERobot(self)
+        else:
+            robot = ERobot2(self)
+
+        robot.plot(*args, **kwargs)
+
+    def teach(self, *args, **kwargs):
+        from roboticstoolbox.robot.ERobot import ERobot, ERobot2
+
+        if isinstance(self, ETS):
+            robot = ERobot(self)
+        else:
+            robot = ERobot2(self)
+
+        robot.teach(*args, **kwargs)
 
 class ETS(BaseETS):
     """
diff --git a/roboticstoolbox/robot/Link.py b/roboticstoolbox/robot/Link.py
@@ -130,15 +130,16 @@ def dynpar(self, name, value, default):
                 return 1
 
         dynchange = 0
+        # link inertial parameters
         dynchange += dynpar(self, 'm', m, 0.0)
         dynchange += dynpar(self, 'r', r, np.zeros((3,)))
         dynchange += dynpar(self, 'I', I, np.zeros((3, 3)))
 
-        # Motor dynamic parameters
+        # Motor inertial and frictional parameters
         dynchange += dynpar(self, 'Jm', Jm, 0.0)
         dynchange += dynpar(self, 'B', B, 0.0)
         dynchange += dynpar(self, 'Tc', Tc, np.zeros((2,)))
-        dynchange += dynpar(self, 'G', G, 1.0)
+        dynchange += dynpar(self, 'G', G, 0.0)
 
         self.actuator = None  # reference to more advanced actuator model
 
diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py
