fix CheckEndEffectorCollision, have manip ik solution solver start from center of free joints, ikfast fixes for getting more equations