# Description

Added optimizations to the yaw_quat function and added a function to
test_math to make sure it works as intended.

**BENCHMARKS:**

Device: `cpu`

```
Time for yaw_quat: <torch.utils.benchmark.utils.common.Measurement object at 0x7a342d1f8640>
math_utils.yaw_quat(q_rand)  `511.79 us`
1 measurement, 1000 runs , 1 thread

Time for iter_yaw_quat: <torch.utils.benchmark.utils.common.Measurement object at 0x7a342d1f9720>
iter_yaw_quat(q_rand)  `2.18 ms`
1 measurement, 1000 runs , 1 thread

Time for iter_old_yaw_quat: <torch.utils.benchmark.utils.common.Measurement object at 0x7a342d1f8640>
iter_old_yaw_quat(q_rand)  `2.36 ms`
1 measurement, 1000 runs , 1 thread
```

---

Device: `cuda:0`

```
Time for yaw_quat: <torch.utils.benchmark.utils.common.Measurement object at 0x7a342d1f8730>
math_utils.yaw_quat(q_rand)  `246.16 us`
1 measurement, 1000 runs , 1 thread

Time for iter_yaw_quat: <torch.utils.benchmark.utils.common.Measurement object at 0x7a342d1f8100>
iter_yaw_quat(q_rand)  `2.97 ms`
1 measurement, 1000 runs , 1 thread

Time for iter_old_yaw_quat: <torch.utils.benchmark.utils.common.Measurement object at 0x7a342d1f8730>
iter_old_yaw_quat(q_rand)  `3.46 ms`
1 measurement, 1000 runs , 1 thread
```

## Type of change

- New feature (non-breaking change which adds functionality)

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [ ] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there