mink 0.0.9__tar.gz → 0.0.10__tar.gz

{mink-0.0.9 → mink-0.0.10}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: mink
-Version: 0.0.9
+Version: 0.0.10
 Summary: mink: MuJoCo inverse kinematics.
 Keywords: inverse,kinematics,mujoco
 Author-email: Kevin Zakka <zakka@berkeley.edu>

{mink-0.0.9 → mink-0.0.10}/mink/lie/so3.py

@@ -85,6 +85,7 @@ class SO3(MatrixLieGroup):
         assert matrix.shape == (SO3.matrix_dim, SO3.matrix_dim)
         wxyz = np.empty(SO3.parameters_dim, dtype=np.float64)
         mujoco.mju_mat2Quat(wxyz, matrix.ravel())
+        # NOTE mju_mat2Quat normalizes the quaternion.
         return SO3(wxyz=wxyz)
 
     @classmethod
@@ -163,20 +164,22 @@ class SO3(MatrixLieGroup):
     def exp(cls, tangent: np.ndarray) -> SO3:
         axis = np.array(tangent)
         theta = mujoco.mju_normalize3(axis)
-        wxyz = np.zeros(4)
+        wxyz = np.empty(4, dtype=np.float64)
+        # NOTE mju_axisAngle2Quat does not normalize the quaternion but is guaranteed
+        # to return a unit quaternion when axis is a unit vector. In our case,
+        # mju_normalize3 ensures that axis is a unit vector.
         mujoco.mju_axisAngle2Quat(wxyz, axis, theta)
         return SO3(wxyz=wxyz)
 
     # Eq. 133.
     def log(self) -> np.ndarray:
-        if self.wxyz[0] < 0.0:
-            theta = 2.0 * np.arccos(-self.wxyz[0])
-            axis = -1.0 * np.array(self.wxyz[1:])
-        else:
-            theta = 2.0 * np.arccos(self.wxyz[0])
-            axis = np.array(self.wxyz[1:])
-        mujoco.mju_normalize3(axis)
-        return theta * axis
+        q = np.array(self.wxyz)
+        q *= np.sign(q[0])
+        w, v = q[0], q[1:]
+        norm = mujoco.mju_normalize3(v)
+        if norm < get_epsilon(v.dtype):
+            return np.zeros_like(v)
+        return 2 * np.arctan2(norm, w) * v
 
     # Eq. 139.
     def adjoint(self) -> np.ndarray:

{mink-0.0.9 → mink-0.0.10}/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "flit_core.buildapi"
 
 [project]
 name = "mink"
-version = "0.0.9"
+version = "0.0.10"
 readme = "README.md"
 authors = [
     {name = "Kevin Zakka", email = "zakka@berkeley.edu"},