function T_final = fk_urdf(joints, q)
    N = size(joints,1);
    T_final = eye(4);
    for i = 1:N
        x = joints(i,1); y = joints(i,2); z = joints(i,3);
        r = joints(i,4); p = joints(i,5); yv = joints(i,6);
        T_origin = HT(x,y,z) * HRZ(yv) * HRY(p) * HRX(r);
        T_joint = HRZ(q(i)); 
        T_final = T_final * T_origin * T_joint;
    end
end


