[core] 'EngineMultiRobot.Start' no longer reset the engine automatica…

…lly. (#342)

* [core] 'EngineMultiRobot.Start' no longer reset the engine automatically.
* [core] Biases for random model generation are now relative instead of absolute.
* [core] Fix computation of center of mass positions and velocities.
* [python/viewer] Fix log replay without velocity and external forces data available.
* [python/viewer] Log replay now support overwriting mesh package dir.
* [gym/example] Add option to specify log subdirectory name interactively.
* [misc] Fix CI numpy version issues.

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>

.github/workflows/linux.yml

@@ -46,8 +46,8 @@ jobs:
         sudo apt install -y gnupg curl wget build-essential cmake doxygen graphviz texlive-latex-base
 
         "${PYTHON_EXECUTABLE}" -m pip install --upgrade pip
-        "${PYTHON_EXECUTABLE}" -m pip install wheel
-        "${PYTHON_EXECUTABLE}" -m pip install "numpy<1.20"
+        "${PYTHON_EXECUTABLE}" -m pip install --upgrade wheel
+        "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy
 
         echo "RootDir=${GITHUB_WORKSPACE}" >> $GITHUB_ENV
         echo "InstallDir=${GITHUB_WORKSPACE}/install" >> $GITHUB_ENV

.github/workflows/ubuntu.yml

@@ -49,6 +49,7 @@ jobs:
     - name: Installing requirements
       run: |
         sudo "${GITHUB_WORKSPACE}/build_tools/easy_install_deps_ubuntu.sh"
+        "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy
         "${PYTHON_EXECUTABLE}" -m pip install "torch==1.8.0+cpu" -f https://download.pytorch.org/whl/torch_stable.html
         "${PYTHON_EXECUTABLE}" -m pip install "gym>=0.17.3,<0.18.0" "stable_baselines3>=0.10"
 

.github/workflows/win.yml

@@ -44,8 +44,8 @@ jobs:
       run: |
         git config --global advice.detachedHead false
         python -m pip install --upgrade pip
-        python -m pip install wheel pefile machomachomangler
-        python -m pip install numpy
+        python -m pip install --upgrade wheel pefile machomachomangler
+        python -m pip install --upgrade numpy
         python -m pip uninstall -y pipx  # Uninstall unecessary packages causing conflicts with the new resolver
     - name: Build project dependencies
       run: |

CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.6.16)
+set(BUILD_VERSION 1.6.17)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")

core/include/jiminy/core/engine/Engine.h

@@ -34,22 +34,17 @@ namespace jiminy
 
         /// \brief Reset the engine and compute initial state.
         ///
-        /// \details This function reset the engine, the robot and the controller, and update internal data
-        ///          to match the given initial state.
+        /// \details This function does NOT reset the engine, robot and controller.
+        ///          It is up to the user to do so, by calling `reset` method first.
         ///
         /// \param[in] qInit Initial configuration.
         /// \param[in] vInit Initial velocity.
         /// \param[in] aInit Initial acceleration. Optional: Zero by default.
         /// \param[in] isStateTheoretical Specify if the initial state is associated with the current or theoretical model
-        /// \param[in] resetRandomNumbers Whether or not to reset the random number generator.
-        /// \param[in] removeAllForce Whether or not to register the external force profiles applied
-        ///                                      during the simulation.
         hresult_t start(vectorN_t const & qInit,
                         vectorN_t const & vInit,
                         boost::optional<vectorN_t> const & aInit = boost::none,
-                        bool_t    const & isStateTheoretical = false,
-                        bool_t    const & resetRandomNumbers = false,
-                        bool_t    const & removeAllForce = false);
+                        bool_t    const & isStateTheoretical = false);
 
         /// \brief Run a simulation of duration tEnd, starting at xInit.
         ///

core/include/jiminy/core/engine/EngineMultiRobot.h

@@ -410,19 +410,15 @@ namespace jiminy
 
         /// \brief Reset the engine and compute initial state.
         ///
-        /// \details This function reset the engine, the robot and the controller, and update internal data
-        ///          to match the given initial state.
+        /// \details This function does NOT reset the engine, robot and controller.
+        ///          It is up to the user to do so, by calling `reset` method first.
         ///
         /// \param[in] qInit Initial configuration of every system.
         /// \param[in] vInit Initial velocity of every system.
         /// \param[in] aInit Initial acceleration of every system. Optional: Zero by default.
