Unverified Commit 3d6f55b9 authored by Özhan Özen's avatar Özhan Özen Committed by GitHub

Fixes the implementation of `quat_inv()` (#2797)

# Description

Corrects `quat_inv()` in utils/math.py: the inverse is now computed as:

`quat_conjugate(q) / q.pow(2).sum(dim=-1, keepdim=True).clamp(min=eps)`

ensuring correct results for **non-unit** quaternions.  

Fixes #1263 (see discussion for details). Also updated `CHANGELOG.rst`
and corrected a few minor typos within.

## Type of change

<!-- As you go through the list, delete the ones that are not
applicable. -->

- Bug fix (non-breaking change which fixes an issue)
- This change requires a documentation update

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [x] 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

---------
Co-authored-by: 's avatarKelly Guo <kellyg@nvidia.com>
parent 4a7d15db
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.40.10"
version = "0.40.11"
# Description
title = "Isaac Lab framework for Robot Learning"
......
Changelog
---------
0.40.11 (2025-06-27)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added unit test for :func:`~isaaclab.utils.math.quat_inv`.
Fixed
^^^^^
* Fixed the implementation mistake in :func:`~isaaclab.utils.math.quat_inv`.
0.40.10 (2025-06-25)
~~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
* Fixed :meth:`omni.isaac.lab.utils.dict.update_class_from_dict` preventing setting flat Iterables with different lengths.
* Fixed :func:`~isaaclab.utils.dict.update_class_from_dict` preventing setting flat Iterables with different lengths.
0.40.9 (2025-06-25)
......@@ -38,7 +52,7 @@ Fixed
Added
^^^^^
* :class:`NoiseModel` support for manager-based workflows.
* :class:`~isaaclab.utils.noise.NoiseModel` support for manager-based workflows.
Changed
^^^^^^^
......
......@@ -254,16 +254,17 @@ def quat_conjugate(q: torch.Tensor) -> torch.Tensor:
@torch.jit.script
def quat_inv(q: torch.Tensor) -> torch.Tensor:
"""Compute the inverse of a quaternion.
def quat_inv(q: torch.Tensor, eps: float = 1e-9) -> torch.Tensor:
"""Computes the inverse of a quaternion.
Args:
q: The quaternion orientation in (w, x, y, z). Shape is (N, 4).
eps: A small value to avoid division by zero. Defaults to 1e-9.
Returns:
The inverse quaternion in (w, x, y, z). Shape is (N, 4).
"""
return normalize(quat_conjugate(q))
return quat_conjugate(q) / q.pow(2).sum(dim=-1, keepdim=True).clamp(min=eps)
@torch.jit.script
......
......@@ -593,6 +593,35 @@ def test_quat_apply_inverse(device):
torch.testing.assert_close(scipy_result.to(device=device), apply_result, atol=2e-4, rtol=2e-4)
@pytest.mark.parametrize("device", ["cpu", "cuda:0"])
def test_quat_inv(device):
"""Test for quat_inv method.
For random unit and non-unit quaternions q, the Hamilton products
q ⊗ q⁻¹ and q⁻¹ ⊗ q must both equal the identity quaternion (1,0,0,0)
within numerical precision.
"""
num = 2048
# -------- non-unit sample (average ‖q‖ ≈ 10) --------
q_nonunit = torch.randn(num, 4, device=device) * 5.0
# -------- unit sample (‖q‖ = 1) --------
q_unit = torch.randn(num, 4, device=device)
q_unit = q_unit / q_unit.norm(dim=-1, keepdim=True)
identity = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device)
for q in (q_nonunit, q_unit):
q_inv = math_utils.quat_inv(q)
id_batch = identity.expand_as(q)
# left and right products must both be identity
torch.testing.assert_close(math_utils.quat_mul(q, q_inv), id_batch, atol=1e-4, rtol=1e-4)
torch.testing.assert_close(math_utils.quat_mul(q_inv, q), id_batch, atol=1e-4, rtol=1e-4)
def test_quat_apply_benchmarks():
"""Test for quat_apply and quat_apply_inverse methods compared to old methods using torch.bmm and torch.einsum.
The new implementation uses :meth:`torch.einsum` instead of `torch.bmm` which allows
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment