[gym_jiminy/common] Fix 'matrix_to_rpy' to return unique angle consis…

…tent with 'pin.matrixToRpy'.

python/gym_jiminy/common/gym_jiminy/common/blocks/mahony_filter.py

@@ -257,7 +257,7 @@ def __init__(self,
                  name: str,
                  env: InterfaceJiminyEnv[BaseObsT, BaseActT],
                  *,
-                 twist_time_constant: Optional[float] = None,
+                 twist_time_constant: Optional[float] = 0.0,
                  exact_init: bool = True,
                  kp: Union[np.ndarray, float] = 1.0,
                  ki: Union[np.ndarray, float] = 0.1,
@@ -272,7 +272,7 @@ def __init__(self,
             Filter. If `0.0`, then its is kept constant equal to zero. `None`
             to kept the original estimate provided by Mahony Filter.
             See `remove_twist` and `update_twist` documentation for details.
-            Optional: `None` by default.
+            Optional: `0.0` by default.
         :param exact_init: Whether to initialize orientation estimate using
                            accelerometer measurements or ground truth. `False`
                            is not recommended because the robot is often

python/gym_jiminy/common/gym_jiminy/common/utils/math.py

@@ -251,15 +251,17 @@ def matrix_to_rpy(mat: np.ndarray,
     else:
         assert out.shape == (3, *mat.shape[2:])
         out_ = out
+
+    # Direct translation of Eigen: `R.eulerAngles(2, 1, 0).reverse()`
     # roll, pitch, yaw = out_
-    out_[2] = np.arctan2(mat[1, 0], mat[0, 0])
     cos_pitch = np.sqrt(mat[2, 2] ** 2 + mat[2, 1] ** 2)
-    out_[1] = np.arctan2(- mat[2, 0], np.sign(out_[2]) * cos_pitch)
-    out_[2] += np.pi * (out_[2] < 0.0)
+    out_[1] = np.arctan2(- mat[2, 0], cos_pitch)
+    out_[2] = np.arctan2(mat[1, 0], mat[0, 0])
     sin_yaw, cos_yaw = np.sin(out_[2]), np.cos(out_[2])
     out_[0] = np.arctan2(
         sin_yaw * mat[0, 2] - cos_yaw * mat[1, 2],
         cos_yaw * mat[1, 1] - sin_yaw * mat[0, 1])
+
     return out_
 
 

python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/__init__.py

@@ -1,12 +1,11 @@
 # pylint: disable=missing-module-docstring
 
 from .qhull import ConvexHull, compute_distance_convex_to_point
-from .generic import squared_norm_2, quat_average
+from .generic import quat_average
 
 __all__ = [
     "ConvexHull",
     "compute_distance_convex_to_point",
-    "squared_norm_2",
     "quat_average"
 ]
 

python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/generic.py

@@ -6,14 +6,6 @@
 import numba as nb
 
 
-@nb.jit(nopython=True, cache=True)
-def squared_norm_2(array: np.ndarray) -> float:
-    """Fast implementation of the sum of squared array elements, optimized for
-    small to medium size 1D arrays.
-    """
-    return np.sum(np.square(array))
-
-
 def quat_average(quat: np.ndarray,
                  axis: Optional[Union[Tuple[int, ...], int]] = None
                  ) -> np.ndarray:

python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py

@@ -7,7 +7,7 @@
 from numba.np.extensions import cross2d
 from scipy.spatial import _qhull
 
-from .generic import squared_norm_2
+from gym_jiminy.common.utils import squared_norm_2
 
 
 @nb.jit(nopython=True, nogil=True, cache=True, inline='always')

python/gym_jiminy/unit_py/test_pipeline_control.py

@@ -19,7 +19,9 @@
 
 from gym_jiminy.envs import (
     AtlasPDControlJiminyEnv, CassiePDControlJiminyEnv, DigitPDControlJiminyEnv)
-from gym_jiminy.common.blocks import PDController
+from gym_jiminy.common.blocks import PDController, MahonyFilter
+from gym_jiminy.common.blocks.mahony_filter import remove_twist
+from gym_jiminy.common.utils import quat_to_rpy, matrix_to_rpy, matrix_to_quat
 
 
 IMAGE_DIFF_THRESHOLD = 5.0
@@ -125,7 +127,7 @@ def test_mahony_filter(self):
         """
         # Instantiate and reset the environment
         env = AtlasPDControlJiminyEnv()
-        env.reset()
+        assert isinstance(env.observer, MahonyFilter)
 
         # Define a constant action that move the upper-body in all directions
         robot = env.robot
@@ -139,13 +141,38 @@ def test_mahony_filter(self):
         imu_rot = robot.pinocchio_data.oMf[sensor.frame_index].rotation
 
         # Check that the estimate IMU orientation is accurate over the episode
-        for i in range(200):
-            env.step(action * (1 - 2 * ((i // 50) % 2)))
-            rpy_true = pin.rpy.matrixToRpy(imu_rot)
-            rpy_est = pin.rpy.matrixToRpy(
-                pin.Quaternion(env.observer.observation).matrix())
-            np.testing.assert_allclose(rpy_true, rpy_est, atol=0.01)
-        env.stop()
+        for twist_time_constant in (None, float("inf"), 0.0):
+            # Reinitialize the observer
+            env.observer = MahonyFilter(
+                env.observer.name,
+                env.observer.env,
+                twist_time_constant=twist_time_constant,
+                exact_init=True,
+                kp=env.observer.kp,
+                ki=env.observer.ki)
+
+            # Reset the environment
+            env.reset()
+
+            # Run of few simulation steps
+            for i in range(200):
+                env.step(action * (1 - 2 * ((i // 50) % 2)))
+
+                if twist_time_constant == 0.0:
+                    # The twist must be ignored as it is not observable
+                    obs_true = matrix_to_quat(imu_rot)
+                    remove_twist(obs_true)
+                    rpy_true = quat_to_rpy(obs_true)
+                    obs_est = env.observer.observation[:, 0].copy()
+                    remove_twist(obs_est)
+                    rpy_est = quat_to_rpy(obs_est)
+                else:
+                    # The twist is either measured or estimated
+                    rpy_true = matrix_to_rpy(imu_rot)
+                    rpy_est = quat_to_rpy(env.observer.observation[:, 0])
+
+                np.testing.assert_allclose(rpy_true, rpy_est, atol=0.01)
+            env.stop()
 
     def test_pid_controller(self):
         """ TODO: Write documentation.