-        /// \param[in] resetRandomNumbers Whether or not to reset the random number generators.
-        /// \param[in] removeAllForce Whether or not to remove registered external forces.
         hresult_t start(std::map<std::string, vectorN_t> const & qInit,
                         std::map<std::string, vectorN_t> const & vInit,
-                        boost::optional<std::map<std::string, vectorN_t> > const & aInit = boost::none,
-                        bool_t const & resetRandomNumbers = false,
-                        bool_t const & removeAllForce = false);
+                        boost::optional<std::map<std::string, vectorN_t> > const & aInit = boost::none);
 
         /// \brief Integrate system from current state for a duration equal to stepSize
         ///

core/src/engine/Engine.cc

@@ -159,9 +159,7 @@ namespace jiminy
     hresult_t Engine::start(vectorN_t const & qInit,
                             vectorN_t const & vInit,
                             boost::optional<vectorN_t> const & aInit,
-                            bool_t    const & isStateTheoretical,
-                            bool_t    const & resetRandomNumbers,
-                            bool_t    const & removeAllForce)
+                            bool_t    const & isStateTheoretical)
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
@@ -182,8 +180,7 @@ namespace jiminy
 
         if (returnCode == hresult_t::SUCCESS)
         {
-            returnCode = EngineMultiRobot::start(
-                qInitList, vInitList, aInitList, resetRandomNumbers, removeAllForce);
+            returnCode = EngineMultiRobot::start(qInitList, vInitList, aInitList);
         }
 
         return returnCode;

core/src/engine/EngineMultiRobot.cc

@@ -904,24 +904,21 @@ namespace jiminy
            computations here instead of 'computeForwardKinematics', we are
            doing the assumption that it is varying slowly enough to consider
            it constant during one integration step. */
