Wrong MDH kinematics

[TimWalter]

Hey it seems like your MDH parametrisation:

def modified_dh_to_se3(a: float, alpha: float, d: float, theta: float) -> SE3:
    """Transform a single set of modified DH parameters into an SE3 matrix
    :param a: displacement along x
    :param alpha: rotation about x
    :param d: translation along new z
    :param theta: rotation around new z
    :returns: SE3 matrix
    :rtype: lie.SE3Matrix
    """

    # TransX = SE3(SO3.identity(), np.array([a, 0, 0]))
    # RX = SO3(rotx(alpha))
    # RotX = SE3(RX, np.zeros(3))
    # TransZ = SE3(SO3.identity(), np.array([0, 0, d]))
    # RZ = SO3(rotz(theta))
    # RotZ = SE3(RZ, np.zeros(3))

    TransX = trans_axis(a,"x")
    RotX = rot_axis(alpha,"x")
    TransZ = trans_axis(d,"z")
    RotZ = rot_axis(theta,"z")

    return TransX.dot(RotX.dot(TransZ.dot(RotZ)))

Does not follow the canonical definitions (https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters#Modified_DH_parameters)
the order should be TransZ.dot(RotZ.dot(TransX.dot(RotX))).

Moreover, all transformations are applied to the global axes and not the local coordinate frame.

Suggested fix:

def modified_dh_to_se3(a: float, alpha: float, d: float, theta: float) -> SE3:
    """Transform a single set of modified DH parameters into an SE3 matrix
    :param a: displacement along x
    :param alpha: rotation about x
    :param d: translation along new z
    :param theta: rotation around new z
    :returns: SE3 matrix
    :rtype: lie.SE3Matrix
    """
    ca, sa = np.cos(alpha), np.sin(alpha)
    ct, st = np.cos(theta), np.sin(theta)

    mat = np.array([
        [ct, -st, 0, a],
        [st * ca, ct * ca, -sa, -d * sa],
        [st * sa, ct * sa, ca, d * ca],
        [0, 0, 0, 1]
    ])

    return SE3.from_matrix(mat)```