Added columns for joint limits

petercorke · petercorke · commit 368b889f3e06 · 2020-10-27T21:06:12.000+10:00
displayed if the robot has them
diff --git a/roboticstoolbox/robot/DHRobot.py b/roboticstoolbox/robot/DHRobot.py
@@ -119,38 +119,64 @@ def qs(j, link):
 
         def angle(theta):
             if sym.issymbol(theta):
-                return "<<red>>" + str(L.alpha)
+                return "<<red>>" + str(theta)
             else:
-                str(L.alpha * deg) + "\u00b0"
+                return str(theta * deg) + "\u00b0"
 
+        has_qlim = any([link._qlim is not None for link in self])
+        if has_qlim:
+            qlim_columns = [
+                Column("q⁻", headalign="^"), Column("q⁺", headalign="^"),
+            ]
+            qlim = self.qlim
+
+        else:
+            qlim_columns = []
         if self.mdh:
+            # MDH format
             table = ANSITable(
                 Column("aⱼ₋₁", headalign="^"),
                 Column("⍺ⱼ₋₁", headalign="^"),
                 Column("θⱼ", headalign="^"),
                 Column("dⱼ", headalign="^"),
+                *qlim_columns, 
                 border="thick"
                 )
             for j, L in enumerate(self):
+                if has_qlim:
+                    if L.isprismatic():
+                        ql = [qlim[0, j], qlim[1, j]]
+                    else:
+                        ql = [angle(qlim[k, j]) for k in [0, 1]]
+                else:
+                    ql = []
                 if L.isprismatic():
-                    table.row(L.a, angle(L.alpha), angle(L.theta), qs(j, L))
+                    table.row(L.a, angle(L.alpha), angle(L.theta), qs(j, L), *ql)
                 else:
-                    table.row(L.a, angle(L.alpha), qs(j, L), L.d)
+                    table.row(L.a, angle(L.alpha), qs(j, L), L.d, *ql)
         else:
             # DH format
             table = ANSITable(
                 Column("θⱼ", headalign="^", colalign="<"),
                 Column("dⱼ", headalign="^"),
                 Column("aⱼ", headalign="^"),
                 Column("⍺ⱼ", headalign="^"),
+                *qlim_columns,
                 border="thick"
                 )
             for j, L in enumerate(self):
+                if has_qlim:
+                    if L.isprismatic():
+                        ql = [qlim[0, j], qlim[1, j]]
+                    else:
+                        ql = [angle(qlim[k, j]) for k in [0, 1]]
+                else:
+                    ql = []
                 if L.isprismatic():
                     table.row(
-                        angle(L.theta), qs(j, L), L.a, angle(L.alpha * deg))
+                        angle(L.theta), qs(j, L), L.a, angle(L.alpha), *ql)
                 else:
-                    table.row(qs(j, L), L.d, L.a, angle(L.alpha))
+                    table.row(qs(j, L), L.d, L.a, angle(L.alpha), *ql)
 
         s = str(table)
 
@@ -2515,12 +2541,13 @@ def __init__(self, *args, **kwargs):
     import roboticstoolbox as rtb
     # import spatialmath.base.symbolic as sym
 
-    puma = rtb.models.DH.Planar2()
+    planar = rtb.models.DH.Planar2()
     # J = puma.jacob0(puma.qn)
     # print(J)
     # print(puma.manipulability(puma.qn))
     # print(puma.manipulability(puma.qn, 'asada'))
     #tw, T0 = puma.twists(puma.qz)
-    print(puma.qlim)
-    print(puma[0].qlim)
-    print(puma.islimit([0, -5]))
+    print(planar)
+
+    puma = rtb.models.DH.Puma560()
+    print(puma)
