fixed (1) setting of redundant joints, (2) checking when ik has succeeded (3) added some more debug