AdoHaha
MDH to urdf does not work for alpha != 0 and th != 0 at the same time
Thanks for publishing this :). I think I found a small mistake: If alpha and th are non-zero, the resulting urdf joint transform is T_urdf = T(a, dy, dz)*R_z(th)*R_x(alpha)*R_z(q) while the mdh transform actually is T_mdh = R_x(alpha)*T_x(a)*T_z(d)*R_z(th + q) = T(a, dy, dz)*R_x(alpha)*R_z(th + q) != T_urdf. This is because the rpy transformation is R_z(r)*R_y(p)*R_x(y) switching the order of rotations. In such cases, additional joints and links are required as done for the dh parameters.
I found a solution that requires only one line in that case at the cost of some calculation (something similar is probably possible for the dh-case):
keep a, dy, dz, calculate:
r = atan2(cos(theta)*sin(alpha), cos(alpha))
y = atan2(cos(alpha)*sin(theta), cos(theta))
if (abs(sin(r))>abs(cos(r))) {
p = atan2(-sin(alpha)*sin(theta), cos(theta)*sin(alpha)/sin(r))
} else {
p = atan2(-sin(alpha)*sin(theta), cos(alpha)/cos(r))
}
and set origin as: