Added generic robot

jhavl · jhavl · commit 5a78b884c90a · 2021-01-13T19:24:23.000+10:00
diff --git a/examples/plot.py b/examples/plot.py
@@ -18,12 +18,15 @@
 # Plot the joint trajectory with a 50ms delay between configurations
 # panda.plot(q=q, backend='pyplot', dt=0.050, vellipse=True, fellipse=True)
 panda.plot(q=q, backend='pyplot', dt=0.050, vellipse=False, fellipse=False)
-# # q = panda.qr
+
+# r = rp.models.ETS.GenericSeven()
+
+# q = r.qr
 
 # env = rp.backends.PyPlot()
 # env.launch()
 
-# env.add(panda)
-# print(panda)
+# env.add(r)
+# print(r)
 
 # env.hold()
diff --git a/examples/swifty.py b/examples/swifty.py
@@ -8,6 +8,19 @@
 import numpy as np
 import numpy.testing as nt
 
+import matplotlib
+import matplotlib.pyplot as plt
+
+matplotlib.rcParams['pdf.fonttype'] = 42
+matplotlib.rcParams['ps.fonttype'] = 42
+plt.style.use('ggplot')
+matplotlib.rcParams['font.size'] = 4.5
+matplotlib.rcParams['lines.linewidth'] = 0.5
+matplotlib.rcParams['xtick.major.size'] = 1.5
+matplotlib.rcParams['ytick.major.size'] = 1.5
+matplotlib.rcParams['axes.labelpad'] = 1
+plt.rc('grid', linestyle="-", color='#dbdbdb')
+
 # Launch the simulator Swift
 # env = rtb.backends.Swift()
 # env.launch()
@@ -76,15 +89,15 @@
 # env.add(r)
 # env.hold()
 
-r = rtb.models.ETS.Panda()
-r.q = r.qr
+# r = rtb.models.ETS.Panda()
+# r.q = r.qr
 
-q2 = [1, 2, -1, -2, 1, 1, 2]
-Tep = r.fkine(q2)
-Tep = sm.SE3(0.7, 0.2, 0.1) * sm.SE3.OA([0, 1, 0], [0, 0, -1])
+# q2 = [1, 2, -1, -2, 1, 1, 2]
+# Tep = r.fkine(q2)
+# Tep = sm.SE3(0.7, 0.2, 0.1) * sm.SE3.OA([0, 1, 0], [0, 0, -1])
 
-import cProfile
-cProfile.run('qp = r.ikine_mmc(Tep)')
+# import cProfile
+# cProfile.run('qp = r.ikine_mmc(Tep)')
 
 
 
@@ -160,3 +173,64 @@
 
 # import cProfile
 # cProfile.run('runner()')
+
+
+fig, ax = plt.subplots()
+fig.set_size_inches(8, 8)
+
+ax.set(xlabel='Manipulability', ylabel='Condition')
+ax.grid()
+plt.grid(True)
+# ax.set_xlim(xmin=0, xmax=3.1)
+# ax.set_ylim(ymin=0, ymax=0.11)
+
+plt.ion()
+plt.show()
+
+rng = np.random.default_rng(1)
+r = rtb.models.ETS.GenericSeven()
+
+
+def randn(a, b, size):
+    return (b - a) * rng.random(size) + a
+
+
+def rand_q():
+    return randn(np.pi / 2, -np.pi / 2, (7,))
+
+
+def rand_v():
+    # return randn(0, 1, (6,))
+    return np.array([0.1, 0.1, 0.1, 0.0, 0.0, 0.0])
+
+
+for i in range(100000):
+
+    q = rand_q()
+    v = rand_v()
+
+    J = r.jacob0(q)
+    cond = np.linalg.cond(J[:3, :])
+    m = r.manipulability(J=J, axes='trans')
+
+    qd = np.linalg.pinv(J) @ v
+
+    print(np.linalg.norm(np.abs(qd)))
+
+    qd_norm = np.linalg.norm(np.abs(qd))
+
+
+    # if cond > 1000:
+    #     print(np.round(q, 2), np.round(cond), np.round(r.manipulability(q), 3))
+
+    # ax.plot(m, np.log10(cond), 'o', color='black')
+    ax.plot(m, np.log10(qd_norm), 'o', color='black')
+    # ax.plot(1, 0.002, 'o', color='black')
+    plt.pause(0.001)
+
+    # if m > 3 and cond > 30:
+    #     print(np.round(q, 2), np.round(cond), np.round(m, 3))
+
+
+plt.ioff()
+plt.show()
diff --git a/roboticstoolbox/models/ETS/GenericSeven.py b/roboticstoolbox/models/ETS/GenericSeven.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+
+import numpy as np
+from roboticstoolbox.robot.ETS import ETS
+from roboticstoolbox.robot.ERobot import ERobot
+from roboticstoolbox.robot.ELink import ELink
+
+
+class GenericSeven(ERobot):
+    """
+    Create model of a generic seven degree-of-freedom robot
+
+    robot = GenericSeven() creates a robot object. This robot is represented
+    using the elementary transform sequence (ETS).
+
+    """
+    def __init__(self):
+
+        deg = np.pi/180
+        mm = 1e-3
+        tool_offset = (103)*mm
+
+        link_length = 0.5
+
+        l0 = ELink(
+            ETS.tz(link_length) * ETS.rz(),
+            name='link0',
+            parent=None
+        )
+
+        l1 = ELink(
+            ETS.ry(),
+            name='link1',
+            parent=l0
+        )
+
+        l2 = ELink(
+            ETS.tz(link_length) * ETS.rz(),
+            name='link2',
+            parent=l1
+        )
+
+        l3 = ELink(
+            ETS.ry(),
+            name='link3',
+            parent=l2
+        )
+
+        l4 = ELink(
+            ETS.tz(link_length) * ETS.rz(),
+            name='link4',
+            parent=l3
+        )
+
+        l5 = ELink(
+            ETS.ry(),
+            name='link5',
+            parent=l4
+        )
+
+        l6 = ELink(
+            ETS.tx(link_length) * ETS.rz(),
+            name='link6',
+            parent=l5
+        )
+
+        ee = ELink(
+            ETS.tz(-link_length),
+            name='ee',
+            parent=l6
+        )
+
+        elinks = [l0, l1, l2, l3, l4, l5, l6, ee]
+        # elinks = [l0, l1, l2, l3, l4, l5, ee]
+
+        super(GenericSeven, self).__init__(
+            elinks,
+            name='Generic Seven',
+            manufacturer="Jesse's Imagination")
+
+        # self.addconfiguration(
+        #     "qz", np.array([0, 0, 0, 0, 0, 0, 0]))
+        # self.addconfiguration(
+        #     "qr", np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi/4]))
diff --git a/roboticstoolbox/models/ETS/__init__.py b/roboticstoolbox/models/ETS/__init__.py
@@ -2,11 +2,13 @@
 from roboticstoolbox.models.ETS.Frankie import Frankie
 from roboticstoolbox.models.ETS.Puma560 import Puma560
 from roboticstoolbox.models.ETS.Planar_Y import Planar_Y
+from roboticstoolbox.models.ETS.GenericSeven import GenericSeven
 
 
 __all__ = [
     'Panda',
     'Frankie',
     'Puma560',
     'Planar_Y',
+    'GenericSeven'
 ]
