small fix to AngleAxisTemplate<Scalar>::angle() fct

Small fix to adjust when angle is above PI.