[core] Enable to specify update period of registered force profiles. (#…

…319)

* [python/viewer] Greatly improve mesh loading speed. 'set_color' now set colors of both visual and collision meshes.
* [python/viewer] Enable to zoom very close in 'panda3d' without seeing through the model.
* [misc] Fix inter-operability with custom eigenpy/hppfcl/pinocchio install.
* [misc] Add typing to 'panda3d_visualizer' module.
* [core] Increase constants + variable names memory size. Clearer error message in case of overflow.
* [core/python] Automatically reset to Zero the return variable of functors returning by value in C++.
* [python/viewer] Add 'cone' and 'arrow' primitives to 'panda3d' rendering backend.
* [python/viewer] Add methods 'move_node' and 'set_scale' to move and rescale a single node in 'panda3d'.
* [python/viewer] More natural shadow casting in 'panda3d'.
* [python/viewer] Default rendering backend is now 'panda3d' systematically for non-interactive mode.
* [python/viewer] Move named color support from 'replay' module to 'viewer'. Improve default color cycle. 
* [python/viewer] 'set_color' also disables texture if specified, and reset original color and texture otherwise.
* [python/viewer] Increase meshcat opening timeout to avoid false positive on low-end hardware.
* [python/viewer] It is now possible to add custom 'markers', automatically updated at refresh.
* [python/viewer] Fix viewer not properly closed in some cases, and internal state not cleaned-up. 
* [python/viewer] Fix viewer instance of simulator wrongly detected as still available after closing environment.
* [python/viewer] Fix delete-on-close for panda3d.
* [misc] Fix C++ error message not showing properly on Windows.
* [misc] Check if Python executable provided to Cmake is valid.
* [misc] Make sure OpenGL is installed by 'easy_install_deps_ubuntu.sh' script.
* [python/viewer] Only display relevant error messages (if possible) when starting panda3d.
* [python/viewer] Fallback to offscreen rendering if opening graphical window is impossible.
* [core] Enable to specify update period of registered force profiles.
* [core] Make sure system states are cleared at reset.
* [core] Make it impossible to register external forces to the universe itself.

Co-authored-by: Alexis Duburcq <[email protected]>

.github/workflows/manylinux.yml

@@ -46,7 +46,7 @@ jobs:
 
         PYTHON_EXECUTABLE="${pythonLocation}/bin/python${{ matrix.PYTHON_VERSION }}"
         "${PYTHON_EXECUTABLE}" -m pip install --upgrade pip
-        "${PYTHON_EXECUTABLE}" -m pip install twine wheel
+        "${PYTHON_EXECUTABLE}" -m pip install twine wheel auditwheel
         "${PYTHON_EXECUTABLE}" -m pip install cmake numpy
         echo "PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}" >> $GITHUB_ENV
 
@@ -90,7 +90,10 @@ jobs:
         cd "$RootDir/build"
         cmake . -DCOMPONENT=pypi -P ./cmake_install.cmake
 
