Fixed example

jhavl · jhavl · commit 770f12a3845e · 2021-01-19T14:54:32.000+10:00
diff --git a/examples/baur.py b/examples/baur.py
@@ -32,17 +32,17 @@
 n = 7
 
 # Set the desired end-effector pose
-Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
+Tep = panda.fkine(panda.q) * sm.SE3(0.3, 0.2, 0.3)
 
 arrived = False
 
 while not arrived:
 
     # The pose of the Panda's end-effector
-    Te = panda.fkine()
+    Te = panda.fkine(panda.q)
 
     # The manipulator Jacobian in the end-effecotr frame
-    Je = panda.jacobe()
+    Je = panda.jacobe(panda.q)
 
     # Calulate the required end-effector spatial velocity for the robot
     # to approach the goal. Gain is set to 1.0
@@ -52,7 +52,7 @@
     Y = 0.01
 
     # The manipulability Jacobian
-    Jm = panda.jacobm()
+    Jm = panda.jacobm(panda.q)
 
     # The minimum angle (in radians) in which the joint is allowed to approach
     # to its limit
diff --git a/examples/mmc.py b/examples/mmc.py
@@ -31,14 +31,14 @@
 n = 7
 
 # Set the desired end-effector pose
-Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
+Tep = panda.fkine(panda.q) * sm.SE3(0.3, 0.2, 0.3)
 
 arrived = False
 
 while not arrived:
 
     # The pose of the Panda's end-effector
-    Te = panda.fkine()
+    Te = panda.fkine(panda.q)
 
     # Transform from the end-effector to desired pose
     eTep = Te.inv() * Tep
@@ -63,7 +63,7 @@
     Q[n:, n:] = (1 / e) * np.eye(6)
 
     # The equality contraints
-    Aeq = np.c_[panda.jacobe(), np.eye(6)]
+    Aeq = np.c_[panda.jacobe(panda.q), np.eye(6)]
     beq = v.reshape((6,))
 
     # The inequality constraints for joint limit avoidance
diff --git a/examples/swifty.py b/examples/swifty.py
@@ -209,11 +209,16 @@ def rand_v():
 
 x = []
 y = []
+rands = 100
+v = np.zeros((rands, 6))
+
+for i in range(rands):
+    v[i, :] = rand_v()
 
 for i in range(10000000):
 
     q = rand_q()
-    v = rand_v()
+    # v = rand_v()
 
     J = r.jacob0(q)
     Jt = J[:3, :]
@@ -222,15 +227,16 @@ def rand_v():
 
     q_n = [10000, 0]
     q_m = [10000, 0]
+    q_mn = 0
 
     # cond = np.linalg.cond(J[:3, :])
     m = r.manipulability(J=J, axes='trans')
     # infn = np.linalg.norm(Jt, 2)
     psi = (np.cbrt(np.linalg.det(Jt @ np.transpose(Jt)))) / \
         (np.trace(Jt @ np.transpose(Jt)) / 3)
 
-    for j in range(1000):
-        qd = np.linalg.pinv(J) @ v
+    for j in range(rands):
+        qd = np.linalg.pinv(J) @ v[j, :]
 
         if np.max(qd) > q_m[1]:
             q_m[1] = np.max(qd)
@@ -243,12 +249,16 @@ def rand_v():
         elif np.linalg.norm(qd) < q_n[0]:
             q_n[0] = np.linalg.norm(qd)
 
+        q_mn += np.linalg.norm(qd)
+
+    q_mn /= rands
+
     # # ax.plot(m, np.log10(cond), 'o', color='black')
     # ax.plot(m, infn, 'o', color='black')
     # # ax.plot(1, 0.002, 'o', color='black')
 
-    x.append(psi)
-    y.append(np.log10(q_m[1]))
+    x.append(m)
+    y.append(np.log10(q_n[0]))
     # y.append(psi)
 
     # ax.plot(m, np.log10(q_m[1]), 'o', color='black')
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -97,7 +97,7 @@ def __init__(
             if len(tool) > 0:
                 elinks.append(ELink(tool, parent=elinks[-1], name="ee"))
         elif isinstance(elinks, list):
-            # were passed a list of ELinks
+            # we're passed a list of ELinks
 
             # check all the incoming ELink objects
             n = 0
@@ -1183,14 +1183,26 @@ def _getlink(self, link, default=None):
         if link is None:
             link = default
 
+        # print(link)
+
+        # for link in self.links:
+        #     print(link)
+
         if isinstance(link, str):
-            if link not in self.link_dict:
-                raise ValueError(f'no link named {link}')
-            return self.link_dict[link]
+            if link in self.link_dict:
+                return self.link_dict[link]
+
+            raise ValueError(f'no link named {link}')
+
         elif isinstance(link, ELink):
-            if link not in self.links:
+            if link in self.links:
+                return link
+            else:
+                for gripper in self.grippers:
+                    if link in gripper.links:
+                        return link
+
                 raise ValueError('link not in robot links')
-            return link
         else:
             raise TypeError('unknown argument')
 
@@ -1614,6 +1626,8 @@ def jacobm(self, q=None, J=None, H=None, endlink=None, startlink=None):
         else:
             verifymatrix(H, (6, n, n))
 
+        print(q)
+
         manipulability = self.manipulability(
             q, J=J, startlink=startlink, endlink=endlink)
         b = np.linalg.inv(J @ np.transpose(J))
