[gym_jiminy/common] Add suport of scalar concatenation to 'Concatenat…

…edQuantity'.

python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py

@@ -1533,47 +1533,6 @@ def refresh(self) -> bool:
         return self.is_colliding.get()
 
 
-@nb.jit(nopython=True, cache=True, fastmath=True, inline='always')
-def angle_difference(delta: ArrayOrScalar) -> ArrayOrScalar:
-    """Compute the signed element-wise difference (aka. oriented angle) between
-    two batches of angles.
-
-    The oriented angle is defined as the smallest angle in absolute value
-    between right and left angles (ignoring multi-turns), signed in accordance
-    with the angle going from right to left angles.
-
-    .. seealso::
-        This proposed implementation is the most efficient one for batch size
-        of 1000. See this posts for reference about other implementations:
-        https://stackoverflow.com/a/7869457/4820605
-
-    :param delta: Pre-computed difference between left and right angles.
-    """
-    return delta - np.floor((delta + np.pi) / (2 * np.pi)) * (2 * np.pi)
-
-
-@nb.jit(nopython=True, cache=True, fastmath=True)
-def angle_total(angles: np.ndarray) -> np.ndarray:
-    """Compute the total signed multi-turn angle from start to end of
-    time-series of angles.
-
-    The method is fully compliant with individual angles restricted between
-    [-pi, pi], but it requires the distance between the angles at successive
-    timesteps to be smaller than pi.
-
-    .. seealso::
-        See `angle_difference` documentation for details.
-
-    :param angle: Temporal sequence of angles as a multi-dimensional array
-                  whose last dimension gathers all the successive timesteps.
-    """
-    # Note that `angle_difference` has been manually inlined as it results in
-    # about 50% speedup, which is surprising.
-    delta = angles[..., 1:] - angles[..., :-1]
-    delta -= np.floor((delta + np.pi) / (2.0 * np.pi)) * (2 * np.pi)
-    return np.sum(delta, axis=-1)
-
-
 @dataclass(unsafe_hash=True)
 class DeltaBaseOdometryPosition(InterfaceQuantity[ArrayOrScalar]):
     """Variation of the base odometry position (X, Y) over a given horizon.
@@ -1639,6 +1598,47 @@ def refresh(self) -> ArrayOrScalar:
         return self.data.get()
 
 
+@nb.jit(nopython=True, cache=True, fastmath=True, inline='always')
+def angle_difference(delta: ArrayOrScalar) -> ArrayOrScalar:
+    """Compute the signed element-wise difference (aka. oriented angle) between
+    two batches of angles.
+
+    The oriented angle is defined as the smallest angle in absolute value
+    between right and left angles (ignoring multi-turns), signed in accordance
+    with the angle going from right to left angles.
+
+    .. seealso::
+        This proposed implementation is the most efficient one for batch size
+        of 1000. See this posts for reference about other implementations:
+        https://stackoverflow.com/a/7869457/4820605
+
+    :param delta: Pre-computed difference between left and right angles.
+    """
+    return delta - np.floor((delta + np.pi) / (2 * np.pi)) * (2 * np.pi)
+
+
+@nb.jit(nopython=True, cache=True, fastmath=True)
+def angle_total(angles: np.ndarray) -> np.ndarray:
+    """Compute the total signed multi-turn angle from start to end of
+    time-series of angles.
+
+    The method is fully compliant with individual angles restricted between
+    [-pi, pi], but it requires the distance between the angles at successive
+    timesteps to be smaller than pi.
+
+    .. seealso::
+        See `angle_difference` documentation for details.
+
+    :param angle: Temporal sequence of angles as a multi-dimensional array
+                  whose last dimension gathers all the successive timesteps.
+    """
+    # Note that `angle_difference` has been manually inlined as it results in
+    # about 50% speedup, which is surprising.
+    delta = angles[..., 1:] - angles[..., :-1]
+    delta -= np.floor((delta + np.pi) / (2.0 * np.pi)) * (2 * np.pi)
+    return np.sum(delta, axis=-1)
+
+
 @dataclass(unsafe_hash=True)
 class DeltaBaseOdometryOrientation(InterfaceQuantity[ArrayOrScalar]):
     """Variation of the base odometry orientation (Yaw,) over a given horizon.

python/gym_jiminy/common/gym_jiminy/common/quantities/transform.py

@@ -458,6 +458,11 @@ class ConcatenatedQuantity(InterfaceQuantity[np.ndarray]):
 
     All the quantities must have the same shape, except for the dimension
     corresponding to concatenation axis.
+
+    .. note::
+        For efficiency and convenience, built-in scalars and 0-D arrays are
+        treated as 1D arrays. For instance, multiple floats can be concatenated
+        as a vector.
     """
 
     quantities: Tuple[InterfaceQuantity[np.ndarray], ...]
@@ -516,10 +521,14 @@ def initialize(self) -> None:
         super().initialize()
 
         # Get current value of all the quantities
-        values = [quantity.get() for quantity in self.quantities]
+        # Dealing with special case where value is a float, as it would impede
+        # performance to force allocating a 1D array before concatenation.
+        values = tuple(
+            np.atleast_1d(quantity.get()) for quantity in self.quantities)
 
         # Allocate contiguous memory
-        self._data = np.concatenate(values, axis=self.axis)
+        self._data = np.concatenate(
+            values, axis=self.axis)
 
         # Compute slices of data
         self._data_slices.clear()