URDFs working

jhavl · jhavl · commit a131f3db1595 · 2021-01-11T15:32:56.000+10:00
diff --git a/roboticstoolbox/backends/PyPlot/RobotPlot.py b/roboticstoolbox/backends/PyPlot/RobotPlot.py
@@ -81,6 +81,7 @@ def axes_calcs(self):
         # Joint axes arrow calcs
         if isinstance(self.robot, rp.ERobot):
             i = 0
+            j = 0
             for link in self.robot.links:
                 loc[:, i + 1] = link._fk.t
 
@@ -94,7 +95,8 @@ def axes_calcs(self):
                     elif link.v.axis == 'Rx' or link.v.axis == 'tx':
                         Tji = link._fk * Tjx
 
-                    joints[:, i] = Tji.t
+                    joints[:, j] = Tji.t
+                    j += 1
 
                 i += 1
             loc = np.c_[loc, loc[:, -1]]
@@ -211,9 +213,14 @@ def init(self):
 
         # Plot joint z coordinates
         if self.jointaxes:
-            for i in range(self.robot.n):
-                self.joints.append(
-                    self._plot_quiver(loc[:, i+1], joints[:, i], '#8FC1E2', 2))
+            j = 0
+            for i in range(len(self.robot.links)):
+                if isinstance(self.robot, rp.DHRobot) or \
+                        self.robot.links[i].isjoint:
+                    self.joints.append(
+                        self._plot_quiver(
+                            loc[:, i+1], joints[:, j], '#8FC1E2', 2))
+                    j += 1
 
         # Plot the shadow of the robot links, draw first so robot is always
         # in front
