reinstate EE frame
@@ -94,6 +94,7 @@ class RobotPlot():
self.eeframes = []
+ if self.eeframe:
# Axes arrow transforms
Tjx = SE3([0.06, 0, 0])
Tjy = SE3([0, 0.06, 0])
@@ -105,7 +106,7 @@ class RobotPlot():
# add new ee coordinate frame
for link in self.robot.ee_links:
- Te = T[link.number + 1]
+ Te = T[link.number]
# ee axes arrows
Tex = Te * Tjx
@@ -136,11 +137,11 @@ class RobotPlot():
direction = None
if isinstance(self.robot, rp.DHRobot):
# should test MDH I think
- Tj = T[link.number]
+ Tj = T[link.number - 1]
R = Tj.R
direction = R[:, 2] # z direction
elif link.isjoint:
- Tj = T[link.number + 1]
+ Tj = T[link.number]
R = Tj.R
if link.v.axis[1] == 'z':
direction = R[:, 2] # z direction
