attempt to move jacobm upto Robot

petercorke · petercorke · commit ea3dbc5a7ab1 · 2021-04-07T15:15:37.000+10:00
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -1878,180 +1878,9 @@ def cross(a, b):
 
         return H
 
-    def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes='all'):
-        r"""
-        Calculates the manipulability Jacobian. This measure relates the rate
-        of change of the manipulability to the joint velocities of the robot.
-        One of J or q is required. Supply J and H if already calculated to
-        save computation time
-
-        :param q: The joint angles/configuration of the robot (Optional,
-            if not supplied will use the stored q values).
-        :type q: float ndarray(n)
-        :param J: The manipulator Jacobian in any frame
-        :type J: float ndarray(6,n)
-        :param H: The manipulator Hessian in any frame
-        :type H: float ndarray(6,n,n)
-        :param end: the final link or Gripper which the Hessian represents
-        :type end: str or ELink or Gripper
-        :param start: the first link which the Hessian represents
-        :type start: str or ELink
-
-        :return: The manipulability Jacobian
-        :rtype: float ndarray(n)
-
-        Yoshikawa's manipulability measure
-
-        .. math::
-
-            m(\vec{q}) = \sqrt{\mat{J}(\vec{q}) \mat{J}(\vec{q})^T}
-
-        This method returns its Jacobian with respect to configuration
-
-        .. math::
-
-            \frac{\partial m(\vec{q})}{\partial \vec{q}}
-
-        :references:
-            - Kinematic Derivatives using the Elementary Transform
-              Sequence, J. Haviland and P. Corke
-        """
-
-        end, start, _ = self._get_limit_links(end, start)
-        path, n, _ = self.get_path(end, start)
-
-        if axes == 'all':
-            axes = [True, True, True, True, True, True]
-        elif axes.startswith('trans'):
-            axes = [True, True, True, False, False, False]
-        elif axes.startswith('rot'):
-            axes = [False, False, False, True, True, True]
-        else:
-            raise ValueError('axes must be all, trans or rot')
-
-        if J is None:
-            if q is None:
-                q = np.copy(self.q)
-            else:
-                q = getvector(q, self.n)
-
-            J = self.jacob0(q, start=start, end=end)
-        else:
-            verifymatrix(J, (6, n))
-
-        if H is None:
-            H = self.hessian0(J0=J, start=start, end=end)
-        else:
-            verifymatrix(H, (6, n, n))
-
-        manipulability = self.manipulability(
-            q, J=J, start=start, end=end, axes=axes)
-
-        J = J[axes, :]
-        H = H[axes, :, :]
-
-        b = np.linalg.inv(J @ np.transpose(J))
-        Jm = np.zeros((n, 1))
-
-        for i in range(n):
-            c = J @ np.transpose(H[:, :, i])
-            Jm[i, 0] = manipulability * \
-                np.transpose(c.flatten('F')) @ b.flatten('F')
-
-        return Jm
-
-    def __str__(self):
-        """
-        Pretty prints the ETS Model of the robot.
-
-        :return: Pretty print of the robot model
-        :rtype: str
-
-        .. note::
-            - Constant links are shown in blue.
-            - End-effector links are prefixed with an @
-            - Angles in degrees
-            - The robot base frame is denoted as ``BASE`` and is equal to the
-              robot's ``base`` attribute.
-        """
-        table = ANSITable(
-            Column("id", headalign="^", colalign=">"),
-            Column("link", headalign="^", colalign="<"),
-            Column("joint", headalign="^", colalign=">"),
-            Column("parent", headalign="^", colalign="<"),
-            Column("ETS", headalign="^", colalign="<"),
-            border="thin")
-        for k, link in enumerate(self):
-            color = "" if link.isjoint else "<<blue>>"
-            ee = "@" if link in self.ee_links else ""
-            ets = link.ets()
-            if link.parent is None:
-                parent_name = "BASE"
-            else:
-                parent_name = link.parent.name
-            s = ets.__str__(f"q{link._jindex}")
-            if len(s) > 0:
-                s = " \u2295 " + s
-
-            if link.isjoint:
-                if link._joint_name is not None:
-                    jname = link._joint_name
-                else:
-                    jname = link.jindex
-            else:
-                jname = ''
-            table.row(
-                k,
-                color + ee + link.name,
-                jname,
-                parent_name,
-                f"{{{link.name}}} = {{{parent_name}}}{s}"
-            )
-
-        s = self.name
-        if self.manufacturer is None or len(self.manufacturer) == 0:
-            s += f" (by {self.manufacturer})"
-        s += f", {self.n} axes ({self.structure})"
-        if self.nbranches > 1:
-            s += f", {self.nbranches} branches"
-        if self._hasdynamics:
-            s += ", dynamics"
-        if any([len(link.geometry) > 0 for link in self.links]):
-            s += ", geometry"
-        if any([len(link.collision) > 0 for link in self.links]):
-            s += ", collision"
-        s += f", ETS model\n"
-
-        s += str(table)
-        s += self.configurations_str()
-
-        return s
-
-    def hierarchy(self):
-        """
-        Pretty print the robot link hierachy
-
-        :return: Pretty print of the robot model
-        :rtype: str
-
-        Example:
-
-        .. runblock:: pycon
-
-            >>> import roboticstoolbox as rtb
-            >>> robot = rtb.models.URDF.Panda()
-            >>> robot.hierarchy()
-
-        """
 