-        auditwheel repair -w "$RootDir/build/wheelhouse" "$RootDir"/build/pypi/dist/jiminy_py/*.whl
+        # Note that `--strip` option is not used but rather done manually because the
+        # built-in option corrupts the shared library for some reason...
+        "${PYTHON_EXECUTABLE}" "$RootDir/build_tools/wheel_repair_linux.py" repair \
+            -w "$RootDir/build/wheelhouse" "$RootDir"/build/pypi/dist/jiminy_py/*.whl
         "${PYTHON_EXECUTABLE}" -m pip install --force-reinstall --no-deps "$RootDir"/build/wheelhouse/*.whl
     - name: Upload the wheel for Linux of Jiminy_py
       if: success() && github.repository == 'duburcqa/jiminy'
@@ -103,8 +106,6 @@ jobs:
 
     - name: Run unit tests
       run: |
-        export LD_LIBRARY_PATH="$LD_LIBRARY_PATH:$InstallDir/lib:/usr/local/lib"
-
         ./build/unit/unit
 
         cd "$RootDir/unit_py"

.github/workflows/win.yml

@@ -42,7 +42,8 @@ jobs:
       run: |
         git config --global advice.detachedHead false
         python -m pip install --upgrade pip
-        python -m pip install wheel numpy
+        python -m pip install wheel pefile machomachomangler
+        python -m pip install numpy
         python -m pip uninstall -y pipx  # Uninstall unecessary packages causing conflicts with the new resolver
     - name: Build project dependencies
       run: |
@@ -101,12 +102,11 @@ jobs:
         Set-Location -Path "$RootDir/build"
         cmake . -DCOMPONENT=pypi -P ./cmake_install.cmake
 
-        python -m pip install pefile machomachomangler
         Remove-Item -Recurse -Path "$RootDir/build/pypi" -Include *.tar.gz
         Get-ChildItem -Recurse -Path "$RootDir/build/pypi/dist/jiminy_py" -Filter *.whl |
         Foreach-Object {
             $wheel_path = ($_.FullName)
-            python "$RootDir/build_tools/wheel_repair.py" "$wheel_path" -d "$InstallDir/lib" -w "$RootDir/build/wheelhouse"
+            python "$RootDir/build_tools/wheel_repair_win.py" "$wheel_path" -d "$InstallDir/lib" -w "$RootDir/build/wheelhouse"
         }
         Get-ChildItem -Path "$RootDir/build/wheelhouse" -Filter *.whl |
         Foreach-Object {

CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.6.8)
+set(BUILD_VERSION 1.6.9)
 
 # Add definition of Jiminy version for C++ headers
 add_definitions("-DJIMINY_VERSION=\"${BUILD_VERSION}\"")

build_tools/cmake/setupPython.cmake

@@ -20,6 +20,11 @@ if(NOT DEFINED PYTHON_EXECUTABLE)
         endif()
         set(PYTHON_EXECUTABLE "${Python_EXECUTABLE}")
     endif()
+else()
+    find_program(PYTHON_EXECUTABLE python PATHS "${PYTHON_EXECUTABLE}" NO_DEFAULT_PATH)
+    if(NOT PYTHON_EXECUTABLE)
+        message(FATAL_ERROR "User-specified Python executable not valid, CMake will exit.")
+    endif()
 endif()
 
 execute_process(COMMAND "${PYTHON_EXECUTABLE}" -c

build_tools/easy_install_deps_ubuntu.sh

@@ -32,17 +32,20 @@ sudo -u $(id -nu "$SUDO_UID") python3 -m pip install wheel && \
 sudo -u $(id -nu "$SUDO_UID") python3 -m pip install "numpy>=1.16"
 
 # Install Python 3 toolsuite for testing and documentation generation
+sudo -u $(id -nu "$SUDO_UID") python3 -m pip install --upgrade setuptools auditwheel && \
+sudo -u $(id -nu "$SUDO_UID") python3 -m pip install --upgrade flake8 pylint mypy && \
 sudo -u $(id -nu "$SUDO_UID") python3 -m pip install --upgrade \
-    setuptools pygments colorama \
-    flake8 pylint mypy \
-    sphinx sphinx_rtd_theme recommonmark nbsphinx breathe aafigure
+    pygments colorama sphinx sphinx_rtd_theme recommonmark nbsphinx breathe aafigure
 
 # Install standard linux utilities
 apt install -y gnupg curl wget build-essential cmake doxygen graphviz pandoc
 
 # Install some additional dependencies
 apt install -y libeigen3-dev libboost-all-dev liboctomap-dev
 
+# Install OpenGL
+apt install -y mesa-utils
+
 # Install robotpkg tools suite
 if ! [ -d "/opt/openrobots/lib/${PYTHON_BIN}/site-packages/" ] ; then
     # Add apt repository if necessary

build_tools/wheel_repair_linux.py

@@ -0,0 +1,45 @@
+import os
+import sys
+import stat
+import shutil
+import itertools
+from typing import Tuple
+
+import auditwheel.repair
+from auditwheel.repair import logger
+from auditwheel.elfutils import elf_read_rpaths
+from auditwheel.patcher import ElfPatcher
+from auditwheel.main import main
+
+
+copylib_orig = auditwheel.repair.copylib
+
+def copylib(src_path: str, dest_dir: str,
+            patcher: ElfPatcher) -> Tuple[str, str]:
+    # Do NOT hash filename to make it unique in the particular case of boost
+    # python modules, since otherwise it will be impossible to share a common
+    # registery, which is necessary for cross module interoperability.
+    if "libboost_python" in src_path:
+        src_name = os.path.basename(src_path)
+        dest_path = os.path.join(dest_dir, src_name)
+        if os.path.exists(dest_path):
+            return src_name, dest_path
+
+        logger.debug('Grafting: %s -> %s', src_path, dest_path)
+        shutil.copy2(src_path, dest_path)
+        rpaths = elf_read_rpaths(src_path)
+        statinfo = os.stat(dest_path)
+        if not statinfo.st_mode & stat.S_IWRITE:
+            os.chmod(dest_path, statinfo.st_mode | stat.S_IWRITE)
+        patcher.set_soname(dest_path, src_name)
+        if any(itertools.chain(rpaths['rpaths'], rpaths['runpaths'])):
+            patcher.set_rpath(dest_path, dest_dir)
+
+        return src_name, dest_path
+    return copylib_orig(src_path, dest_dir, patcher)
+
+auditwheel.repair.copylib = copylib
+
+
+if __name__ == '__main__':
+    sys.exit(main())

build_tools/wheel_repair.py → build_tools/wheel_repair_win.py

@@ -19,16 +19,25 @@
 
 
 def hash_filename(filepath, blocksize=65536):
-    hasher = hashlib.sha256()
+    # Split original filename from extension
+    root, ext = os.path.splitext(filepath)
+    filename = os.path.basename(root)
+
+    # Do NOT hash filename to make it unique in the particular case of boost
+    # python modules, since otherwise it will be impossible to share a common
+    # registery, which is necessary for cross module interoperability.
+    if "boost_python" in filepath:
+        return f"{filename}{ext}"
 
+    # Compute unique hash based on file's content
+    hasher = hashlib.sha256()
     with open(filepath, "rb") as afile:
         buf = afile.read(blocksize)
         while len(buf) > 0:
             hasher.update(buf)
             buf = afile.read(blocksize)
 
-    root, ext = os.path.splitext(filepath)
-    return f"{os.path.basename(root)}-{hasher.hexdigest()[:8]}{ext}"
+    return f"{filename}-{hasher.hexdigest()[:8]}{ext}"
 
 
 def find_dll_dependencies(dll_filepath, lib_dir):
@@ -99,24 +108,25 @@ def mangle_filename(old_filename, new_filename, mapping):
         rel_path = next(path for path in pyd_rel_paths if path.endswith(dll))
 
     for dep in dependencies:
-        hashed_name = hash_filename(os.path.join(args.DLL_DIR, dep))  # already basename
+        src_path = os.path.join(args.DLL_DIR, dep)
+        hashed_name = hash_filename(src_path)  # already basename
+        new_path = os.path.join(bundle_path, hashed_name)
         if dll.endswith(".pyd"):
             bundle_rel_path = os.path.join(
                 "..\\" * rel_path.count(os.path.sep), bundle_name)
             mapping[dep.encode("ascii")] = os.path.join(
                 bundle_rel_path, hashed_name).encode("ascii")
         else:
             mapping[dep.encode("ascii")] = hashed_name.encode("ascii")
-        shutil.copy(
-            os.path.join(args.DLL_DIR, dep),
-            os.path.join(bundle_path, hashed_name))
+        if not os.path.exists(new_path):
+            shutil.copy2(src_path, new_path)
 
     if dll.endswith(".pyd"):
         old_name = os.path.join(old_wheel_dir, rel_path)
         new_name = os.path.join(new_wheel_dir, rel_path)
     else:
         old_name = os.path.join(args.DLL_DIR, dll)
-        hashed_name = hash_filename(os.path.join(args.DLL_DIR, dll))  # already basename
+        hashed_name = hash_filename(old_name)  # already basename
         new_name = os.path.join(bundle_path, hashed_name)
 
     mangle_filename(old_name, new_name, mapping)

core/include/jiminy/core/Macros.h

@@ -37,7 +37,7 @@ namespace jiminy
     template <class F, class... Args>
     void do_for(F f, Args... args)
     {
-        int x[] = {(f(args), 0)...};
+        (f(args), ...);
     }
 
     template<class F, class dF=std::decay_t<F> >
@@ -73,6 +73,24 @@ namespace jiminy
         return std::static_pointer_cast<T>(shared_from_base(derived));
     }
 
+    // ======================== is_iterator ===========================
+
+    template<typename T, typename = void>
+    struct is_iterator
+    {
+        static constexpr bool value = false;
+    };
+
+    template<typename T>
+    struct is_iterator<T, typename std::enable_if<!std::is_same<
+        typename std::iterator_traits<T>::value_type, void>::value>::type>
+    {
+        static constexpr bool value = true;
+    };
+
+    template<typename T>
+    inline constexpr bool is_iterator_v = is_iterator<T>::value;
+
     // ======================== is_vector ===========================
 
     template<typename T>
@@ -263,14 +281,18 @@ namespace jiminy
 
     #define FILE_LINE __FILE__ ":" STRINGIFY(__LINE__)
 
+    /* ANSI escape codes is used here as a cross-platform way to color text.
+       For reference, see:
+       https://solarianprogrammer.com/2019/04/08/c-programming-ansi-escape-codes-windows-macos-linux-terminals/ */
+
     #define PRINT_ERROR(...) \
