[gym/common] Add 'DeformationEstimator' block.

duburcqa · Mar 3, 2024 · a0f99dc · a0f99dc
1 parent f39e6c7
commit a0f99dc
Show file tree

Hide file tree

Showing 16 changed files with 1,244 additions and 67 deletions.
diff --git a/core/src/control/abstract_controller.cc b/core/src/control/abstract_controller.cc
@@ -203,7 +203,7 @@ namespace jiminy
                                            { return element.first == *fieldIt; });
             if (variableIt != registeredVariables.end())
             {
-                THROW_ERROR(lookup_error, "Variable already registered.");
+                THROW_ERROR(lookup_error, "Variable '", *fieldIt, "' already registered.");
             }
             registeredVariables.emplace_back(*fieldIt, &values[i]);
         }

diff --git a/data/toys_models/ant/ant_hardware.toml b/data/toys_models/ant/ant_hardware.toml
@@ -7,20 +7,6 @@ contactFrameNames = []  #['tip_1', 'tip_2', 'tip_3', 'tip_4']
 
 # ============== Motors ===============
 
-[Motor.SimpleMotor.hip_4]
-joint_name = "hip_4"
-mechanicalReduction = 150.0
-armature = 1.0
-frictionViscousPositive = -1.0
-frictionViscousNegative = -1.0
-
-[Motor.SimpleMotor.ankle_4]
-joint_name = "ankle_4"
-mechanicalReduction = 150.0
-armature = 1.0
-frictionViscousPositive = -1.0
-frictionViscousNegative = -1.0
-
 [Motor.SimpleMotor.hip_1]
 joint_name = "hip_1"
 mechanicalReduction = 150.0
@@ -63,6 +49,20 @@ armature = 1.0
 frictionViscousPositive = -1.0
 frictionViscousNegative = -1.0
 
+[Motor.SimpleMotor.hip_4]
+joint_name = "hip_4"
+mechanicalReduction = 150.0
+armature = 1.0
+frictionViscousPositive = -1.0
+frictionViscousNegative = -1.0
+
+[Motor.SimpleMotor.ankle_4]
+joint_name = "ankle_4"
+mechanicalReduction = 150.0
+armature = 1.0
+frictionViscousPositive = -1.0
+frictionViscousNegative = -1.0
+
 # ============= Encoder sensors ===============
 
 [Sensor.EncoderSensor.hip_1]

diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/block.py b/python/gym_jiminy/common/gym_jiminy/common/bases/block.py
@@ -12,7 +12,7 @@
 
 import gymnasium as gym
 
-from ..utils import FieldNested, DataNested, get_fieldnames, fill, zeros
+from ..utils import FieldNested, DataNested, get_fieldnames, zeros
 
 from .generic import (ObsT,
                       ActT,
@@ -159,9 +159,6 @@ def _setup(self) -> None:
         # Compute the update period
         self.observe_dt = self.env.observe_dt * self.update_ratio
 
-        # Set default observation
-        fill(self.observation, 0)
-
         # Make sure the controller period is lower than environment timestep
         assert self.observe_dt <= self.env.step_dt, (
             "The observer update period must be lower than or equal to the "

diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/generic.py b/python/gym_jiminy/common/gym_jiminy/common/bases/generic.py
@@ -16,7 +16,7 @@
 from jiminy_py.simulator import Simulator
 from jiminy_py.viewer.viewer import is_display_available
 
-from ..utils import DataNested, fill
+from ..utils import DataNested
 
 
 # Temporal resolution of simulator steps
@@ -155,7 +155,7 @@ def compute_reward(self,
 
         :returns: Aggregated reward for the current step.
         """
-        raise NotImplementedError
+        return 0.0
 
 
 # Note that `InterfaceJiminyEnv` must inherit from `InterfaceObserver`
@@ -220,10 +220,6 @@ def _setup(self) -> None:
         # The observation must always be refreshed after setup
         self.__is_observation_refreshed = False
 
-        # Reset observation and action buffers
-        fill(self.observation, 0)
-        fill(self.action, 0)
-
     @no_type_check
     def _observer_handle(self,
                          t: float,
@@ -259,7 +255,11 @@ def _observer_handle(self,
             sensor_measurements_it = iter(sensor_measurements.values())
             for sensor_type in self._sensors_types:
                 measurement_sensors[sensor_type] = next(sensor_measurements_it)
-            self.refresh_observation(measurement)
+            try:
+                self.refresh_observation(measurement)
+            except RuntimeError as e:
+                raise RuntimeError(
+                    "The observation space must be constant.") from e
             self.__is_observation_refreshed = True
 
     def _controller_handle(self,

diff --git a/python/gym_jiminy/common/gym_jiminy/common/blocks/__init__.py b/python/gym_jiminy/common/gym_jiminy/common/blocks/__init__.py
@@ -3,10 +3,12 @@
 from .mahony_filter import MahonyFilter
 from .motor_safety_limit import MotorSafetyLimit
 from .proportional_derivative_controller import PDController
+from .deformation_estimator import DeformationEstimator
 
 
 __all__ = [
     'MahonyFilter',
     'MotorSafetyLimit',
     'PDController',
+    'DeformationEstimator'
 ]