DLS_ik inverse kinematics based on damped least square DH parameters needed. It works. Not very fast though. Can be optimized, but I'm too lazy to do it.