forward_kinematics.py
def fwd_kin(theta, i_unit='r', o_unit='n'):
    """Solve the HTM based on a list of joint values.

    Args:
        theta: A list of joint values. (unit: radian)
        i_unit: Output format. 'r' for radian; 'd' for degree.
        o_unit: Output format. 'n' for np.array; 'p' for ROS Pose.

    Returns:
        The HTM of end-effector joint w.r.t. base joint
    """

    T_06 = np.matrix(np.identity(4))

    if i_unit == 'd':
        theta = [radians(i) for i in theta]

    for i in range(6):
        T_06 *= HTM(i, theta)

    if o_unit == 'n':
        return T_06
    elif o_unit == 'p':
        return np2ros(T_06)
Solve the HTM based on a list of joint values.

    Args:
        theta: A list of joint values. (unit: radian)
        i_unit: Output format. 'r' for radian; 'd' for degree.
        o_unit: Output format. 'n' for np.array; 'p' for ROS Pose.

    Returns:
        The HTM of end-effector joint w.r.t. base joint
    
Solve the HTM based on a list of joint values.
Solve the HTM based on a list of joint values .
def fwd_kin(theta, i_unit='r', o_unit='n'):
    T_06 = np.matrix(np.identity(4))
    if i_unit == 'd':
        theta = [radians(i) for i in theta]
    for i in range(6):
        T_06 *= HTM(i, theta)
    if o_unit == 'n':
        return T_06
    elif o_unit == 'p':
        return np2ros(T_06)
def fwd_kin ( theta , i_unit = 'r' , o_unit = 'n' ) : T_06 = np . matrix ( np . identity ( 4 ) ) if i_unit == 'd' : theta = [ radians ( i ) for i in theta ] for i in range ( 6 ) : T_06 *= HTM ( i , theta ) if o_unit == 'n' : return T_06 elif o_unit == 'p' : return np2ros ( T_06 )
Solve the HTM based on a list of joint values.
Solve the HTM based on a list of joint values .
"""Solve the HTM based on a list of joint values.

    Args:
        theta: A list of joint values. (unit: radian)
        i_unit: Output format. 'r' for radian; 'd' for degree.
        o_unit: Output format. 'n' for np.array; 'p' for ROS Pose.

    Returns:
        The HTM of end-effector joint w.r.t. base joint
    """
The HTM of end-effector joint w.r.t. base joint
The HTM of end - effector joint w . r . t . base joint
theta
A list of joint values. (unit: radian)
A list of joint values . ( unit : radian )
i_unit
Output format. 'r' for radian; 'd' for degree.
Output format . ' r ' for radian ; ' d ' for degree .
o_unit
Output format. 'n' for np.array; 'p' for ROS Pose.
Output format . ' n ' for np . array ; ' p ' for ROS Pose .