-    std::cerr << "In " FILE_LINE ": In " << BOOST_CURRENT_FUNCTION << ":\n\033[1;31merror:\033[0m " << to_string(__VA_ARGS__) << std::endl
+    std::cerr << "In " FILE_LINE ": In " << BOOST_CURRENT_FUNCTION << ":\n\x1b[1;31merror:\x1b[0m " << to_string(__VA_ARGS__) << std::endl
 
     #ifdef NDEBUG
-    #define PRINT_WARNING(...)
+        #define PRINT_WARNING(...)
     #else
-    #define PRINT_WARNING(...) \
-    std::cerr << "In " FILE_LINE ": In " << BOOST_CURRENT_FUNCTION << ":\n\033[1;93mwarning:\033[0m " << to_string(__VA_ARGS__) << std::endl
+        #define PRINT_WARNING(...) \
+        std::cerr << "In " FILE_LINE ": In " << BOOST_CURRENT_FUNCTION << ":\n\x1b[1;93mwarning:\x1b[0m " << to_string(__VA_ARGS__) << std::endl
     #endif
 }
 

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

@@ -68,8 +68,9 @@ namespace jiminy
                                        float64_t        const & t,
                                        float64_t        const & dt,
                                        pinocchio::Force const & F);
-        hresult_t registerForceProfile(std::string           const & frameName,
-                                       forceProfileFunctor_t         forceFct);
+        hresult_t registerForceProfile(std::string const & frameName,
+                                       forceProfileFunctor_t const & forceFct,
+                                       float64_t const & updatePeriod = 0.0);
 
         // Redefined to take advantage of C++ name hiding of overloaded methods of base class in dervied class
         hresult_t removeForcesImpulse(void);

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

