function q = ikine(T, q0, DH_para, cfg1)

Alpha = DH_para.Alpha;
A = DH_para.A;
D = DH_para.D;
Theta = DH_para.Theta;
Sigma = DH_para.Sigma;

a2 = A(2);
a3 = A(3);

px = T(1, 4); py = T(2, 4); pz = T(3, 4); rz = atan2(T(2, 1), T(2, 2));

a0 = norm([px; py]);
q0 = atan2(py, px);

if cfg1 == 0
    q1 = acos((px^2 + py^2 + a2^2 - a3^2) / (2 * a0 * a2)) + q0;
    q2 = -acos((px^2 + py^2 + a3^2 - a2^2) / (2 * a0 * a3)) - q1 + q0;
else
    q1 = -acos((px^2 + py^2 + a2^2 - a3^2) / (2 * a0 * a2)) + q0;
    q2 = acos((px^2 + py^2 + a3^2 - a2^2) / (2 * a0 * a3)) - q1 + q0;
end
q3 = pz;
q4 = rz - q1 - q2;


q = [q1, q2, q3, q4];