-        # link = self.base_link
 
-        def recurse(link, indent=0):
-            print(' ' * indent * 2, link.name)
-            for child in link.child:
-                recurse(child, indent + 1)
 
-        recurse(self.base_link)
 
     def jacobev(
             self, q, end=None, start=None,
diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py
@@ -2,7 +2,7 @@
 import copy
 import numpy as np
 import roboticstoolbox as rtb
-from spatialmath import SE3
+from spatialmath import SE3, SE2
 from spatialmath.base.argcheck import isvector, getvector, getmatrix, \
     getunit
 from roboticstoolbox.robot.Link import Link
@@ -632,6 +632,88 @@ def jacob_dot(self, q=None, qd=None, J0=None):
             Jd += H[:, :, i] * qd[i]
 
         return Jd
+
+    def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes='all'):
+        r"""
+        Calculates the manipulability Jacobian. This measure relates the rate
+        of change of the manipulability to the joint velocities of the robot.
+        One of J or q is required. Supply J and H if already calculated to
+        save computation time
+
+        :param q: The joint angles/configuration of the robot (Optional,
+            if not supplied will use the stored q values).
+        :type q: float ndarray(n)
+        :param J: The manipulator Jacobian in any frame
+        :type J: float ndarray(6,n)
+        :param H: The manipulator Hessian in any frame
+        :type H: float ndarray(6,n,n)
+        :param end: the final link or Gripper which the Hessian represents
+        :type end: str or ELink or Gripper
+        :param start: the first link which the Hessian represents
+        :type start: str or ELink
+
+        :return: The manipulability Jacobian
+        :rtype: float ndarray(n)
+
+        Yoshikawa's manipulability measure
+
+        .. math::
+
+            m(\vec{q}) = \sqrt{\mat{J}(\vec{q}) \mat{J}(\vec{q})^T}
+
+        This method returns its Jacobian with respect to configuration
+
+        .. math::
+
+            \frac{\partial m(\vec{q})}{\partial \vec{q}}
+
+        :references:
+            - Kinematic Derivatives using the Elementary Transform
+              Sequence, J. Haviland and P. Corke
+        """
+
+        end, start, _ = self._get_limit_links(end, start)
+        # path, n, _ = self.get_path(end, start)
+
+        if axes == 'all':
+            axes = [True, True, True, True, True, True]
+        elif axes.startswith('trans'):
+            axes = [True, True, True, False, False, False]
+        elif axes.startswith('rot'):
+            axes = [False, False, False, True, True, True]
+        else:
+            raise ValueError('axes must be all, trans or rot')
+
+        if J is None:
+            if q is None:
+                q = np.copy(self.q)
+            else:
+                q = getvector(q, self.n)
+
+            J = self.jacob0(q, start=start, end=end)
+        else:
+            verifymatrix(J, (6, n))
+
+        if H is None:
+            H = self.hessian0(J0=J, start=start, end=end)
+        else:
+            verifymatrix(H, (6, n, n))
+
+        manipulability = self.manipulability(
+            q, J=J, start=start, end=end, axes=axes)
+
+        J = J[axes, :]
+        H = H[axes, :, :]
+
+        b = np.linalg.inv(J @ np.transpose(J))
+        Jm = np.zeros((n, 1))
+
+        for i in range(n):
+            c = J @ np.transpose(H[:, :, i])
+            Jm[i, 0] = manipulability * \
+                np.transpose(c.flatten('F')) @ b.flatten('F')
+
+        return Jm
 # --------------------------------------------------------------------- #
 
     @property
@@ -719,12 +801,23 @@ def base(self):
     def base(self, T):
         # if not isinstance(T, SE3):
         #     T = SE3(T)
-        if T is None or isinstance(T, SE3):
+        if T is None:
             self._base = T
-        elif SE3.isvalid(T):
-            self._tool = SE3(T, check=False)
+        elif isinstance(self, rtb.ERobot2):
+            # 2D robot
+            if isinstance(T, SE2):
+                self._base = T
+            elif SE2.isvalid(T):
+                self._tool = SE2(T, check=True)
+        elif isinstance(self, rtb.Robot):
+            # all other 3D robots
+            if isinstance(T, SE3):
+                self._base = T
+            elif SE3.isvalid(T):
+                self._tool = SE3(T, check=True)
+
         else:
-            raise ValueError('base must be set to None (no tool) or an SE3')
+            raise ValueError('base must be set to None (no tool), SE2, or SE3')
 # --------------------------------------------------------------------- #
 
     @property
