diff --git a/roboticstoolbox/robot/DHRobot.py b/roboticstoolbox/robot/DHRobot.py index 858647bb2..b924349d5 100644 --- a/roboticstoolbox/robot/DHRobot.py +++ b/roboticstoolbox/robot/DHRobot.py @@ -1465,7 +1465,7 @@ def removesmall(x): # revolute axis w_ = Rt @ w + z0 * qd_k[j] wd_ = Rt @ wd + z0 * qdd_k[j] + _cross(Rt @ w, z0 * qd_k[j]) - vd_ = Rt @ _cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd + vd_ = Rt @ (_cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd) else: # prismatic axis w_ = Rt @ w