Add body position and orientation task in cost function.