Capsule casting is working. Need to make obstacle avoidance behave properly.