@@ -457,11 +457,13 @@ namespace jiminy
                                        float64_t        const & t,
                                        float64_t        const & dt,
                                        pinocchio::Force const & F);
-        /// \brief Apply an time-continuous external force on a frame.
-        ///        The force can be time and state dependent, and must be given in the world frame.
+        /// \brief Apply an external force profile on a frame.
+        ///        It can be either time-continuous or discrete. The force can be time
+        ///        and state dependent, and must be given in the world frame.
         hresult_t registerForceProfile(std::string const & systemName,
                                        std::string const & frameName,
-                                       forceProfileFunctor_t forceFct);
+                                       forceProfileFunctor_t const & forceFct,
+                                       float64_t const & updatePeriod = 0.0);
 
         hresult_t removeForcesImpulse(std::string const & systemName);
         hresult_t removeForcesProfile(std::string const & systemName);
@@ -550,11 +552,11 @@ namespace jiminy
                                     vectorN_t          const & v,
                                     forceVector_t            & fext) const;
         void computeExternalForces(systemHolder_t     const & system,
-                                   systemDataHolder_t const & systemData,
+                                   systemDataHolder_t       & systemData,
                                    float64_t          const & t,
                                    vectorN_t          const & q,
                                    vectorN_t          const & v,
-                                   forceVector_t            & fext) const;
+                                   forceVector_t            & fext);
         void computeForcesCoupling(float64_t              const & t,
                                    std::vector<vectorN_t> const & qSplit,
                                    std::vector<vectorN_t> const & vSplit);

