[gym/common] Add 'DeformationEstimator' block.

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]);
         }

core/src/robot/model.cc

@@ -1672,22 +1672,25 @@ namespace jiminy
         }
     }
 
-    void Model::getRigidPositionFromFlexible(const Eigen::VectorXd & qFlex,
-                                             Eigen::VectorXd & qRigid) const
+
+    void Model::getFlexibleVelocityFromRigid(const Eigen::VectorXd & vRigid,
+                                             Eigen::VectorXd & vFlex) const
     {
         // Define some proxies
-        uint32_t nqFlex = pncModelFlexibleOrig_.nq;
+        uint32_t nvRigid = pinocchioModelOrig_.nv;
+        uint32_t nvFlex = pncModelFlexibleOrig_.nv;
 
         // Check the size of the input state
-        if (qFlex.size() != nqFlex)
+        if (vRigid.size() != nvRigid)
         {
-            THROW_ERROR(std::invalid_argument, "Size of qFlex inconsistent with flexible model.");
+            THROW_ERROR(std::invalid_argument,
+                        "Size of vRigid inconsistent with theoretical model.");
         }
 
-        // Initialize the rigid state
-        qRigid = pinocchio::neutral(pinocchioModelOrig_);
+        // Initialize the flexible state
+        vFlex.setZero(nvFlex);
 
-        // Compute the rigid state based on the flexible state
+        // Compute the flexible state based on the rigid state
         int32_t idxRigid = 0;
         int32_t idxFlex = 0;
         for (; idxRigid < pinocchioModelOrig_.njoints; ++idxFlex)
@@ -1700,32 +1703,30 @@ namespace jiminy
                 const auto & jointFlex = pncModelFlexibleOrig_.joints[idxFlex];
                 if (jointRigid.idx_q() >= 0)
                 {
-                    qRigid.segment(jointRigid.idx_q(), jointRigid.nq()) =
-                        qFlex.segment(jointFlex.idx_q(), jointFlex.nq());
+                    vFlex.segment(jointFlex.idx_v(), jointFlex.nv()) =
+                        vRigid.segment(jointRigid.idx_v(), jointRigid.nv());
                 }
                 ++idxRigid;
             }
         }
     }
 
-    void Model::getFlexibleVelocityFromRigid(const Eigen::VectorXd & vRigid,
-                                             Eigen::VectorXd & vFlex) const
+    void Model::getRigidPositionFromFlexible(const Eigen::VectorXd & qFlex,
+                                             Eigen::VectorXd & qRigid) const
     {
         // Define some proxies
-        uint32_t nvRigid = pinocchioModelOrig_.nv;
-        uint32_t nvFlex = pncModelFlexibleOrig_.nv;
+        uint32_t nqFlex = pncModelFlexibleOrig_.nq;
 
         // Check the size of the input state
-        if (vRigid.size() != nvRigid)
+        if (qFlex.size() != nqFlex)
         {
-            THROW_ERROR(std::invalid_argument,
-                        "Size of vRigid inconsistent with theoretical model.");
+            THROW_ERROR(std::invalid_argument, "Size of qFlex inconsistent with flexible model.");
         }
 
-        // Initialize the flexible state
-        vFlex.setZero(nvFlex);
+        // Initialize the rigid state
+        qRigid = pinocchio::neutral(pinocchioModelOrig_);
 
-        // Compute the flexible state based on the rigid state
+        // Compute the rigid state based on the flexible state
         int32_t idxRigid = 0;
         int32_t idxFlex = 0;
         for (; idxRigid < pinocchioModelOrig_.njoints; ++idxFlex)
@@ -1738,8 +1739,8 @@ namespace jiminy
                 const auto & jointFlex = pncModelFlexibleOrig_.joints[idxFlex];
                 if (jointRigid.idx_q() >= 0)
                 {
-                    vFlex.segment(jointFlex.idx_v(), jointFlex.nv()) =
-                        vRigid.segment(jointRigid.idx_v(), jointRigid.nv());
+                    qRigid.segment(jointRigid.idx_q(), jointRigid.nq()) =
+                        qFlex.segment(jointFlex.idx_q(), jointFlex.nq());
                 }
                 ++idxRigid;
             }
@@ -1778,10 +1779,7 @@ namespace jiminy
                     vRigid.segment(jointRigid.idx_v(), jointRigid.nv()) =
                         vFlex.segment(jointFlex.idx_v(), jointFlex.nv());
                 }
-            }
-            else
-            {
-                ++idxFlex;
+                ++idxRigid;
             }
         }
     }

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]

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 "

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,

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'
 ]