Development Guide

Naming Conventions

Lie

• Use the variable name T_dst_src to denote the transformation \(T_{\text{dst}\,\text{src}} \in SE(3)\), which maps a vector \(v_{\text{src}} \in \mathbb{R}^3\) from the source frame to the destination frame. \(v_{\text{dst}} = T_{\text{dst}\,\text{src}}\, v_{\text{src}}\)

• T_dst_src consists of (t_dst_src, q_wxyz_dst_src) (translation followed by rotation).

  • t_dst_src: a 3D vector \((x, y, z)\).

  • q_wxyz_dst_src: a quaternion \((q_w, q_x, q_y, q_z)\).

• Do not use pose, tf, or similar terms; use T_dst_src.

• Do not use Ts_dst_src to indicate batch transformations; use T_dst_src.

• Keep wxyz in parameter names for quaternions to denote the order.

Joint Values

• Use q to denote joint positions.

Formatting, Linting, and Type Checking

• Use ruff for formatting and linting:

  > ruff check
  All checks passed!
  

• Use pyright for type checking:

  > pyright
  0 errors, 0 warnings, 0 informations
  

Testing

Doctest

• Ensure public functions have corresponding doctests:

  def quaternion_multiply(
      q_wxyz_1: Float[torch.Tensor, "... 4"], q_wxyz_2: Float[torch.Tensor, "... 4"]
  ) -> Float[torch.Tensor, "... 4"]:
      """
      Multiply two quaternions.
  
      Example:
          >>> q1 = torch.tensor([0.9238795, 0.0, 0.0, 0.3826834])
          >>> q2 = torch.tensor([0.9238795, 0.0, 0.0, 0.3826834])
          >>> expected = torch.tensor([0.7071068, 0.0, 0.0, 0.7071068])
          >>> torch.allclose(quaternion_multiply(q1, q2), expected, atol=1e-6)
          True
      """
      return standardize_quaternion(quaternion_raw_multiply(q_wxyz_1, q_wxyz_2))
  

Pytest

• Aggregate test cases into a single class, and compare results across different implementations:

  class TestQuaternionToMatrix:
      @pytest.fixture(autouse=True)
      def setup(self):
          """Initialize Warp before each test."""
          wp.init()
  
      def _test_consistency(self, quat_torch: torch.Tensor):
          """Helper function to test consistency between implementations."""
          quat_np = to_numpy(quat_torch)
  
          result_warp = quaternion_to_matrix_warp(quat_torch)
          result_torch = quaternion_to_matrix_torch(quat_torch)
          result_numpy = to_torch(quaternion_to_matrix_numpy(quat_np)).to(result_warp.device)
  
          torch.testing.assert_close(result_warp, result_torch, atol=1e-6, rtol=1e-6)
          torch.testing.assert_close(result_warp, result_numpy, atol=1e-6, rtol=1e-6)
  
          return result_warp
  
      def test_identity_quaternion(self):
          """Test identity quaternion conversion."""
          quat = torch.tensor([1.0, 0.0, 0.0, 0.0])
          result = self._test_consistency(quat)
          expected = torch.eye(3)
          assert result.shape == (3, 3)
          torch.testing.assert_close(result, expected, atol=1e-6, rtol=1e-6)