core/include/jiminy/core/engine/System.h

@@ -23,12 +23,15 @@ namespace jiminy
         forceProfile_t(void) = default;
         forceProfile_t(std::string           const & frameNameIn,
                        int32_t               const & frameIdxIn,
+                       float64_t             const & updatePeriodIn,
                        forceProfileFunctor_t const & forceFctIn);
         ~forceProfile_t(void) = default;
 
     public:
         std::string frameName;
         int32_t frameIdx;
+        float64_t updatePeriod;
+        pinocchio::Force forcePrev;
         forceProfileFunctor_t forceFct;
     };
 
@@ -111,6 +114,8 @@ namespace jiminy
         hresult_t initialize(Robot const & robot);
         bool_t const & getIsInitialized(void) const;
 
+        void clear(void);
+
     public:
         vectorN_t q;
         vectorN_t v;

core/include/jiminy/core/telemetry/TelemetryData.h

@@ -27,7 +27,7 @@ namespace jiminy
     std::string const START_LINE_TOKEN("StartLine");      ///< Marker of the beginning of a line of data.
     std::string const START_DATA("StartData");            ///< Marker of the beginning of the data section.
 
-    std::size_t const CONSTANTS_MEM_SIZE = 1024U * 1024U;
+    std::size_t const CONSTANTS_MEM_SIZE = 2U * 1024U * 1024U;
     std::size_t const INTEGERS_MEM_SIZE  = 32U * 1024U;
     std::size_t const FLOATS_MEM_SIZE    = 42U * 1024U;
 

core/include/jiminy/core/telemetry/TelemetryData.tpp

@@ -46,7 +46,7 @@ namespace jiminy
 
         if ((header->nextFreeNameOffset + static_cast<int64_t>(variableName.size()) + 1) >= header->startDataSection)
         {
-            PRINT_ERROR("Unspecified error.");  // TODO: write an appropriate error message
+            PRINT_ERROR("Trying to allocate too much memory to hold constants and variable names.");
             return hresult_t::ERROR_GENERIC;
         }
 

core/include/jiminy/core/utilities/Helpers.h

@@ -121,6 +121,30 @@ namespace jiminy
                     float64_t const & minThr = -INF,
                     float64_t const & maxThr = +INF);
 
+    template<typename InputIt, typename Function>
+    inline constexpr bool is_unary_functor_v = std::is_invocable_r_v<
+        float64_t, Function, typename std::iterator_traits<InputIt>::value_type>;
+
+    template<typename... Args>
+    float64_t minClipped(float64_t val1, float64_t val2, Args ... vs);
+
+    template<typename InputIt, typename UnaryFunction, typename ...Args>
+    std::enable_if_t<is_iterator_v<InputIt> && is_unary_functor_v<InputIt, UnaryFunction>,
+        std::tuple<bool_t, float64_t> >
+    isGcdIncluded(InputIt first, InputIt last, UnaryFunction f, Args... values);
+
+    template<typename InputIt, typename UnaryFunction>
+    std::enable_if_t<is_iterator_v<InputIt> && is_unary_functor_v<InputIt, UnaryFunction>,
+        std::tuple<bool_t, float64_t> >
+    isGcdIncluded(InputIt first, InputIt last, UnaryFunction f);
+
+    template<typename InputIt>
+    std::enable_if_t<is_iterator_v<InputIt>, std::tuple<bool_t, float64_t> >
+    isGcdIncluded(InputIt first, InputIt last);
+
+    template<typename ...Args>
+    std::tuple<bool_t, float64_t> isGcdIncluded(Args... values);
+
     // ********************* Std::vector helpers **********************
 
     template<typename T>