teach for ERobot2 allows initial value to be passed

petercorke · petercorke · commit a947bc2d5cbb · 2021-04-08T12:42:33.000+10:00
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -2224,7 +2224,8 @@ def plot(
         
     def teach(
             self, q=None, block=True, limits=None,
-            vellipse=False, fellipse=False, eeframe=True, name=False, backend='pyplot2'):
+            vellipse=False, fellipse=False, eeframe=True, name=False, 
+            unit='rad', backend='pyplot2'):
         """
         2D Graphical teach pendant
 
@@ -2248,6 +2249,8 @@ 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
@@ -2280,6 +2283,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)          