-        if (!system.robot->hasConstraint())
+        data.oYcrb[0].setZero();
+        for (int32_t i = 1; i < model.njoints; ++i)
         {
-            data.oYcrb[0].setZero();
-            for (int32_t i = 1; i < model.njoints; ++i)
-            {
-                data.Ycrb[i] = model.inertias[i];
-                data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
-            }
-            for (int32_t i = model.njoints - 1; i > 0; --i)
+            data.Ycrb[i] = model.inertias[i];
+            data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
+        }
+        for (int32_t i = model.njoints - 1; i > 0; --i)
+        {
+            jointIndex_t const & jointIdx = model.joints[i].id();
+            jointIndex_t const & parentIdx = model.parents[jointIdx];
+            if (parentIdx > 0)
             {
-                jointIndex_t const & jointIdx = model.joints[i].id();
-                jointIndex_t const & parentIdx = model.parents[jointIdx];
-                if (parentIdx > 0)
-                {
-                    data.Ycrb[parentIdx] += data.liMi[jointIdx].act(data.Ycrb[jointIdx]);
-                }
-                data.oYcrb[parentIdx] += data.oYcrb[i];
+                data.Ycrb[parentIdx] += data.liMi[jointIdx].act(data.Ycrb[jointIdx]);
             }
+            data.oYcrb[parentIdx] += data.oYcrb[i];
         }
 
         /* Neither 'aba' nor 'forwardDynamics' are computed the actual joints
@@ -961,6 +958,7 @@ namespace jiminy
         data.Ig.lever().setZero();
         data.Ig.inertia() = data.oYcrb[0].inertia();
         data.com[0] = data.oYcrb[0].lever();
+        data.vcom[0] = data.h[0].linear() / data.mass[0];
         for (int32_t i = 1; i < model.njoints; ++i)
         {
             data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever());
@@ -1007,9 +1005,7 @@ namespace jiminy
 
     hresult_t EngineMultiRobot::start(std::map<std::string, vectorN_t> const & qInit,
                                       std::map<std::string, vectorN_t> const & vInit,
-                                      boost::optional<std::map<std::string, vectorN_t> > const & aInit,
-                                      bool_t const & resetRandomNumbers,
-                                      bool_t const & removeAllForce)
+                                      boost::optional<std::map<std::string, vectorN_t> > const & aInit)
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
@@ -1152,9 +1148,6 @@ namespace jiminy
             }
         }
 
-        // Reset the robot, controller, engine, and registered impulse forces if requested
-        reset(resetRandomNumbers, removeAllForce);
-
         auto systemIt = systems_.begin();
         auto systemDataIt = systemsDataHolder_.begin();
         for ( ; systemIt != systems_.end(); ++systemIt, ++systemDataIt)
@@ -1527,10 +1520,13 @@ namespace jiminy
             returnCode = hresult_t::ERROR_BAD_INPUT;
         }
 
-        // Reset the robot, controller, and engine
         if (returnCode == hresult_t::SUCCESS)
         {
-            returnCode = start(qInit, vInit, aInit, true, false);
+            // Reset the robot, controller, and engine
+            reset(true, false);
+
+            // Start the simulation
+            returnCode = start(qInit, vInit, aInit);
         }
 
         // Now that telemetry has been initialized, check simulation duration.

core/src/robot/Model.cc

@@ -1100,7 +1100,7 @@ namespace jiminy
                 if (comBiasStd > EPS)
                 {
                     vector3_t & comRelativePositionBody = pncModel_.inertias[jointIdx].lever();
-                    comRelativePositionBody += randVectorNormal(3U, comBiasStd);
+                    comRelativePositionBody.array() *= randVectorNormal(3U, comBiasStd).array();
                 }
 
                 /* Add bias to body mass.
@@ -1109,7 +1109,8 @@ namespace jiminy
                 if (massBiasStd > EPS)
                 {
                     float64_t & massBody = pncModel_.inertias[jointIdx].mass();
-                    massBody = std::max(massBody + randNormal(0.0, massBiasStd), std::min(massBody, 1.0e-3));
+                    massBody = std::max(massBody * (1.0 + randNormal(0.0, massBiasStd)),
+                                        std::min(massBody, 1.0e-3));
                 }
 
                 /* Add bias to inertia matrix of body.
@@ -1128,7 +1129,7 @@ namespace jiminy
                     matrix3_t inertiaBodyAxes = solver.eigenvectors();
                     vector3_t const randAxis = randVectorNormal(3U, inertiaBiasStd);
                     inertiaBodyAxes = inertiaBodyAxes * quaternion_t(pinocchio::exp3(randAxis));
-                    inertiaBodyMoments += randVectorNormal(3U, inertiaBiasStd);
+                    inertiaBodyMoments.array() *= randVectorNormal(3U, inertiaBiasStd).array();
                     inertiaBody = pinocchio::Symmetric3((
                         inertiaBodyAxes * inertiaBodyMoments.asDiagonal() * inertiaBodyAxes.transpose()).eval());
                 }
@@ -1138,7 +1139,7 @@ namespace jiminy
                 if (relativeBodyPosBiasStd > EPS)
                 {
                     vector3_t & relativePositionBody = pncModel_.jointPlacements[jointIdx].translation();
-                    relativePositionBody += randVectorNormal(3U, relativeBodyPosBiasStd);
+                    relativePositionBody.array() *= randVectorNormal(3U, relativeBodyPosBiasStd).array();
                 }
             }
 

python/gym_jiminy/examples/rllib/tools/utilities.py

@@ -130,8 +130,8 @@ def _flatten_dict(dt, delimiter="/", prevent_delimiter=False):
 ray.tune.logger.flatten_dict = _flatten_dict
 
 
-def initialize(num_cpus: int = 0,
-               num_gpus: int = 0,
+def initialize(num_cpus: int,
+               num_gpus: int,
                log_root_path: Optional[str] = None,
                log_name: Optional[str] = None,
                debug: bool = False,
@@ -141,29 +141,34 @@ def initialize(num_cpus: int = 0,
     It will be used later for almost everything from dashboard, remote/client
     management, to multithreaded environment.
 
+    .. note:
+        The default Tensorboard port will be used, namely 6006 if available,
+        using 0.0.0.0 (binding to all IPv4 addresses on local machine).
+        Similarly, Ray dashboard port is 8265 if available. In both cases, the
+        port will be increased interatively until to find one available.
+
     :param num_cpus: Maximum number of CPU threads that can be executed in
                      parallel. Note that it does not actually reserve part of
                      the CPU, so that several processes can reserve the number
-                     of threads available on the system at the same time. 0 for
-                     unlimited.
-                     Optional: Unlimited by default.
+                     of threads available on the system at the same time.
     :param num_gpu: Maximum number of GPU unit that can be used, which can be
                     fractional to only allocate part of the resource. Note that
                     contrary to CPU resource, the memory is likely to actually
                     be reserve and allocated by the process, in particular
-                    using Tensorflow backend. 0 for unlimited.
-                    Optional: Unlimited by default.
+                    using Tensorflow backend.
     :param log_root_path: Fullpath of root log directory.
                           Optional: location of this file / log by default.
-    :param log_name: Name of the subdirectory where to save data.
+    :param log_name: Name of the subdirectory where to save data. `None` to
+                     use default name, empty string '' to set it interactively
+                     in command prompt. It must be a valid Python identifier.
                      Optional: full date _ hostname by default.
     :param debug: Whether or not to display debugging trace.
                   Optional: Disable by default.
     :param verbose: Whether or not to print information about what is going on.
                     Optional: True by default.
 
-    :returns: lambda function to pass Ray Trainers to monitor the learning
-              progress in tensorboard.
+    :returns: lambda function to pass a `ray.Trainer` to monitor learning
+              progress in Tensorboard.
     """
     # Initialize Ray server, if not already running
     if not ray.is_initialized():
@@ -187,10 +192,13 @@ def initialize(num_cpus: int = 0,
             # The host to bind the dashboard server to
             dashboard_host="0.0.0.0")
 
-    # Configure Tensorboard
+    # Handling of default log root directory
     if log_root_path is None:
         log_root_path = os.path.join(
             os.path.dirname(os.path.realpath(__file__)), "..", "log")
+    log_root_path = os.path.abspath(log_root_path)
+
+    # Configure Tensorboard
     if 'tb' not in locals().keys():
         tb = TensorBoard()
         tb.configure(host="0.0.0.0", logdir=log_root_path)
@@ -199,10 +207,25 @@ def initialize(num_cpus: int = 0,
             print(f"Started Tensorboard {url}. "
                   f"Root directory: {log_root_path}")
 
-    # Create log directory
-    if log_name is None:
+    # Define log filename interactively if requested
+    if log_name == "":
+        while True:
+            log_name = input(
+                "Enter desired log subdirectory name (empty for default)...")
+            if not log_name or log_name.isidentifier():
+                break
+            else:
+                print("Unvalid name. Only Python identifiers are supported.")
+
+    # Handling of default log name and sanity checks
+    if not log_name:
         log_name = "_".join((datetime.now().strftime("%Y_%m_%d_%H_%M_%S"),
                             socket.gethostname().replace('-', '_')))
+    else:
+        assert log_name.isidentifier(), (
+            "Log name must be a valid Python identifier.")
+
+    # Create log directory
     log_path = os.path.join(log_root_path, log_name)
     pathlib.Path(log_path).mkdir(parents=True, exist_ok=True)
     if verbose:

python/jiminy_py/src/jiminy_py/log.py

@@ -8,7 +8,7 @@
 from itertools import cycle
 from bisect import bisect_right
 from collections import OrderedDict
-from typing import Callable, Tuple, Dict, Optional, Any, Sequence
+from typing import Callable, Tuple, Dict, Optional, Any, Sequence, Union
 
 import h5py
 import numpy as np
@@ -211,7 +211,9 @@ def extract_trajectory_data_from_log(log_data: Dict[str, np.ndarray],
     return traj_data
 
 
-def build_robot_from_log(log_file: str) -> jiminy.Robot:
+def build_robot_from_log(log_file: str,
+                         mesh_package_dirs: Union[str, Sequence[str]] = ()
+                         ) -> jiminy.Robot:
     """Extract log data and build robot from it.
 
     .. note::
@@ -229,23 +231,28 @@ def build_robot_from_log(log_file: str) -> jiminy.Robot:
         archive for now.
 
     :param log_file: Path of the simulation log file, in any format.
+    :param mesh_package_dirs: Prepend custom mesh package seach path
+                              directories to the ones provided by log file. It
+                              may be necessary to specify it to read log
+                              generated on a different environment.
 
     :returns: Reconstructed robot, and parsed log data as returned by
               `jiminy_py.log.read_log` method.
     """
     # Parse log file
     log_data, log_constants = read_log(log_file)
 
+    # Make sure provided 'mesh_package_dirs' is a list
+    mesh_package_dirs = list(mesh_package_dirs)
+
     # Extract robot info
     pinocchio_model_str = log_constants[
         "HighLevelController.pinocchio_model"]
     urdf_file = log_constants["HighLevelController.urdf_file"]
     has_freeflyer = int(log_constants["HighLevelController.has_freeflyer"])
     if "HighLevelController.mesh_package_dirs" in log_constants.keys():
-        mesh_package_dirs = log_constants[
+        mesh_package_dirs += log_constants[
             "HighLevelController.mesh_package_dirs"].split(";")
-    else:
-        mesh_package_dirs = []
     all_options = jiminy.load_config_json_string(
         log_constants["HighLevelController.options"])