[python/dynamics] Fix trajectory interpolation preferring t+ over t-.

python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py

@@ -471,7 +471,7 @@ def reset(self,
         if (not ignore_auto_refresh and self.auto_refresh and
                 not self.has_cache):
             raise RuntimeError(
-                "Automatic refresh enabled but no shared cache available. "
+                "Automatic refresh enabled but no shared cache is available. "
                 "Please add one before calling this method.")
 
         # Reset all requirements first
@@ -994,7 +994,7 @@ def initialize(self) -> None:
             else:
                 f_external_batch = np.array([])
             self.state = State(
-                self.env.stepper_state.t,
+                0.0,
                 self.env.robot_state.q,
                 self.env.robot_state.v,
                 self.env.robot_state.a,
@@ -1039,6 +1039,7 @@ def refresh(self) -> State:
         """
         # Update state at which the quantity must be evaluated
         if self.mode == QuantityEvalMode.TRUE:
+            self.state.t = self.env.stepper_state.t
             multi_array_copyto(self._f_external_slices, self._f_external_list)
         else:
             self.state = self.trajectory.get()

python/gym_jiminy/unit_py/test_quantities.py

@@ -309,8 +309,10 @@ def test_true_vs_reference(self):
             env.quantities["ref"] = quantity_creator(
                 QuantityEvalMode.REFERENCE)
 
+            # No trajectory has been selected
             with self.assertRaises(RuntimeError):
                 env.reset(seed=0)
+                env.quantities["ref"]
 
             env.quantities.add_trajectory("reference", trajectory)
             env.quantities.select_trajectory("reference")

python/jiminy_py/src/jiminy_py/dynamics.py

@@ -7,9 +7,9 @@
 """
 # pylint: disable=invalid-name,no-member
 import logging
-from bisect import bisect_right
+from bisect import bisect_left
 from dataclasses import dataclass
-from typing import Dict, Optional, Tuple, Sequence, Callable, Literal
+from typing import Optional, Tuple, Sequence, Callable, Literal
 
 import numpy as np
 
@@ -205,7 +205,7 @@ def __init__(self,
 
         # Keep track of last request to speed up nearest neighbors search
         self._t_prev = 0.0
-        self._index_prev = 0
+        self._index_prev = 1
 
         # List of optional state fields that are provided
         self._has_velocity = False
@@ -318,14 +318,20 @@ def get(self,
         else:
             t = max(t, t_start)  # Clipping right it is sufficient
 
-        # Get nearest neighbors timesteps for linear interpolation
+        # Get nearest neighbors timesteps for linear interpolation.
+        # Note that the left and right data points may be associated with the
+        # same timestamp, corresponding respectively t- and t+. These values
+        # are different for quantities that may change discontinuously such as
+        # the acceleration. If the state at such a timestamp is requested, then
+        # returning the left value is preferred, because it corresponds to the
+        # only state that was accessible to the user, ie after call `step`.
         if t < self._t_prev:
-            self._index_prev = 0
-        self._index_prev = bisect_right(
+            self._index_prev = 1
+        self._index_prev = bisect_left(
             self._times, t, self._index_prev, len(self._times) - 1)
         self._t_prev = t
 
-        # Skip interpolation if not necessary
+        # Skip interpolation if not necessary.
         index_left, index_right = self._index_prev - 1, self._index_prev
         t_left, s_left = self._times[index_left], self.states[index_left]
         if t - t_left < TRAJ_INTERP_TOL: