[gym/envs] Fix broken 'Ant' environment. (#731)

* [core] Print warnings even for release build type.
* [core/python] Expose more methods for 'Model'.
* [core/python] Fix multiple typos.
* [python/simulator] Fix 'robot' and 'robot_state' corrupted if robot is replaced.
* [gym/common|gym/toolbox] Promote generic SE3 math utilities from toolbox to common module.
* [gym/common] Add 'compute_tilt_from_quat', 'swing_from_vector', and 'remove_twist_from_quat'.
* [gym/common] Fix 'refresh_observation' being called before 'initialize_buffers' at simulation start.
* [gym/common] Fix broken mahony filter for 'twist_time_constant != None'.
* [gym/common] Fix support of non-batched mode for generic math.
* [gym/common] Fix 'matrix_to_rpy' to be consistent with 'pin.matrixToRpy'.
* [gym/envs] Fix broken 'Ant' environment.

core/include/jiminy/core/macros.h

@@ -65,19 +65,14 @@ namespace jiminy::internal
 */
 #define THROW_ERROR(exception, ...)                                                       \
     throw exception(                                                                      \
-        toString("In ",                                                                   \
-                 jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION), \
-                 " (" FILE_LINE "):\n",                                                   \
+        toString(jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION), \
+                 "(" FILE_LINE "):\n",                                                    \
                  __VA_ARGS__))
 
-#ifdef NDEBUG
-#    define PRINT_WARNING(...)
-#else
-#    define PRINT_WARNING(...)                                                               \
-        std::cout << "\x1b[1;93mWARNING\x1b[0m: In "                                         \
-                  << jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION) \
-                  << " (" FILE_LINE "):\n"                                                   \
-                  << toString(__VA_ARGS__) << std::endl
-#endif
+#define PRINT_WARNING(...)                                                               \
+    std::cout << "\x1b[1;93mWARNING\x1b[0m:"                                             \
+              << jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION) \
+              << "(" FILE_LINE "):\n"                                                    \
+              << toString(__VA_ARGS__) << std::endl
 
 #endif  // JIMINY_MACRO_H

core/include/jiminy/core/traits.h

@@ -67,7 +67,7 @@ namespace jiminy
 
     template<class T, class I = std::size_t>
     inline constexpr bool is_contiguous_container_v =
-        std::conjunction<internal::hasSize<T>, internal::isContiguousIndexable<T, I>>::value;
+        std::conjunction_v<internal::hasSize<T>, internal::isContiguousIndexable<T, I>>;
 
     // ************************************** select_last ************************************** //
 

core/src/engine/engine.cc

@@ -3305,7 +3305,9 @@ namespace jiminy
              const std::shared_ptr<AbstractConstraintBase> & constraint,
              Eigen::VectorXd & /* u */)
         {
+#ifndef NDEBUG
             PRINT_WARNING("No position bounds implemented for this type of joint.");
+#endif
             if (contactModel == ContactModelType::CONSTRAINT)
             {
                 // Disable fixed joint constraint
@@ -3393,7 +3395,9 @@ namespace jiminy
              ContactModelType /* contactModel */,
              Eigen::VectorXd & /* u */)
         {
+#ifndef NDEBUG
             PRINT_WARNING("No velocity bounds implemented for this type of joint.");
+#endif
         }
     };
 
@@ -3829,20 +3833,20 @@ namespace jiminy
             }
 
             // Call forward dynamics
