forward_kinematics.py
def HTM(i, theta):
    """Calculate the HTM between two links.

    Args:
        i: A target index of joint value.
        theta: A list of joint value solution. (unit: radian)

    Returns:
        An HTM of Link l w.r.t. Link l-1, where l = i + 1.
    """

    Rot_z = np.matrix(np.identity(4))
    Rot_z[0, 0] = Rot_z[1, 1] = cos(theta[i])
    Rot_z[0, 1] = -sin(theta[i])
    Rot_z[1, 0] = sin(theta[i])

    Trans_z = np.matrix(np.identity(4))
    Trans_z[2, 3] = d[i]

    Trans_x = np.matrix(np.identity(4))
    Trans_x[0, 3] = a[i]

    Rot_x = np.matrix(np.identity(4))
    Rot_x[1, 1] = Rot_x[2, 2] = cos(alpha[i])
    Rot_x[1, 2] = -sin(alpha[i])
    Rot_x[2, 1] = sin(alpha[i])

    A_i = Rot_z * Trans_z * Trans_x * Rot_x

    return A_i
Calculate the HTM between two links.

    Args:
        i: A target index of joint value.
        theta: A list of joint value solution. (unit: radian)

    Returns:
        An HTM of Link l w.r.t. Link l-1, where l = i + 1.
    
Calculate the HTM between two links.
Calculate the HTM between two links .
def HTM(i, theta):
    Rot_z = np.matrix(np.identity(4))
    Rot_z[0, 0] = Rot_z[1, 1] = cos(theta[i])
    Rot_z[0, 1] = -sin(theta[i])
    Rot_z[1, 0] = sin(theta[i])
    Trans_z = np.matrix(np.identity(4))
    Trans_z[2, 3] = d[i]
    Trans_x = np.matrix(np.identity(4))
    Trans_x[0, 3] = a[i]
    Rot_x = np.matrix(np.identity(4))
    Rot_x[1, 1] = Rot_x[2, 2] = cos(alpha[i])
    Rot_x[1, 2] = -sin(alpha[i])
    Rot_x[2, 1] = sin(alpha[i])
    A_i = Rot_z * Trans_z * Trans_x * Rot_x
    return A_i
def HTM ( i , theta ) : Rot_z = np . matrix ( np . identity ( 4 ) ) Rot_z [ 0 , 0 ] = Rot_z [ 1 , 1 ] = cos ( theta [ i ] ) Rot_z [ 0 , 1 ] = - sin ( theta [ i ] ) Rot_z [ 1 , 0 ] = sin ( theta [ i ] ) Trans_z = np . matrix ( np . identity ( 4 ) ) Trans_z [ 2 , 3 ] = d [ i ] Trans_x = np . matrix ( np . identity ( 4 ) ) Trans_x [ 0 , 3 ] = a [ i ] Rot_x = np . matrix ( np . identity ( 4 ) ) Rot_x [ 1 , 1 ] = Rot_x [ 2 , 2 ] = cos ( alpha [ i ] ) Rot_x [ 1 , 2 ] = - sin ( alpha [ i ] ) Rot_x [ 2 , 1 ] = sin ( alpha [ i ] ) A_i = Rot_z * Trans_z * Trans_x * Rot_x return A_i
Calculate the HTM between two links.
Calculate the HTM between two links .
"""Calculate the HTM between two links.

    Args:
        i: A target index of joint value.
        theta: A list of joint value solution. (unit: radian)

    Returns:
        An HTM of Link l w.r.t. Link l-1, where l = i + 1.
    """
An HTM of Link l w.r.t.
An HTM of Link l w . r . t .
i
A target index of joint value.
A target index of joint value .
theta
A list of joint value solution. (unit: radian)
A list of joint value solution . ( unit : radian )