It is glad to meet you who take up the robot at here.
I had read your article and vi ,i think your problem is at the
algorithm of your robot arm.
the problem you had encountered is redountant control of robot and
there are many method to solve it .for example optimise theory .
u should look up some
Suppose the caculate is correct,it should run no problem.my robot arm
is work well.
i think maybe the initial position of your robot arm is not correct.
the problem should be caused by the diference between math method and
the arm.