-            bool isSucess = robotData.constraintSolver->SolveBoxedForwardDynamics(
+            bool isSuccess = robotData.constraintSolver->SolveBoxedForwardDynamics(
                 engineOptions_->constraints.regularization, isStateUpToDate, ignoreBounds);
 
             /* Monitor number of successive constraint solving failure. Exception raising is
                delegated to the 'step' method since this method is supposed to always succeed. */
-            if (isSucess)
+            if (isSuccess)
             {
                 robotData.successiveSolveFailed = 0U;
             }
             else
             {
                 if (engineOptions_->stepper.verbose)
                 {
-                    std::cout << "Constraint solver failure." << std::endl;
+                    PRINT_WARNING("Constraint solver failure.");
                 }
                 ++robotData.successiveSolveFailed;
             }

python/gym_jiminy/common/gym_jiminy/common/bases/__init__.py

@@ -1,25 +1,25 @@
 # pylint: disable=missing-module-docstring
 
-from .generic_bases import (DT_EPS,
-                            ObsT,
-                            ActT,
-                            BaseObsT,
-                            BaseActT,
-                            InfoType,
-                            SensorMeasurementStackMap,
-                            EngineObsType,
-                            InterfaceObserver,
-                            InterfaceController,
-                            InterfaceJiminyEnv)
-from .block_bases import (BlockStateT,
-                          InterfaceBlock,
-                          BaseObserverBlock,
-                          BaseControllerBlock)
-from .pipeline_bases import (BasePipelineWrapper,
-                             BaseTransformObservation,
-                             BaseTransformAction,
-                             ObservedJiminyEnv,
-                             ControlledJiminyEnv)
+from .generic import (DT_EPS,
+                      ObsT,
+                      ActT,
+                      BaseObsT,
+                      BaseActT,
+                      InfoType,
+                      SensorMeasurementStackMap,
+                      EngineObsType,
+                      InterfaceObserver,
+                      InterfaceController,
+                      InterfaceJiminyEnv)
+from .block import (BlockStateT,
+                    InterfaceBlock,
+                    BaseObserverBlock,
+                    BaseControllerBlock)
+from .pipeline import (BasePipelineWrapper,
+                       BaseTransformObservation,
+                       BaseTransformAction,
+                       ObservedJiminyEnv,
+                       ControlledJiminyEnv)
 
 
 __all__ = [

python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py → python/gym_jiminy/common/gym_jiminy/common/bases/block.py

@@ -14,13 +14,13 @@
 
 from ..utils import FieldNested, DataNested, get_fieldnames, fill, zeros
 
-from .generic_bases import (ObsT,
-                            ActT,
-                            BaseObsT,
-                            BaseActT,
-                            InterfaceController,
-                            InterfaceObserver,
-                            InterfaceJiminyEnv)
+from .generic import (ObsT,
+                      ActT,
+                      BaseObsT,
+                      BaseActT,
+                      InterfaceController,
+                      InterfaceObserver,
+                      InterfaceJiminyEnv)
 
 
 BlockStateT = TypeVar('BlockStateT', bound=Union[DataNested, None])

python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py → python/gym_jiminy/common/gym_jiminy/common/bases/generic.py

@@ -217,8 +217,8 @@ def _setup(self) -> None:
         self.observe_dt = -1
         self.control_dt = -1
 
-        # It is always necessary to refresh the observation at after reset
-        self.__is_observation_refreshed = True
+        # The observation must always be refreshed after setup
+        self.__is_observation_refreshed = False
 
         # Reset observation and action buffers
         fill(self.observation, 0)
@@ -244,8 +244,13 @@ def _observer_handle(self,
         :param v: Current actual velocity vector.
         :param sensor_measurements: Current sensor data.
         """
-        # Refresh the observation if not already done
-        if not self.__is_observation_refreshed:
+        # Refresh the observation if not already done but only if a simulation
+        # is already running. It would be pointless to refresh the observation
+        # at this point since the controller will be called multiple times at
+        # start. Besides, it would defeat the purpose `_initialize_buffers`,
+        # that is supposed to be executed before `refresh_observation` is being
+        # called for the first time of an episode.
+        if not self.__is_observation_refreshed and self.is_simulation_running:
             measurement = self.__measurement
             measurement["t"][()] = t
             measurement["states"]["agent"]["q"] = q
@@ -255,9 +260,7 @@ def _observer_handle(self,
             for sensor_type in self._sensors_types:
                 measurement_sensors[sensor_type] = next(sensor_measurements_it)
             self.refresh_observation(measurement)
-
-        # Consider observation has been refreshed iif a simulation is running
-        self.__is_observation_refreshed = self.is_simulation_running.item()
+            self.__is_observation_refreshed = True
 
     def _controller_handle(self,
                            t: float,

python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py → python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py

@@ -26,15 +26,15 @@
 
 from ..utils import DataNested, is_breakpoint, zeros, build_copyto, copy
 
-from .generic_bases import (DT_EPS,
-                            ObsT,
-                            ActT,
-                            BaseObsT,
-                            BaseActT,
-                            InfoType,
-                            EngineObsType,
-                            InterfaceJiminyEnv)
-from .block_bases import BaseControllerBlock, BaseObserverBlock
+from .generic import (DT_EPS,
+                      ObsT,
+                      ActT,
+                      BaseObsT,
+                      BaseActT,
+                      InfoType,
+                      EngineObsType,
+                      InterfaceJiminyEnv)
+from .block import BaseControllerBlock, BaseObserverBlock
 
 
 OtherObsT = TypeVar('OtherObsT', bound=DataNested)
@@ -75,8 +75,8 @@ def __init__(self,
         """
         # Initialize some proxies for fast lookup
         self.simulator = env.simulator
-        self.robot = env.robot
         self.stepper_state = env.stepper_state
+        self.robot = env.robot
         self.robot_state = env.robot_state
         self.sensor_measurements = env.sensor_measurements
         self.is_simulation_running = env.is_simulation_running