fix check end effector collision when there is redundancy