From a1e571b7b45df3ddb76f5da05f09fb75f9a97c25 Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Wed, 2 Mar 2022 11:11:21 +0100 Subject: [PATCH] Jiminy 1.7.7 (#486) * [core] Always regenerate original flexible model at reset to allow manual overwrite of rigid original model. (#475) * [core] Fix segfault when adding unspecified constraint. (#476) * [core] Fix 'buildReducedModel' for pinocchio < 2.6.0. (#477) * [core] Fix segfault when initializing engine with null robot. (#477) * [python/viewer] Fallback to generic direct connection through ipykernel for Meshcat (VSCode, Jupyterlab). (#479) * [python/viewer] More robust comm filtering for executing meshcat related requests out of order. (#479) * [python/viewer] Fix connection to existing meshcat process. (#479) * [python/viewer] Speed up frame capture and reduce CPU load in offscreen mode for panda3d backend. (#481) * [python/viewer] Drop support of 'gepetto-gui' backend. (#482) * [python/viewer] Fix random segfault at exit for synchronous panda3d backend. (#484) * [gym/common] Enable to provide custom robot to 'WalkerJiminyEnv'. (#477) * [gym/rllib] Refactor PPO implementation to support 'ray>=1.10.0'. (#474) (#485) * [gym/rllib] PPO supports dict and box spaces for observation/action. (#474) * [gym/rllib] PPO L2 reg only on train params. (#485) * [misc] Use Ctest for managing C++ unit tests. (#475) Co-authored-by: Alexis Duburcq --- .github/workflows/linux.yml | 5 +- .github/workflows/macos.yml | 5 +- .github/workflows/manylinux.yml | 7 +- .github/workflows/ubuntu.yml | 19 +- .github/workflows/win.yml | 5 +- CMakeLists.txt | 4 +- INSTALL.md | 20 +- build_tools/easy_install_deps_ubuntu.sh | 5 +- build_tools/wheel_repair_linux.py | 2 +- build_tools/wheel_repair_win.py | 2 +- .../core/constraints/DistanceConstraint.h | 4 +- .../core/constraints/FixedFrameConstraint.h | 20 +- .../core/constraints/SphereConstraint.h | 2 +- .../jiminy/core/constraints/WheelConstraint.h | 20 +- .../jiminy/core/engine/EngineMultiRobot.h | 8 +- core/include/jiminy/core/engine/System.h | 2 +- .../jiminy/core/robot/AbstractSensor.tpp | 2 +- core/include/jiminy/core/robot/Model.h | 2 +- .../core/robot/PinocchioOverloadAlgorithms.h | 10 +- .../core/stepper/AbstractRungeKuttaStepper.h | 2 +- .../jiminy/core/stepper/AbstractStepper.h | 2 +- .../core/stepper/EulerExplicitStepper.h | 2 +- .../jiminy/core/stepper/RungeKutta4Stepper.h | 2 +- .../core/stepper/RungeKuttaDOPRIStepper.h | 2 +- .../include/jiminy/core/utilities/Pinocchio.h | 7 + core/src/constraints/AbstractConstraint.cc | 2 +- core/src/constraints/DistanceConstraint.cc | 14 +- core/src/constraints/FixedFrameConstraint.cc | 15 +- core/src/constraints/JointConstraint.cc | 11 +- core/src/constraints/SphereConstraint.cc | 10 +- core/src/constraints/WheelConstraint.cc | 10 +- core/src/control/AbstractController.cc | 2 +- core/src/engine/Engine.cc | 4 +- core/src/engine/EngineMultiRobot.cc | 38 +- core/src/engine/System.cc | 34 +- core/src/io/JsonLoader.cc | 2 +- core/src/robot/AbstractSensor.cc | 2 +- core/src/robot/Model.cc | 75 +- core/src/robot/Robot.cc | 2 +- core/src/solver/LCPSolvers.cc | 13 +- core/src/stepper/AbstractRungeKuttaStepper.cc | 2 +- core/src/stepper/AbstractStepper.cc | 4 +- core/src/stepper/EulerExplicitStepper.cc | 2 +- core/src/stepper/RungeKutta4Stepper.cc | 2 +- core/src/stepper/RungeKuttaDOPRIStepper.cc | 2 +- core/src/telemetry/TelemetrySender.cc | 2 +- core/src/utilities/Pinocchio.cc | 27 +- core/src/utilities/Random.cc | 36 +- examples/python/tutorial.ipynb | 6 +- .../gym_jiminy/common/bases/block_bases.py | 5 +- .../gym_jiminy/common/bases/generic_bases.py | 6 +- .../gym_jiminy/common/bases/pipeline_bases.py | 26 +- .../gym_jiminy/common/envs/env_generic.py | 12 +- .../gym_jiminy/common/envs/env_locomotion.py | 38 +- .../gym_jiminy/common/envs/internal/play.py | 20 +- .../common/gym_jiminy/common/utils/spaces.py | 83 +- python/gym_jiminy/common/setup.py | 2 +- python/gym_jiminy/envs/gym_jiminy/envs/ant.py | 10 +- .../gym_jiminy/envs/gym_jiminy/envs/anymal.py | 16 +- .../gym_jiminy/envs/gym_jiminy/envs/atlas.py | 110 ++- .../gym_jiminy/envs/gym_jiminy/envs/cassie.py | 84 +- .../envs/gym_jiminy/envs/spotmicro.py | 16 +- .../rllib/gym_jiminy/rllib/__init__.py | 43 - .../rllib/gym_jiminy/rllib/callbacks.py | 57 +- .../rllib/gym_jiminy/rllib/curriculum.py | 25 +- .../gym_jiminy/rllib/gym_jiminy/rllib/ppo.py | 763 ++++++++++-------- .../rllib/gym_jiminy/rllib/utilities.py | 33 +- python/gym_jiminy/rllib/setup.py | 7 +- .../toolbox/gym_jiminy/toolbox/math/qhull.py | 4 +- .../data/cassie_standing_panda3d_2.png | Bin 6678 -> 6573 bytes python/jiminy_py/setup.py | 14 +- .../jiminy_py/src/jiminy_py/core/__init__.py | 6 +- .../src/jiminy_py/core/_pinocchio_init.py | 19 - python/jiminy_py/src/jiminy_py/robot.py | 10 +- python/jiminy_py/src/jiminy_py/simulator.py | 4 - .../src/jiminy_py/viewer/meshcat/index.html | 127 ++- .../src/jiminy_py/viewer/meshcat/server.py | 1 + .../src/jiminy_py/viewer/meshcat/wrapper.py | 38 +- .../viewer/panda3d/panda3d_visualizer.py | 84 +- .../viewer/panda3d/panda3d_widget.py | 55 +- .../jiminy_py/src/jiminy_py/viewer/replay.py | 27 +- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 390 ++------- .../include/jiminy/python/Utilities.h | 2 +- python/jiminy_pywrap/src/Helpers.cc | 20 +- unit/CMakeLists.txt | 9 +- 85 files changed, 1336 insertions(+), 1308 deletions(-) delete mode 100644 python/jiminy_py/src/jiminy_py/core/_pinocchio_init.py diff --git a/.github/workflows/linux.yml b/.github/workflows/linux.yml index 46b15555e..7fdf2c6c1 100644 --- a/.github/workflows/linux.yml +++ b/.github/workflows/linux.yml @@ -47,7 +47,7 @@ jobs: "${PYTHON_EXECUTABLE}" -m pip install --upgrade pip "${PYTHON_EXECUTABLE}" -m pip install --upgrade wheel - "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy + "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.22" # for numba compat. echo "RootDir=${GITHUB_WORKSPACE}" >> $GITHUB_ENV echo "InstallDir=${GITHUB_WORKSPACE}/install" >> $GITHUB_ENV @@ -103,7 +103,8 @@ jobs: run: | export LD_LIBRARY_PATH="$InstallDir/lib:$InstallDir/lib64:/usr/local/lib" - ./build/unit/unit + cd "$RootDir/build" + ctest cd "$RootDir/unit_py" "${PYTHON_EXECUTABLE}" -m unittest discover -v diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index f67bf008c..cb9318b01 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -66,7 +66,7 @@ jobs: "${PYTHON_EXECUTABLE}" -m pip install --upgrade pip "${PYTHON_EXECUTABLE}" -m pip install --upgrade twine wheel delocate - "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy + "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.22" # for numba compat. - name: Build project dependencies run: | MACOSX_DEPLOYMENT_TARGET=${MACOSX_DEPLOYMENT_TARGET} OSX_ARCHITECTURES=${OSX_ARCHITECTURES} \ @@ -130,7 +130,8 @@ jobs: - name: Run unit tests run: | - ./build/unit/unit + cd "$RootDir/build" + ctest cd "$RootDir/unit_py" "${PYTHON_EXECUTABLE}" -m unittest discover -v diff --git a/.github/workflows/manylinux.yml b/.github/workflows/manylinux.yml index 3626e8a76..8b42f1199 100644 --- a/.github/workflows/manylinux.yml +++ b/.github/workflows/manylinux.yml @@ -59,9 +59,9 @@ jobs: "${PYTHON_EXECUTABLE}" -m pip install --upgrade pip "${PYTHON_EXECUTABLE}" -m pip install --upgrade twine wheel auditwheel cmake if [ "${{ matrix.legacy }}" == true ] ; then - "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.20" + "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.20" # for tensorflow compat. else - "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy + "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.22" # for numba compat. fi - name: Build project dependencies run: | @@ -124,7 +124,8 @@ jobs: - name: Run unit tests run: | - ./build/unit/unit + cd "$RootDir/build" + ctest cd "$RootDir/unit_py" "${PYTHON_EXECUTABLE}" -m unittest discover -v diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 03a367e98..cdc0ee3f8 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -18,9 +18,9 @@ jobs: matrix: include: - os: ubuntu-18.04 - BUILD_TYPE: 'Release' - - os: ubuntu-20.04 BUILD_TYPE: 'Debug' + - os: ubuntu-20.04 + BUILD_TYPE: 'Release' defaults: run: @@ -51,15 +51,14 @@ jobs: run: | sudo env "PATH=$PATH" "${GITHUB_WORKSPACE}/build_tools/easy_install_deps_ubuntu.sh" "${PYTHON_EXECUTABLE}" -m pip install tensorflow - "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy + "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy<1.22" # for numba compat. "${PYTHON_EXECUTABLE}" -m pip install "torch==1.8.0+cpu" -f https://download.pytorch.org/whl/torch_stable.html "${PYTHON_EXECUTABLE}" -m pip install --prefer-binary "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0" - "${PYTHON_EXECUTABLE}" -m pip install "ray[default,rllib]<=1.4.0" # Type checking is not working with 1.4.1 ##################################################################################### - name: PEP8 Code Style Check - if: matrix.os == 'ubuntu-18.04' + if: matrix.os == 'ubuntu-20.04' run: | flake8 --ignore=E121,E126,E123,E226,E241,E266,E402,F405,W504 --count --show-source --statistics "$RootDir/python" @@ -93,7 +92,8 @@ jobs: - name: Run jiminy unit tests run: | - "$RootDir/build/unit/unit" + cd "$RootDir/build" + ctest cd "$RootDir/unit_py" "${PYTHON_EXECUTABLE}" -m unittest discover -v @@ -110,7 +110,7 @@ jobs: # Ubuntu 18 is distributed with Python3.6, which is not supported by Numpy>=1.20. # The new type check support of Numpy is raising pylint and mypy errors, so Ubuntu 18 # is used to do type checking for now. - if: matrix.os == 'ubuntu-18.04' + if: matrix.os == 'ubuntu-20.04' run: | gym_modules=( "common" @@ -120,12 +120,13 @@ jobs: for name in "${gym_modules[@]}"; do cd "$RootDir/python/gym_jiminy/$name" - pylint --unsafe-load-any-extension=y --ignore-imports=y --min-similarity-lines=7 --max-nested-blocks=7 \ + pylint --unsafe-load-any-extension=y --ignore-imports=y --min-similarity-lines=20 --max-nested-blocks=7 \ --good-names=i,j,k,t,q,v,x,e,u,s,v,b,c,f,M,dt,rg,fd,lo,hi,tb,_ \ --disable=fixme,abstract-method,protected-access,useless-super-delegation \ --disable=too-many-instance-attributes,too-many-arguments,too-few-public-methods,too-many-lines \ --disable=too-many-locals,too-many-branches,too-many-statements \ --disable=unspecified-encoding,logging-fstring-interpolation \ + --disable=misplaced-comparison-constant --disable=cyclic-import \ --generated-members=numpy.*,torch.* "gym_jiminy/" mypy --allow-redefinition --check-untyped-defs --disallow-incomplete-defs --disallow-untyped-defs \ @@ -143,7 +144,7 @@ jobs: cmake . -DCOMPONENT=docs -P ./cmake_install.cmake - name: Deploy to GitHub Pages if: >- - matrix.os == 'ubuntu-18.04' && success() && + matrix.os == 'ubuntu-20.04' && success() && github.repository == 'duburcqa/jiminy' && github.event_name == 'push' && github.ref == 'refs/heads/master' uses: crazy-max/ghaction-github-pages@v2 with: diff --git a/.github/workflows/win.yml b/.github/workflows/win.yml index 227a1dd4c..a3aa245f7 100644 --- a/.github/workflows/win.yml +++ b/.github/workflows/win.yml @@ -45,7 +45,7 @@ jobs: git config --global advice.detachedHead false python -m pip install --upgrade pip python -m pip install --upgrade wheel pefile machomachomangler - python -m pip install --upgrade numpy + python -m pip install --upgrade "numpy<1.22" # for numba compat. python -m pip uninstall -y pipx # Uninstall unecessary packages causing conflicts with the new resolver - name: Build project dependencies run: | @@ -153,7 +153,8 @@ jobs: run: | $RootDir = "${env:GITHUB_WORKSPACE}" -replace '\\', '/' - & "$RootDir/build/unit\${env:BUILD_TYPE}/unit.exe" + Set-Location -Path "$RootDir/build" + ctest Set-Location -Path "$RootDir/unit_py" python -m unittest discover -v diff --git a/CMakeLists.txt b/CMakeLists.txt index 22bf8dda3..7a255b956 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10) # Set the build version -set(BUILD_VERSION 1.7.6) +set(BUILD_VERSION 1.7.7) # Set compatibility if(CMAKE_VERSION VERSION_GREATER "3.11.0") @@ -64,6 +64,8 @@ endif() option(BUILD_TESTING "Build the C++ unit tests." ON) if(BUILD_TESTING) + include(CTest) + include(GoogleTest) add_subdirectory(unit) endif() diff --git a/INSTALL.md b/INSTALL.md index 8fe0981bc..8b445c50f 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -6,25 +6,7 @@ #### Dependencies installation -There is not requirement to install `jiminy_py` on linux if one does not want to build it. Nevertheless, this package does not provide the backend viewer `gepetto-gui` (still, the backend `meshcat` is available). - -##### (optional) Gepetto viewer - -The first step to install `gepetto-gui` is to setup the APT repository `robotpkg` to have access to compiled binaries. - -```bash -sh -c "echo 'deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub bionic robotpkg' >> /etc/apt/sources.list.d/robotpkg.list" && \ -curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add - -apt update -``` - -Once done, it is straightforward to install the required package for Python 3.6. - -For Python 3.6 - -```bash -sudo apt install -y robotpkg-gepetto-viewer=4.4.0 robotpkg-py36-qt4-gepetto-viewer-corba=5.1.2 robotpkg-py36-omniorbpy -``` +There is no requirement to install `jiminy_py` on linux if one does not want to build it. #### Install Jiminy Python package diff --git a/build_tools/easy_install_deps_ubuntu.sh b/build_tools/easy_install_deps_ubuntu.sh index e784fb0f5..c742a14c2 100755 --- a/build_tools/easy_install_deps_ubuntu.sh +++ b/build_tools/easy_install_deps_ubuntu.sh @@ -53,7 +53,7 @@ apt update && \ apt install -y sudo python3-setuptools python3-pip python3-tk && \ sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgrade pip && \ sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgrade wheel && \ -sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgrade "numpy>=1.16" +sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgrade "numpy>=1.16,<1.22" # Install Python 3 toolsuite for testing and documentation generation sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgrade setuptools auditwheel && \ @@ -81,8 +81,7 @@ fi # Note that `apt-get` is used instead of `apt` because it supports wildcard in package names apt-get install -y --allow-downgrades --allow-unauthenticated \ robotpkg-octomap=1.9.0 robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=1.7.1 robotpkg-pinocchio=2.5.6 \ - robotpkg-qt5-osgqt=3.5.7r2 robotpkg-py3*-qt5-gepetto-viewer=4.12.0r2 robotpkg-py3*-qt5-gepetto-viewer-corba=5.6.0 \ - robotpkg-py3*-omniorbpy=4.2.4 robotpkg-py3*-eigenpy=2.6.2 robotpkg-py3*-hpp-fcl=1.7.1 robotpkg-py3*-pinocchio=2.5.6 + robotpkg-py3*-eigenpy=2.6.2 robotpkg-py3*-hpp-fcl=1.7.1 robotpkg-py3*-pinocchio=2.5.6 # Add openrobots libraries to python packages search path if ! [ -f "${PYTHON_SITELIB}/openrobots.pth" ]; then diff --git a/build_tools/wheel_repair_linux.py b/build_tools/wheel_repair_linux.py index 2b9cc9124..1bb8c7361 100644 --- a/build_tools/wheel_repair_linux.py +++ b/build_tools/wheel_repair_linux.py @@ -18,7 +18,7 @@ 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. + # registry, 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) diff --git a/build_tools/wheel_repair_win.py b/build_tools/wheel_repair_win.py index 09a0cb6de..38fdd5393 100644 --- a/build_tools/wheel_repair_win.py +++ b/build_tools/wheel_repair_win.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # This tool has been copied from https://github.com/vinayak-mehta/pdftopng/blob/main/scripts/wheel_repair.py -# and extended to supported hierarchical folder architecture with mulitple .pyd +# and extended to supported hierarchical folder architecture with multiple .pyd # to update, and to move all the DLL in a common folder *package*.lib installed # jointly with the package itself, similarly to auditwheel on Linux platform. #(see also https://discuss.python.org/t/delocate-auditwheel-but-for-windows/2589/9). diff --git a/core/include/jiminy/core/constraints/DistanceConstraint.h b/core/include/jiminy/core/constraints/DistanceConstraint.h index 084f3cd45..8ae1f3086 100644 --- a/core/include/jiminy/core/constraints/DistanceConstraint.h +++ b/core/include/jiminy/core/constraints/DistanceConstraint.h @@ -43,8 +43,8 @@ namespace jiminy std::vector framesNames_; ///< Names of the frames on which the constraint operates. std::vector framesIdx_; ///< Corresponding frames indices. float64_t distanceRef_; ///< Reference Distance between the frames - matrixN_t firstFrameJacobian_; ///< Stores first frame jacobian in world. - matrixN_t secondFrameJacobian_; ///< Stores second frame jacobian in world. + matrix6N_t firstFrameJacobian_; ///< Stores first frame jacobian in world. + matrix6N_t secondFrameJacobian_; ///< Stores second frame jacobian in world. }; } diff --git a/core/include/jiminy/core/constraints/FixedFrameConstraint.h b/core/include/jiminy/core/constraints/FixedFrameConstraint.h index d96d3b763..2b48bdfcf 100644 --- a/core/include/jiminy/core/constraints/FixedFrameConstraint.h +++ b/core/include/jiminy/core/constraints/FixedFrameConstraint.h @@ -60,16 +60,16 @@ namespace jiminy vectorN_t const & v) override final; private: - std::string const frameName_; ///< Name of the frame on which the constraint operates. - frameIndex_t frameIdx_; ///< Corresponding frame index. - std::vector dofsFixed_; ///< Degrees of freedom to fix. - bool_t isFixedPositionXY_; ///< Whether or not the frame is fixed for both X and Y translations - pinocchio::SE3 transformRef_; ///< Reference pose of the frame to enforce. - vector3_t normal_; ///< Normal direction locally at the interface. - matrix3_t rotationLocal_; ///< Rotation matrix of the local frame in which to apply masking - matrix6N_t frameJacobian_; ///< Stores full frame jacobian in reference frame. - pinocchio::Motion frameDrift_; ///< Stores full frame drift in reference frame. - matrixN_t UiJt_; ///< Used to store intermediary computation to compute (J.Minv.Jt)_{i,i} + std::string const frameName_; ///< Name of the frame on which the constraint operates. + frameIndex_t frameIdx_; ///< Corresponding frame index. + std::vector dofsFixed_; ///< Degrees of freedom to fix. + bool_t isFixedPositionXY_; ///< Whether or not the frame is fixed for both X and Y translations + pinocchio::SE3 transformRef_; ///< Reference pose of the frame to enforce. + vector3_t normal_; ///< Normal direction locally at the interface. + matrix3_t rotationLocal_; ///< Rotation matrix of the local frame in which to apply masking + matrix6N_t frameJacobian_; ///< Stores full frame jacobian in reference frame. + pinocchio::Motion frameDrift_; ///< Stores full frame drift in reference frame. + Eigen::Matrix UiJt_; ///< Used to store intermediary computation to compute diag(J.Minv.Jt)_t }; } diff --git a/core/include/jiminy/core/constraints/SphereConstraint.h b/core/include/jiminy/core/constraints/SphereConstraint.h index 360369c0d..4d5207a9a 100644 --- a/core/include/jiminy/core/constraints/SphereConstraint.h +++ b/core/include/jiminy/core/constraints/SphereConstraint.h @@ -65,7 +65,7 @@ namespace jiminy vector3_t normal_; ///< Ground normal, world frame. matrix3_t shewRadius_; ///< Skew of ground normal, in world frame, scaled by radius. pinocchio::SE3 transformRef_; ///< Reference pose of the frame to enforce. - matrixN_t frameJacobian_; ///< Stores full frame jacobian in world. + matrix6N_t frameJacobian_; ///< Stores full frame jacobian in world. }; } diff --git a/core/include/jiminy/core/constraints/WheelConstraint.h b/core/include/jiminy/core/constraints/WheelConstraint.h index 00cd2b9a4..0de9a775f 100644 --- a/core/include/jiminy/core/constraints/WheelConstraint.h +++ b/core/include/jiminy/core/constraints/WheelConstraint.h @@ -61,16 +61,16 @@ namespace jiminy vectorN_t const & v) override final; private: - std::string frameName_; ///< Name of the frame on which the constraint operates. - frameIndex_t frameIdx_; ///< Corresponding frame index. - float64_t radius_; ///< Wheel radius. - vector3_t normal_; ///< Ground normal, world frame. - vector3_t axis_; ///< Wheel axis, local frame. - vector3_t x3_; ///< Wheel axis, world frame. - matrix3_t skewRadius_; ///< Skew matrix of wheel axis, in world frame, scaled by radius. - matrix3_t dskewRadius_; ///< Derivative of skew matrix of wheel axis, in world frame, scaled by radius. - pinocchio::SE3 transformRef_; ///< Reference pose of the frame to enforce. - matrixN_t frameJacobian_; ///< Stores full frame jacobian in world. + std::string frameName_; ///< Name of the frame on which the constraint operates. + frameIndex_t frameIdx_; ///< Corresponding frame index. + float64_t radius_; ///< Wheel radius. + vector3_t normal_; ///< Ground normal, world frame. + vector3_t axis_; ///< Wheel axis, local frame. + vector3_t x3_; ///< Wheel axis, world frame. + matrix3_t skewRadius_; ///< Skew matrix of wheel axis, in world frame, scaled by radius. + matrix3_t dskewRadius_; ///< Derivative of skew matrix of wheel axis, in world frame, scaled by radius. + pinocchio::SE3 transformRef_; ///< Reference pose of the frame to enforce. + matrix6N_t frameJacobian_; ///< Stores full frame jacobian in world. }; } diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h index 811dd17ec..22ecf5133 100644 --- a/core/include/jiminy/core/engine/EngineMultiRobot.h +++ b/core/include/jiminy/core/engine/EngineMultiRobot.h @@ -392,10 +392,10 @@ namespace jiminy /// \param[in] forceFct Callback function returning the force that systemName2 /// applies on systemName1, in the global frame of frameName1. hresult_t registerForceCoupling(std::string const & systemName1, - std::string const & systemName2, - std::string const & frameName1, - std::string const & frameName2, - forceCouplingFunctor_t forceFct); + std::string const & systemName2, + std::string const & frameName1, + std::string const & frameName2, + forceCouplingFunctor_t forceFct); hresult_t registerViscoElasticDirectionalForceCoupling(std::string const & systemName1, std::string const & systemName2, std::string const & frameName1, diff --git a/core/include/jiminy/core/engine/System.h b/core/include/jiminy/core/engine/System.h index f053e55e2..b4673ab62 100644 --- a/core/include/jiminy/core/engine/System.h +++ b/core/include/jiminy/core/engine/System.h @@ -151,7 +151,7 @@ namespace jiminy std::vector boundJointsActiveDir; ///< Store the active "direction" of the bound (0 for lower, 1 for higher) forceVector_t contactFramesForces; ///< Contact forces for each contact frames in local frame vector_aligned_t collisionBodiesForces; ///< Contact forces for each geometries of each collision bodies in local frame - matrixN_t jointJacobian; ///< Buffer used for intermediary computation of `data.u` + matrix6N_t jointJacobian; ///< Buffer used for intermediary computation of `data.u` vectorN_t lo; ///< Lower bound of LCP problem vectorN_t hi; ///< Higher bound of LCP problem std::vector > fIndices; ///< Used to indicate linear coupling between bounds of LCP and the solution (i.e. friction cone: - mu * F_z < F_x/F_y < mu * F_z) diff --git a/core/include/jiminy/core/robot/AbstractSensor.tpp b/core/include/jiminy/core/robot/AbstractSensor.tpp index 297c7e10a..b23291cb1 100644 --- a/core/include/jiminy/core/robot/AbstractSensor.tpp +++ b/core/include/jiminy/core/robot/AbstractSensor.tpp @@ -183,7 +183,7 @@ namespace jiminy sharedHolder_->data_.resize(2); for (matrixN_t & data : sharedHolder_->data_) { - data = matrixN_t::Zero(getSize(), sharedHolder_->num_); // Do NOT use setZero since the size may be ill-defined + data.setZero(getSize(), sharedHolder_->num_); } sharedHolder_->dataMeasured_.setZero(); diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h index 000551bff..600d5f4eb 100644 --- a/core/include/jiminy/core/robot/Model.h +++ b/core/include/jiminy/core/robot/Model.h @@ -58,7 +58,7 @@ namespace jiminy std::shared_ptr get(std::string const & key); std::shared_ptr get(std::string const & key, - constraintsHolderType_t const & holderType); + constraintsHolderType_t const & holderType); void insert(constraintsMap_t const & constraintsMap, constraintsHolderType_t const & holderType); diff --git a/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h b/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h index 7b08006cc..d0a431dde 100644 --- a/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h +++ b/core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h @@ -60,7 +60,7 @@ namespace pinocchio_overload pinocchio::container::aligned_vector const & fext) { (void) pinocchio::rnea(model, data, q, v, a, fext); - data.tau += model.rotorInertia.asDiagonal() * a; + data.tau.array() += model.rotorInertia.array() * a.array(); return data.tau; } @@ -485,7 +485,7 @@ namespace pinocchio_overload Eigen::MatrixBase const & J, bool_t const & updateDecomposition = true) { - // Compute Cholesky decomposition of mass matrix M + // Compute the Cholesky decomposition of mass matrix M if requested if (updateDecomposition) { pinocchio::cholesky::decompose(model, data); @@ -494,12 +494,12 @@ namespace pinocchio_overload // Compute sDUiJt := sqrt(D)^-1 * U^-1 * J.T data.sDUiJt = J.transpose(); pinocchio::cholesky::Uiv(model, data, data.sDUiJt); - data.sDUiJt.array().colwise() /= data.D.array().sqrt(); + data.sDUiJt.array().colwise() *= data.Dinv.array().sqrt(); // Compute JMinvJt := sDUiJt.T * sDUiJt - data.JMinvJt = matrixN_t::Zero(J.rows(), J.rows()); + data.JMinvJt.resize(J.rows(), J.rows()); + data.JMinvJt.triangularView().setZero(); data.JMinvJt.selfadjointView().rankUpdate(data.sDUiJt.transpose()); - data.JMinvJt.triangularView() = data.JMinvJt.transpose(); return data.JMinvJt; } diff --git a/core/include/jiminy/core/stepper/AbstractRungeKuttaStepper.h b/core/include/jiminy/core/stepper/AbstractRungeKuttaStepper.h index e4ad044d1..6a7a30ac6 100644 --- a/core/include/jiminy/core/stepper/AbstractRungeKuttaStepper.h +++ b/core/include/jiminy/core/stepper/AbstractRungeKuttaStepper.h @@ -19,7 +19,7 @@ namespace jiminy /// \brief Constructor /// \param[in] f Dynamics function, with signature a = f(t, q, v) /// \param[in] robots Robots whose dynamics the stepper will work on. - AbstractRungeKuttaStepper(systemDynamics f, /* Copy on purpose */ + AbstractRungeKuttaStepper(systemDynamics const & f, std::vector const & robots, matrixN_t const & RungeKuttaMatrix, vectorN_t const & bWeights, diff --git a/core/include/jiminy/core/stepper/AbstractStepper.h b/core/include/jiminy/core/stepper/AbstractStepper.h index 4430d996b..5bb14236c 100644 --- a/core/include/jiminy/core/stepper/AbstractStepper.h +++ b/core/include/jiminy/core/stepper/AbstractStepper.h @@ -33,7 +33,7 @@ namespace jiminy /// \brief Constructor /// \param[in] f Dynamics function, with signature a = f(t, q, v) /// \param[in] robots Robots whose dynamics the stepper will work on. - AbstractStepper(systemDynamics f, /* Copy on purpose */ + AbstractStepper(systemDynamics const & f, std::vector const & robots); virtual ~AbstractStepper(void) = default; diff --git a/core/include/jiminy/core/stepper/EulerExplicitStepper.h b/core/include/jiminy/core/stepper/EulerExplicitStepper.h index d15f9fed6..a487850ee 100644 --- a/core/include/jiminy/core/stepper/EulerExplicitStepper.h +++ b/core/include/jiminy/core/stepper/EulerExplicitStepper.h @@ -19,7 +19,7 @@ namespace jiminy /// \brief Constructor /// \param[in] f Dynamics function, with signature a = f(t, q, v) /// \param[in] robots Robots whose dynamics the stepper will work on. - EulerExplicitStepper(systemDynamics f, /* Copy on purpose */ + EulerExplicitStepper(systemDynamics const & f, std::vector const & robots); protected: diff --git a/core/include/jiminy/core/stepper/RungeKutta4Stepper.h b/core/include/jiminy/core/stepper/RungeKutta4Stepper.h index 890af7170..38c3942dd 100644 --- a/core/include/jiminy/core/stepper/RungeKutta4Stepper.h +++ b/core/include/jiminy/core/stepper/RungeKutta4Stepper.h @@ -27,7 +27,7 @@ namespace jiminy /// \brief Constructor /// \param[in] f Dynamics function, with signature a = f(t, q, v) /// \param[in] robots Robots whose dynamics the stepper will work on. - RungeKutta4Stepper(systemDynamics f, /* Copy on purpose */ + RungeKutta4Stepper(systemDynamics const & f, std::vector const & robots); }; } diff --git a/core/include/jiminy/core/stepper/RungeKuttaDOPRIStepper.h b/core/include/jiminy/core/stepper/RungeKuttaDOPRIStepper.h index ed576d7a7..6127cfad1 100644 --- a/core/include/jiminy/core/stepper/RungeKuttaDOPRIStepper.h +++ b/core/include/jiminy/core/stepper/RungeKuttaDOPRIStepper.h @@ -57,7 +57,7 @@ namespace jiminy /// \param[in] robots Robots whose dynamics the stepper will work on. /// \param[in] tolRel Relative tolerance, used to determine step success and timestep update. /// \param[in] tolAbs Relative tolerance, used to determine step success and timestep update. - RungeKuttaDOPRIStepper(systemDynamics f, /* Copy on purpose */ + RungeKuttaDOPRIStepper(systemDynamics const & f, std::vector const & robots, float64_t const & tolRel, float64_t const & tolAbs); diff --git a/core/include/jiminy/core/utilities/Pinocchio.h b/core/include/jiminy/core/utilities/Pinocchio.h index 974e21c2a..27595ce00 100644 --- a/core/include/jiminy/core/utilities/Pinocchio.h +++ b/core/include/jiminy/core/utilities/Pinocchio.h @@ -118,6 +118,13 @@ namespace jiminy pinocchio::GeometryModel & collisionModel, boost::optional visualModel = boost::none, bool_t const & loadVisualMeshes = false); + + void buildReducedModel(pinocchio::Model const & inputModel, + pinocchio::GeometryModel const & inputGeomModel, + std::vector const & listOfJointsToLock, + vectorN_t const & referenceConfiguration, + pinocchio::Model & reducedModel, + pinocchio::GeometryModel & reducedGeomModel); } #endif // JIMINY_PINOCCHIO_H diff --git a/core/src/constraints/AbstractConstraint.cc b/core/src/constraints/AbstractConstraint.cc index 4a63d90c1..6d77eebba 100644 --- a/core/src/constraints/AbstractConstraint.cc +++ b/core/src/constraints/AbstractConstraint.cc @@ -59,12 +59,12 @@ namespace jiminy void AbstractConstraintBase::enable(void) { - lambda_.setZero(); isEnabled_ = true; } void AbstractConstraintBase::disable(void) { + lambda_.setZero(); isEnabled_ = false; } diff --git a/core/src/constraints/DistanceConstraint.cc b/core/src/constraints/DistanceConstraint.cc index c3c664642..1eae8c405 100644 --- a/core/src/constraints/DistanceConstraint.cc +++ b/core/src/constraints/DistanceConstraint.cc @@ -75,14 +75,14 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { - // Set jacobian, drift and multipliers to right dimension - jacobian_ = matrixN_t::Zero(1, model->pncModel_.nv); - drift_ = vectorN_t::Zero(1); - lambda_ = vectorN_t::Zero(1); - // Initialize frames jacobians buffers - firstFrameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv); - secondFrameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv); + firstFrameJacobian_.setZero(6, model->pncModel_.nv); + secondFrameJacobian_.setZero(6, model->pncModel_.nv); + + // Initialize jacobian, drift and multipliers + jacobian_.setZero(1, model->pncModel_.nv); + drift_.setZero(1); + lambda_.setZero(1); } return returnCode; diff --git a/core/src/constraints/FixedFrameConstraint.cc b/core/src/constraints/FixedFrameConstraint.cc index ef2c5b82b..1761b341f 100644 --- a/core/src/constraints/FixedFrameConstraint.cc +++ b/core/src/constraints/FixedFrameConstraint.cc @@ -28,7 +28,7 @@ namespace jiminy { dofsFixed_.resize(static_cast(maskFixed.cast().array().sum())); uint32_t dofIndex = 0; - for (uint32_t i : std::vector{{2, 1, 0, 3, 4, 5}}) + for (uint32_t const & i : std::array{{2, 1, 0, 3, 4, 5}}) { if (maskFixed[i]) { @@ -102,13 +102,14 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { - // Initialize jacobian, drift and multipliers - frameJacobian_ = matrix6N_t::Zero(6, model->pncModel_.nv); - frameDrift_ = vector6_t::Zero(); + // Initialize frames jacobians buffers + frameJacobian_.setZero(6, model->pncModel_.nv); + + // Initialize constraint jacobian, drift and multipliers Eigen::Index const dim = static_cast(dofsFixed_.size()); - jacobian_ = matrixN_t::Zero(dim, model->pncModel_.nv); - drift_ = vectorN_t::Zero(dim); - lambda_ = vectorN_t::Zero(dim); + jacobian_.setZero(dim, model->pncModel_.nv); + drift_.setZero(dim); + lambda_.setZero(dim); // Get the current frame position and use it as reference transformRef_ = model->pncData_.oMf[frameIdx_]; diff --git a/core/src/constraints/JointConstraint.cc b/core/src/constraints/JointConstraint.cc index 2ce9d395f..2d31488e0 100644 --- a/core/src/constraints/JointConstraint.cc +++ b/core/src/constraints/JointConstraint.cc @@ -74,15 +74,12 @@ namespace jiminy pinocchio::JointModel const & jointModel = model->pncModel_.joints[jointIdx_]; // Initialize the jacobian. It is simply the velocity selector mask. - jacobian_ = matrixN_t::Zero(jointModel.nv(), model->pncModel_.nv); - for (Eigen::Index i=0; i < jointModel.nv(); ++i) - { - jacobian_(i, jointModel.idx_v() + i) = 1.0; - } + jacobian_.setZero(jointModel.nv(), model->pncModel_.nv); + jacobian_.middleCols(jointModel.idx_v(), jointModel.nv()).setIdentity(); // Initialize drift and multipliers - drift_ = vectorN_t::Zero(jointModel.nv()); - lambda_ = vectorN_t::Zero(jointModel.nv()); + drift_.setZero(jointModel.nv()); + lambda_.setZero(jointModel.nv()); // Get the current joint position and use it as reference configurationRef_ = jointModel.jointConfigSelector(q); diff --git a/core/src/constraints/SphereConstraint.cc b/core/src/constraints/SphereConstraint.cc index fff744542..ef0b4dc3b 100644 --- a/core/src/constraints/SphereConstraint.cc +++ b/core/src/constraints/SphereConstraint.cc @@ -72,11 +72,13 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { + // Initialize frames jacobians buffers + frameJacobian_.setZero(6, model->pncModel_.nv); + // Initialize jacobian, drift and multipliers - frameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv); - jacobian_ = matrixN_t::Zero(3, model->pncModel_.nv); - drift_ = vectorN_t::Zero(3); - lambda_ = vectorN_t::Zero(3); + jacobian_.setZero(3, model->pncModel_.nv); + drift_.setZero(3); + lambda_.setZero(3); // Get the current frame position and use it as reference transformRef_ = model->pncData_.oMf[frameIdx_]; diff --git a/core/src/constraints/WheelConstraint.cc b/core/src/constraints/WheelConstraint.cc index 3c06e7369..32225c0b4 100644 --- a/core/src/constraints/WheelConstraint.cc +++ b/core/src/constraints/WheelConstraint.cc @@ -76,11 +76,13 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { + // Initialize frames jacobians buffers + frameJacobian_.setZero(6, model->pncModel_.nv); + // Initialize jacobian, drift and multipliers - frameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv); - jacobian_ = matrixN_t::Zero(3, model->pncModel_.nv); - drift_ = vectorN_t::Zero(3); - lambda_ = vectorN_t::Zero(3); + jacobian_.setZero(3, model->pncModel_.nv); + drift_.setZero(3); + lambda_.setZero(3); // Get the current frame position and use it as reference transformRef_ = model->pncData_.oMf[frameIdx_]; diff --git a/core/src/control/AbstractController.cc b/core/src/control/AbstractController.cc index 5e3d78de6..afa8dd4c0 100644 --- a/core/src/control/AbstractController.cc +++ b/core/src/control/AbstractController.cc @@ -144,7 +144,7 @@ namespace jiminy { objectName = objectPrefixName + TELEMETRY_FIELDNAME_DELIMITER + objectName; } - telemetrySender_.configureObject(std::move(telemetryData), objectName); + telemetrySender_.configureObject(telemetryData, objectName); for (std::pair const & registeredVariable : registeredVariables_) { if (returnCode == hresult_t::SUCCESS) diff --git a/core/src/engine/Engine.cc b/core/src/engine/Engine.cc index df6a01478..e0dcd550e 100644 --- a/core/src/engine/Engine.cc +++ b/core/src/engine/Engine.cc @@ -43,12 +43,12 @@ namespace jiminy if (controller) { returnCode = EngineMultiRobot::addSystem( - "", std::move(robot), std::move(controller), std::move(callbackFct)); + "", robot, controller, std::move(callbackFct)); } else { returnCode = EngineMultiRobot::addSystem( - "", std::move(robot), std::move(callbackFct)); + "", robot, std::move(callbackFct)); } if (returnCode == hresult_t::SUCCESS) diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 6e7872902..37b1bb807 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -103,12 +103,24 @@ namespace jiminy return hresult_t::ERROR_GENERIC; } + if (!robot) + { + PRINT_ERROR("Robot unspecified."); + return hresult_t::ERROR_INIT_FAILED; + } + if (!robot->getIsInitialized()) { PRINT_ERROR("Robot not initialized."); return hresult_t::ERROR_INIT_FAILED; } + if (!controller) + { + PRINT_ERROR("Controller unspecified."); + return hresult_t::ERROR_INIT_FAILED; + } + if (!controller->getIsInitialized()) { PRINT_ERROR("Controller not initialized."); @@ -130,8 +142,8 @@ namespace jiminy // TODO: Check that the callback function is working as expected systems_.emplace_back(systemName, - std::move(robot), - std::move(controller), + robot, + controller, std::move(callbackFct)); systemsDataHolder_.resize(systems_.size()); @@ -142,6 +154,12 @@ namespace jiminy std::shared_ptr robot, callbackFunctor_t callbackFct) { + if (!robot) + { + PRINT_ERROR("Robot unspecified."); + return hresult_t::ERROR_INIT_FAILED; + } + if (!robot->getIsInitialized()) { PRINT_ERROR("Robot not initialized."); @@ -318,13 +336,13 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { forcesCoupling_.emplace_back(systemName1, - std::move(systemIdx1), + systemIdx1, systemName2, - std::move(systemIdx2), + systemIdx2, frameName1, - std::move(frameIdx1), + frameIdx1, frameName2, - std::move(frameIdx2), + frameIdx2, std::move(forceFct)); } @@ -1318,7 +1336,7 @@ namespace jiminy } // Initialize some addition buffers used by impulse contact solver - systemDataIt->jointJacobian = matrixN_t::Zero(6, systemIt->robot->pncModel_.nv); + systemDataIt->jointJacobian.setZero(6, systemIt->robot->pncModel_.nv); // Reset the constraints returnCode = systemIt->robot->resetConstraints(q, v); @@ -3698,7 +3716,7 @@ namespace jiminy if (system.robot->hasConstraints()) { // Define some proxies for convenience - matrixN_t & jointJacobian = systemData.jointJacobian; + matrix6N_t & jointJacobian = systemData.jointJacobian; vectorN_t & lo = systemData.lo; vectorN_t & hi = systemData.hi; std::vector > & fIndices = systemData.fIndices; @@ -3712,8 +3730,8 @@ namespace jiminy data.lambda_c = system.robot->getConstraintsLambda(); // Compute constraints bounds - lo = vectorN_t::Constant(constraintsDrift.size(), -INF); - hi = vectorN_t::Constant(constraintsDrift.size(), +INF); + lo.setConstant(constraintsDrift.size(), -INF); + hi.setConstant(constraintsDrift.size(), +INF); fIndices = std::vector >(constraintsDrift.size()); uint64_t constraintIdx = 0U; diff --git a/core/src/engine/System.cc b/core/src/engine/System.cc index 92d57655d..8cbfde4f5 100644 --- a/core/src/engine/System.cc +++ b/core/src/engine/System.cc @@ -80,8 +80,8 @@ namespace jiminy std::shared_ptr controllerIn, callbackFunctor_t callbackFctIn) : name(systemNameIn), - robot(std::move(robotIn)), - controller(std::move(controllerIn)), + robot(robotIn), + controller(controllerIn), callbackFct(std::move(callbackFctIn)) { // Empty on purpose @@ -127,13 +127,13 @@ namespace jiminy } q = pinocchio::neutral(robot.pncModel_); - v = vectorN_t::Zero(robot.nv()); - a = vectorN_t::Zero(robot.nv()); - command = vectorN_t::Zero(robot.getMotorsNames().size()); - u = vectorN_t::Zero(robot.nv()); - uMotor = vectorN_t::Zero(robot.getMotorsNames().size()); - uInternal = vectorN_t::Zero(robot.nv()); - uCustom = vectorN_t::Zero(robot.nv()); + v.setZero(robot.nv()); + a.setZero(robot.nv()); + command.setZero(robot.getMotorsNames().size()); + u.setZero(robot.nv()); + uMotor.setZero(robot.getMotorsNames().size()); + uInternal.setZero(robot.nv()); + uCustom.setZero(robot.nv()); fExternal = forceVector_t(robot.pncModel_.joints.size(), pinocchio::Force::Zero()); isInitialized_ = true; @@ -148,14 +148,14 @@ namespace jiminy void systemState_t::clear(void) { - q = vectorN_t(); - v = vectorN_t(); - a = vectorN_t(); - command = vectorN_t(); - u = vectorN_t(); - uMotor = vectorN_t(); - uInternal = vectorN_t(); - uCustom = vectorN_t(); + q.resize(0); + v.resize(0); + a.resize(0); + command.resize(0); + u.resize(0); + uMotor.resize(0); + uInternal.resize(0); + uCustom.resize(0); fExternal.clear(); } diff --git a/core/src/io/JsonLoader.cc b/core/src/io/JsonLoader.cc index d865f8eb7..44ed464af 100644 --- a/core/src/io/JsonLoader.cc +++ b/core/src/io/JsonLoader.cc @@ -9,7 +9,7 @@ namespace jiminy JsonLoader::JsonLoader(std::shared_ptr device) : rootJson_(std::make_unique()), payload_(), - device_(std::move(device)) + device_(device) { // Empty on purpose. } diff --git a/core/src/robot/AbstractSensor.cc b/core/src/robot/AbstractSensor.cc index 829bb0a1b..53c891a9b 100644 --- a/core/src/robot/AbstractSensor.cc +++ b/core/src/robot/AbstractSensor.cc @@ -42,7 +42,7 @@ namespace jiminy { objectName = objectPrefixName + TELEMETRY_FIELDNAME_DELIMITER + objectName; } - telemetrySender_.configureObject(std::move(telemetryData), objectName); + telemetrySender_.configureObject(telemetryData, objectName); returnCode = telemetrySender_.registerVariable(getFieldnames(), get()); if (returnCode == hresult_t::SUCCESS) { diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index 6e8736167..332eb6e1c 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -150,10 +150,10 @@ namespace jiminy constraint = get(key, holderType); if (constraint) { - return constraint; + break; } } - return {}; + return constraint; } void constraintsHolder_t::insert(constraintsMap_t const & constraintsMap, @@ -387,6 +387,10 @@ namespace jiminy if (isInitialized_) { + /* Re-generate the true flexible model in case the original rigid + model has been manually modified by the user. */ + generateModelFlexible(); + // Update the biases added to the dynamics properties of the model generateModelBiased(); } @@ -623,15 +627,11 @@ namespace jiminy collisionConstraintsMap.emplace_back(geom.name, std::make_shared( geom.name, (Eigen::Matrix() << true, true, true, false, false, true).finished())); } - else - { - collisionConstraintsMap.emplace_back(geom.name, nullptr); - } } } } - // Add constraints map, even if nullptr for geometry shape not supported + // Add constraints map if (returnCode == hresult_t::SUCCESS) { returnCode = addConstraints(collisionConstraintsMap, constraintsHolderType_t::COLLISION_BODIES); @@ -706,7 +706,7 @@ namespace jiminy collisionModelOrig_.removeCollisionPair(collisionPair); // Append collision geometry to the list of constraints to remove - if (constraintsHolder_.exist(geom.name, constraintsHolderType_t::COLLISION_BODIES)) + if (constraintsHolder_.exist(geom.name, constraintsHolderType_t::COLLISION_BODIES)) { collisionConstraintsNames.emplace_back(geom.name); } @@ -826,14 +826,20 @@ namespace jiminy { hresult_t returnCode = hresult_t::SUCCESS; - // Look for constraint in every constraint holders sequentially + // Check if constraint is properly defined and not already exists for (auto const & constraintPair : constraintsMap) { - std::string const & constraintName = constraintPair.first; + auto const & constraintName = std::get<0>(constraintPair); + auto const & constraintPtr = std::get<1>(constraintPair); + if (!constraintPtr) + { + PRINT_ERROR("Constraint with name '", constraintName, "' is unspecified."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } if (constraintsHolder_.exist(constraintName)) { PRINT_ERROR("A constraint with name '", constraintName, "' already exists."); - return hresult_t::ERROR_BAD_INPUT; + returnCode = hresult_t::ERROR_BAD_INPUT; } } @@ -991,10 +997,7 @@ namespace jiminy auto lambda = [](std::shared_ptr const & constraint, constraintsHolderType_t const & /* holderType */) { - if (constraint) - { - constraint->disable(); - } + constraint->disable(); }; constraintsHolder_.foreach(constraintsHolderType_t::BOUNDS_JOINTS, lambda); constraintsHolder_.foreach(constraintsHolderType_t::CONTACT_FRAMES, lambda); @@ -1370,8 +1373,8 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { // Get the joint position limits from the URDF or the user options - positionLimitMin_ = vectorN_t::Constant(pncModel_.nq, -INF); // Do NOT use robot_->pncModel_.(lower|upper)PositionLimit - positionLimitMax_ = vectorN_t::Constant(pncModel_.nq, +INF); + positionLimitMin_.setConstant(pncModel_.nq, -INF); // Do NOT use robot_->pncModel_.(lower|upper)PositionLimit + positionLimitMax_.setConstant(pncModel_.nq, +INF); if (mdlOptions_->joints.enablePositionLimit) { @@ -1422,7 +1425,7 @@ namespace jiminy } // Get the joint velocity limits from the URDF or the user options - velocityLimit_ = vectorN_t::Constant(pncModel_.nv, +INF); + velocityLimit_.setConstant(pncModel_.nv, +INF); if (mdlOptions_->joints.enableVelocityLimit) { if (mdlOptions_->joints.velocityLimitFromUrdf) @@ -1602,13 +1605,13 @@ namespace jiminy } }); - // Reset jacobian and drift to 0 + // Reset jacobian and drift if (returnCode == hresult_t::SUCCESS) { constraintsMask_ = 0; - constraintsJacobian_ = matrixN_t::Zero(constraintSize, pncModel_.nv); - constraintsDrift_ = vectorN_t::Zero(constraintSize); - constraintsLambda_ = vectorN_t::Zero(constraintSize); + constraintsJacobian_.setZero(constraintSize, pncModel_.nv); + constraintsDrift_.setZero(constraintSize); + constraintsLambda_.setZero(constraintSize); } return returnCode; @@ -1617,8 +1620,7 @@ namespace jiminy hresult_t Model::setOptions(configHolder_t modelOptions) { bool_t internalBuffersMustBeUpdated = false; - bool_t isFlexibleModelInvalid = false; - bool_t isCurrentModelInvalid = false; + bool_t areModelsInvalid = false; bool_t isCollisionDataInvalid = false; if (isInitialized_) { @@ -1688,13 +1690,10 @@ namespace jiminy && (flexibilityConfig.size() != mdlOptions_->dynamics.flexibilityConfig.size() || !std::equal(flexibilityConfig.begin(), flexibilityConfig.end(), - mdlOptions_->dynamics.flexibilityConfig.begin()))) - { - isFlexibleModelInvalid = true; - } - else if (mdlOptions_ && enableFlexibleModel != mdlOptions_->dynamics.enableFlexibleModel) + mdlOptions_->dynamics.flexibilityConfig.begin()) + || enableFlexibleModel != mdlOptions_->dynamics.enableFlexibleModel)) { - isCurrentModelInvalid = true; + areModelsInvalid = true; } } @@ -1733,15 +1732,9 @@ namespace jiminy // Create a fast struct accessor mdlOptions_ = std::make_unique(mdlOptionsHolder_); - if (isFlexibleModelInvalid) - { - // Force flexible model regeneration - generateModelFlexible(); - } - - if (isFlexibleModelInvalid || isCurrentModelInvalid) + if (areModelsInvalid) { - // Trigger biased model regeneration + // Trigger models regeneration reset(); } else if (internalBuffersMustBeUpdated) @@ -1881,7 +1874,7 @@ namespace jiminy } // Initialize the flexible state - vFlex = vectorN_t::Zero(nvFlex); + vFlex.setZero(nvFlex); // Compute the flexible state based on the rigid state int32_t idxRigid = 0; @@ -1921,7 +1914,7 @@ namespace jiminy } // Initialize the rigid state - vRigid = vectorN_t::Zero(nvRigid); + vRigid.setZero(nvRigid); // Compute the rigid state based on the flexible state int32_t idxRigid = 0; @@ -2081,7 +2074,7 @@ namespace jiminy [&hasConstraintsEnabled](std::shared_ptr const & constraint, constraintsHolderType_t const & /* holderType */) { - if (constraint && constraint->getIsEnabled()) + if (constraint->getIsEnabled()) { hasConstraintsEnabled = true; } diff --git a/core/src/robot/Robot.cc b/core/src/robot/Robot.cc index 8412a1446..dfb75f6fb 100644 --- a/core/src/robot/Robot.cc +++ b/core/src/robot/Robot.cc @@ -105,7 +105,7 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { - telemetryData_ = std::move(telemetryData); + telemetryData_ = telemetryData; } if (returnCode == hresult_t::SUCCESS) diff --git a/core/src/solver/LCPSolvers.cc b/core/src/solver/LCPSolvers.cc index c8d708d6f..f6985d45a 100644 --- a/core/src/solver/LCPSolvers.cc +++ b/core/src/solver/LCPSolvers.cc @@ -119,7 +119,8 @@ namespace jiminy assert(b.size() > 0 && "The number of inequality constraints must be larger than 0."); // Initialize the residuals - y_.noalias() = A * x - b; + y_ = - b; + y_.noalias() += A * x; // Perform multiple PGS loop until convergence or max iter reached for (uint32_t iter = 0; iter < maxIter_; ++iter) @@ -129,7 +130,8 @@ namespace jiminy // Check if terminate conditions are satisfied yPrev_ = y_; - y_.noalias() = A * x - b; + y_ = - b; + y_.noalias() += A * x; dy_ = y_ - yPrev_; if ((dy_.array().abs() < tolAbs_ || (dy_.array() / y_.array()).abs() < tolRel_).all()) { @@ -162,8 +164,8 @@ namespace jiminy pinocchio::cholesky::solve(model, data, data.torque_residual); // Compute b - b_.noalias() = - J * data.torque_residual; - b_ -= gamma; + b_ = - gamma; + b_.noalias() -= J * data.torque_residual; /* Add regularization term in case A is not inversible. Note that Mujoco defines an impedance function that depends on @@ -194,6 +196,9 @@ namespace jiminy } else { + // Full matrix is needed + A.triangularView() = A.transpose(); + // Run standard PGS algorithm isSuccess = ProjectedGaussSeidelSolver(A, b_, lo, hi, fIndices, f); } diff --git a/core/src/stepper/AbstractRungeKuttaStepper.cc b/core/src/stepper/AbstractRungeKuttaStepper.cc index b582da917..452092bda 100644 --- a/core/src/stepper/AbstractRungeKuttaStepper.cc +++ b/core/src/stepper/AbstractRungeKuttaStepper.cc @@ -4,7 +4,7 @@ namespace jiminy { - AbstractRungeKuttaStepper::AbstractRungeKuttaStepper(systemDynamics f, /* Copy on purpose */ + AbstractRungeKuttaStepper::AbstractRungeKuttaStepper(systemDynamics const & f, std::vector const & robots, matrixN_t const & RungeKuttaMatrix, vectorN_t const & bWeights, diff --git a/core/src/stepper/AbstractStepper.cc b/core/src/stepper/AbstractStepper.cc index b59e0ba20..5c99b0bc4 100644 --- a/core/src/stepper/AbstractStepper.cc +++ b/core/src/stepper/AbstractStepper.cc @@ -3,9 +3,9 @@ namespace jiminy { - AbstractStepper::AbstractStepper(systemDynamics f, + AbstractStepper::AbstractStepper(systemDynamics const & f, std::vector const & robots): - f_(std::move(f)), + f_(f), robots_(robots), state_(robots), stateDerivative_(robots), diff --git a/core/src/stepper/EulerExplicitStepper.cc b/core/src/stepper/EulerExplicitStepper.cc index c07d24f2f..316c764e8 100644 --- a/core/src/stepper/EulerExplicitStepper.cc +++ b/core/src/stepper/EulerExplicitStepper.cc @@ -3,7 +3,7 @@ namespace jiminy { - EulerExplicitStepper::EulerExplicitStepper(systemDynamics f, /* Copy on purpose */ + EulerExplicitStepper::EulerExplicitStepper(systemDynamics const & f, std::vector const & robots): AbstractStepper(f, robots) { diff --git a/core/src/stepper/RungeKutta4Stepper.cc b/core/src/stepper/RungeKutta4Stepper.cc index fabdec3ba..759b105a2 100644 --- a/core/src/stepper/RungeKutta4Stepper.cc +++ b/core/src/stepper/RungeKutta4Stepper.cc @@ -2,7 +2,7 @@ namespace jiminy { - RungeKutta4Stepper::RungeKutta4Stepper(systemDynamics f, /* Copy on purpose */ + RungeKutta4Stepper::RungeKutta4Stepper(systemDynamics const & f, std::vector const & robots): AbstractRungeKuttaStepper(f, robots, RK4::A, RK4::b, RK4::c, false) { diff --git a/core/src/stepper/RungeKuttaDOPRIStepper.cc b/core/src/stepper/RungeKuttaDOPRIStepper.cc index 7535dfa39..8b391739f 100644 --- a/core/src/stepper/RungeKuttaDOPRIStepper.cc +++ b/core/src/stepper/RungeKuttaDOPRIStepper.cc @@ -2,7 +2,7 @@ namespace jiminy { - RungeKuttaDOPRIStepper::RungeKuttaDOPRIStepper(systemDynamics f, /* Copy on purpose */ + RungeKuttaDOPRIStepper::RungeKuttaDOPRIStepper(systemDynamics const & f, std::vector const & robots, float64_t const & tolRel, float64_t const & tolAbs): diff --git a/core/src/telemetry/TelemetrySender.cc b/core/src/telemetry/TelemetrySender.cc index 0797145c9..a872c374e 100644 --- a/core/src/telemetry/TelemetrySender.cc +++ b/core/src/telemetry/TelemetrySender.cc @@ -98,7 +98,7 @@ namespace jiminy std::string const & objectNameIn) { objectName_ = objectNameIn; - telemetryData_ = std::move(telemetryDataInstance); + telemetryData_ = telemetryDataInstance; intBufferPosition_.clear(); floatBufferPosition_.clear(); } diff --git a/core/src/utilities/Pinocchio.cc b/core/src/utilities/Pinocchio.cc index a487d3dc7..eba02c747 100644 --- a/core/src/utilities/Pinocchio.cc +++ b/core/src/utilities/Pinocchio.cc @@ -11,6 +11,7 @@ #include "pinocchio/multibody/visitor.hpp" // `pinocchio::fusion::JointUnaryVisitorBase` #include "pinocchio/multibody/joint/joint-model-base.hpp" // `pinocchio::JointModelBase` #include "pinocchio/algorithm/joint-configuration.hpp" // `pinocchio::isNormalized` +#include "pinocchio/algorithm/model.hpp" // `pinocchio::buildReducedModel` #include "hpp/fcl/mesh_loader/loader.h" #include "hpp/fcl/BVH/BVH_model.h" @@ -275,7 +276,7 @@ namespace jiminy { frameIndex_t frameIdx; returnCode = getFrameIdx(model, name, frameIdx); - framesIdx.push_back(std::move(frameIdx)); + framesIdx.push_back(frameIdx); } } @@ -310,7 +311,7 @@ namespace jiminy { frameIndex_t frameIdx; returnCode = getFrameIdx(model, name, frameIdx); - bodiesIdx.push_back(std::move(frameIdx)); + bodiesIdx.push_back(frameIdx); } } @@ -1053,4 +1054,26 @@ namespace jiminy return returnCode; } + + void buildReducedModel(pinocchio::Model const & inputModel, + pinocchio::GeometryModel const & inputGeomModel, + std::vector const & listOfJointsToLock, + vectorN_t const & referenceConfiguration, + pinocchio::Model & reducedModel, + pinocchio::GeometryModel & reducedGeomModel) + { + // Fix `parentFrame` not updated for reduced geometry model in Pinocchio < 2.6.0 + pinocchio::buildReducedModel(inputModel, + inputGeomModel, + listOfJointsToLock, + referenceConfiguration, + reducedModel, + reducedGeomModel); + for (auto const & geom : inputGeomModel.geometryObjects) + { + geomIndex_t reducedGeomIdx = reducedGeomModel.getGeometryId(geom.name); + auto & reducedGeom = reducedGeomModel.geometryObjects[reducedGeomIdx]; + reducedGeom.parentFrame = reducedModel.getBodyId(inputModel.frames[geom.parentFrame].name); + } + } } diff --git a/core/src/utilities/Random.cc b/core/src/utilities/Random.cc index d445e9e0e..9889ddbc7 100644 --- a/core/src/utilities/Random.cc +++ b/core/src/utilities/Random.cc @@ -278,7 +278,7 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// /// - /// \brief Lower Cholesky factor of a Toeplitz positive semi_definite matrix. + /// \brief Lower Cholesky factor of a Toeplitz positive semi-definite matrix. /// /// \details In practice, it is advisable to combine this algorithm with Tikhonov /// regularization of relative magnitude 1e-9 to avoid numerical instabilities @@ -297,7 +297,7 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// template void toeplitzCholeskyLower(Eigen::MatrixBase const & a, - Eigen::MatrixBase & l) + Eigen::MatrixBase & l) { /* Initialize lower Cholesky factor. Resizing is enough, no need to initialize it. */ @@ -305,8 +305,8 @@ namespace jiminy l.resize(n, n); /* Compute compressed representation of the matrix, which - coincide with the Schur genetor for Toepliz matrices. */ - matrixN_t g = matrixN_t(2, n); + coincide with the Schur genetor for Toepliz matrices. */ + matrixN_t g(2, n); g.row(0) = a.row(0); g.row(1).tail(n - 1) = a.col(0).tail(n - 1); g(1, 0) = 0.0; @@ -350,7 +350,7 @@ namespace jiminy } // Sample normal vector - vectorN_t const normalVec = vectorN_t(numTimes_).unaryExpr( + vectorN_t const normalVec = vectorN_t::NullaryExpr(numTimes_, [](float64_t const &) { return randNormal(); }); // Compute discrete periodic gaussian process values @@ -414,19 +414,13 @@ namespace jiminy return std::exp(-2.0 * std::pow(std::sin(M_PI / period * dist), 2) / wavelength2); }); - /* Perform Square-Root-Free Cholesky decomposition (LDLT) . - In practice, since the covariance matrix is symmetric, Eigen Value, Singular Value, - Square-Root-Free Cholesky or Schur decompositions are equivalent, but Cholesky is - by far the most efficient one: - - https://math.stackexchange.com/q/22825/375496 - Moreover, note that the covariance is positive semi-definite toepliz - matrix, so computational complexity of the decomposition could be reduced - even further using adapted Cholesky algorithm since speed is very critical. */ - //auto covLDLT = cov.ldlt(); - //covSqrtRoot_ = covLDLT.matrixL(); - //covSqrtRoot_ *= covLDLT.vectorD().array().cwiseMax(0.0).sqrt().matrix().asDiagonal(); - toeplitzCholeskyLower(cov + 1.0e-9 * matrixN_t::Identity(numTimes_, numTimes_), - covSqrtRoot_); + /* Perform Square-Root-Free Cholesky decomposition (LDLT). + All decompositions are equivalent as the covariance matrix is symmetric, namely Eigen + Value, Singular Value, Square-Root-Free Cholesky and Schur decompositions. Cholesky + is by far the most efficient one (https://math.stackexchange.com/q/22825/375496). + Moreover, the covariance is positive semi-definite toepliz matrix, so computational + complexity can be reduced even further using optimized Cholesky algorithm. */ + toeplitzCholeskyLower(cov + 1.0e-9 * matrixN_t::Identity(numTimes_, numTimes_), covSqrtRoot_); // At this point, it is fully initialized isInitialized_ = true; @@ -456,9 +450,9 @@ namespace jiminy } // Sample normal vectors - vectorN_t normalVec1 = vectorN_t(numHarmonics_).unaryExpr( + vectorN_t normalVec1 = vectorN_t::NullaryExpr(numHarmonics_, [](float64_t const &) { return randNormal(); }); - vectorN_t normalVec2 = vectorN_t(numHarmonics_).unaryExpr( + vectorN_t normalVec2 = vectorN_t::NullaryExpr(numHarmonics_, [](float64_t const &) { return randNormal(); }); // Compute discrete periodic gaussian process values @@ -876,7 +870,7 @@ namespace jiminy [&size, &seed] (vectorN_t::Index const & i) -> float64_t { Eigen::Matrix key; - key << i; + key[0] = i; return randomDouble(key, 1, size[i], seed); }); diff --git a/examples/python/tutorial.ipynb b/examples/python/tutorial.ipynb index ae4062cb8..6ba72b63c 100644 --- a/examples/python/tutorial.ipynb +++ b/examples/python/tutorial.ipynb @@ -141,10 +141,10 @@ }, { "cell_type": "markdown", + "metadata": {}, "source": [ - "The results of a simulation can also be visualized in a 3D viewer: either `gepetto-gui` or `meshcat`. We use the latter here as it can be integrated in jupyter." - ], - "metadata": {} + "The results of a simulation can also be visualized in a 3D viewer: either `panda3d` or `meshcat`. We use the latter here as it can be integrated in jupyter." + ] }, { "cell_type": "code", diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py b/python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py index ca030feb8..eb67a755e 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py @@ -52,7 +52,7 @@ def __init__(self, self.action_space = None # Call super to allow mixing interfaces through multiple inheritance - super().__init__(**kwargs) # type: ignore[call-arg] + super().__init__(**kwargs) # Refresh the observation and action spaces self._initialize_observation_space() @@ -272,6 +272,9 @@ def get_fieldnames(self) -> FieldNested: This method is not supposed to be called before `reset`, so that the controller should be already initialized at this point. """ + # Assertion(s) for type checker + assert self.action_space is not None + return get_fieldnames(self.action_space) diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py b/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py index 9e67ee20e..7dc040b9e 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py @@ -35,7 +35,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._observation = None # Call super to allow mixing interfaces through multiple inheritance - super().__init__(*args, **kwargs) # type: ignore[call-arg] + super().__init__(*args, **kwargs) def get_observation(self) -> DataNested: """Get post-processed observation. @@ -50,6 +50,8 @@ def get_observation(self) -> DataNested: In most cases, it is not necessary to overloaded this method, and doing so may lead to unexpected behavior if not done carefully. """ + # Assertion(s) for type checker + assert self._observation is not None return self._observation # methods to override: @@ -104,7 +106,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: __func__ is not ControllerInterface.compute_reward_terminal) # Call super to allow mixing interfaces through multiple inheritance - super().__init__(*args, **kwargs) # type: ignore[call-arg] + super().__init__(*args, **kwargs) # methods to override: # ---------------------------- diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py index db869b5b4..654592186 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py @@ -87,6 +87,9 @@ def _controller_handle(self, .. warning:: This method is not supposed to be called manually nor overloaded. """ + # Assertion(s) for type checker + assert self._action is not None + command[:] = self.compute_command( self.env.get_observation(), self._action) @@ -113,6 +116,9 @@ def get_observation(self) -> DataNested: .. warning:: This method is not supposed to be called manually nor overloaded. """ + # Assertion(s) for type checker + assert self._observation is not None + return copy(self._observation) def reset(self, @@ -162,7 +168,7 @@ def register() -> Tuple[ObserverHandleType, ControllerHandleType]: return observer_handle, controller_handle # Reset base pipeline - self.env.reset(register, **kwargs) # type: ignore[call-arg] + self.env.reset(register, **kwargs) return self.get_observation() @@ -176,6 +182,9 @@ def step(self, :returns: Next observation, reward, status of the episode (done or not), and a dictionary of extra information. """ + # Assertion(s) for type checker + assert self._action is not None + # Backup the action to perform, if any if action is not None: set_value(self._action, action) @@ -206,6 +215,9 @@ def _setup(self) -> None: # Call base implementation super()._setup() + # Assertion(s) for type checker + assert self._action is not None + # Reset some internal buffers fill(self._action, 0.0) fill(self._command, 0.0) @@ -234,6 +246,10 @@ def compute_command(self, :param measure: Observation of the environment. :param action: Target to achieve. """ + # Assertion(s) for type checker + assert self._action is not None + assert self.env._action is not None + set_value(self._action, action) set_value(self.env._action, action) return self.env.compute_command(measure, action) @@ -380,7 +396,7 @@ def refresh_observation(self) -> None: # type: ignore[override] if not self.simulator.is_simulation_running: features = self.observer.get_observation() if self.augment_observation: - # Assertion for type checker + # Assertion(s) for type checker assert isinstance(self._observation, dict) # Make sure to store references if isinstance(obs, gym.spaces.Dict): @@ -563,6 +579,10 @@ def compute_command(self, :param measure: Observation of the environment. :param action: High-level target to achieve. """ + # Assertion(s) for type checker + assert self._observation is not None + assert self.env._action is not None + # Update the target to send to the subsequent block if necessary. # Note that `_observation` buffer has already been updated right before # calling this method by `_controller_handle`, so it can be used as @@ -609,7 +629,7 @@ def refresh_observation(self) -> None: # type: ignore[override] if not self.simulator.is_simulation_running: obs = self.env.get_observation() if self.augment_observation: - # Assertion for type checker + # Assertion(s) for type checker assert isinstance(self._observation, dict) # Make sure to store references if isinstance(obs, dict): diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py index 3475fa438..0ae61cd22 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py @@ -239,6 +239,7 @@ def _controller_handle(self, .. warning:: This method is not supposed to be called manually nor overloaded. """ + assert self._action is not None command[:] = self.compute_command( self.get_observation(), self._action) @@ -546,6 +547,7 @@ def reset(self, # Assertion(s) for type checker assert self.observation_space is not None + assert self._action is not None # Stop the simulator self.simulator.stop() @@ -736,6 +738,9 @@ def step(self, :returns: Next observation, reward, status of the episode (done or not), and a dictionary of extra information """ + # Assertion(s) for type checker + assert self._action is not None + # Make sure a simulation is already running if not self.simulator.is_simulation_running: raise RuntimeError( @@ -1357,7 +1362,7 @@ def refresh_observation(self) -> None: # type: ignore[override] def compute_command(self, measure: DataNested, - action: np.ndarray + action: DataNested ) -> np.ndarray: """Compute the motors efforts to apply on the robot. @@ -1378,6 +1383,11 @@ def compute_command(self, if self.debug and not self.action_space.contains(action): logger.warn("The action is out-of-bounds.") + if not isinstance(action, np.ndarray): + raise RuntimeError( + "`BaseJiminyEnv.compute_command` must be overloaded unless " + "the action space has type `gym.spaces.Box`.") + return action def is_done(self, *args: Any, **kwargs: Any) -> bool: diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py index a61822642..44831ac59 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py @@ -11,7 +11,8 @@ ContactSensor as contact, ForceSensor as force, ImuSensor as imu, - PeriodicGaussianProcess) + PeriodicGaussianProcess, + Robot) from jiminy_py.simulator import Simulator import pinocchio as pin @@ -74,7 +75,7 @@ class WalkerJiminyEnv(BaseJiminyEnv): } def __init__(self, - urdf_path: str, + urdf_path: Optional[str], hardware_path: Optional[str] = None, mesh_path: Optional[str] = None, simu_duration_max: float = DEFAULT_SIMULATION_DURATION, @@ -84,7 +85,8 @@ def __init__(self, std_ratio: Optional[dict] = None, config_path: Optional[str] = None, avoid_instable_collisions: bool = True, - debug: bool = False, + debug: bool = False, *, + robot: Optional[Robot] = None, **kwargs: Any) -> None: r""" :param urdf_path: Path of the urdf model to be used for the simulation. @@ -118,6 +120,10 @@ def __init__(self, its vertices. :param debug: Whether or not the debug mode must be activated. Doing it enables telemetry recording. + :param robot: Robot being simulated, already instantiated and + initialized. Build default robot using 'urdf_path', + 'hardware_path' and 'mesh_path' if omitted. + Optional: None by default. :param kwargs: Keyword arguments to forward to `BaseJiminyEnv` class. """ # Handling of default arguments @@ -155,15 +161,23 @@ def __init__(self, self._height_neutral = 0.0 # Configure the backend simulator - simulator = Simulator.build(**{**dict( - urdf_path=self.urdf_path, - hardware_path=self.hardware_path, - mesh_path=self.mesh_path, - has_freeflyer=True, - use_theoretical_model=False, - config_path=self.config_path, - avoid_instable_collisions=self.avoid_instable_collisions, - debug=debug), **kwargs}) + if robot is None: + assert isinstance(self.urdf_path, str) + simulator = Simulator.build( + urdf_path=self.urdf_path, + hardware_path=self.hardware_path, + mesh_path=self.mesh_path, + config_path=self.config_path, + avoid_instable_collisions=self.avoid_instable_collisions, + debug=debug, + **{**dict( + has_freeflyer=True, + use_theoretical_model=False), + **kwargs}) + else: + # Instantiate a simulator and load the options + simulator = Simulator(robot) + simulator.import_options(config_path) # Initialize base class super().__init__( diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/internal/play.py b/python/gym_jiminy/common/gym_jiminy/common/envs/internal/play.py index 8d3478cce..4ec4057ef 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/internal/play.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/internal/play.py @@ -3,8 +3,9 @@ # pylint: disable=import-outside-toplevel,import-error import os -import queue +import sys import time +import queue import threading from typing import Optional, Callable, Any @@ -28,8 +29,7 @@ def __init__(self, """ self.stop_event = stop_event self.max_rate = max_rate - if os.name != 'nt': - import sys + if sys.platform.startswith('linux'): import fcntl import termios self.fd = sys.stdin.fileno() @@ -46,7 +46,7 @@ def __del__(self) -> None: stream are restored at python exit, otherwise the user may not be able to enter any command without closing the terminal. """ - if os.name != 'nt': + if sys.platform.startswith('linux'): import fcntl import termios termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.oldterm) @@ -57,15 +57,15 @@ def __call__(self) -> str: return it. Any previous characters not fetched before calling this method will be discarded. """ - if os.name != 'nt': # pylint: disable=no-else-return + if sys.platform.startswith('linux'): # pylint: disable=no-else-return char = '' try: - import sys import termios termios.tcflush(self.fd, termios.TCIFLUSH) while self.stop_event is None or \ not self.stop_event.is_set(): if self.max_rate is not None: + # Busy loop is not used to avoid unnecessary cpu load time.sleep(self.max_rate) try: char += sys.stdin.read(1) @@ -82,8 +82,8 @@ def __call__(self) -> str: not self.stop_event.is_set(): if self.max_rate is not None: time.sleep(self.max_rate) - if msvcrt.kbhit(): # type: ignore[attr-defined] - return msvcrt.getch() # type: ignore[attr-defined] + if msvcrt.kbhit(): + return msvcrt.getch() return '' @@ -156,7 +156,7 @@ def wrapped_func(*args: Any, **kwargs: Any) -> None: if pause_key: print(f"Press '{pause_key}' to start...") - # Loop infinitly until termination is triggered + # Loop infinitely until termination is triggered key = None stop = False is_paused = start_paused @@ -176,7 +176,7 @@ def wrapped_func(*args: Any, **kwargs: Any) -> None: stop = True # Sleep for a while if necessary, using busy loop only if - # already started to avoid unecessary cpu load. + # already started to avoid unnecessary cpu load. if max_rate is not None and max_rate > 0.0: dt = max(max_rate - (time.time() - t_init), 0.0) if is_paused: diff --git a/python/gym_jiminy/common/gym_jiminy/common/utils/spaces.py b/python/gym_jiminy/common/gym_jiminy/common/utils/spaces.py index d56e517d6..59266ed34 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/utils/spaces.py +++ b/python/gym_jiminy/common/gym_jiminy/common/utils/spaces.py @@ -1,10 +1,12 @@ """ TODO: Write documentation. """ +from collections import OrderedDict from typing import Optional, Union, Dict, Sequence, TypeVar -import numpy as np -import tree import gym +import tree +import numpy as np +from numpy.random.mtrand import _rand as global_randstate ValueType = TypeVar('ValueType') @@ -19,15 +21,13 @@ def _space_nested_raw(space_nested: gym.Space) -> StructNested[gym.Space]: """Replace any `gym.spaces.Dict` by the raw `OrderedDict` dict it contains. .. note:: - It is necessary because it does not inherit from - `collection.abc.Mapping`, which is necessary for `dm-tree` to operate - properly on it. - # TODO: remove this patch after release of gym==0.22.0 (hopefully) + It is necessary because non primitive objects must inherit from + `collection.abc.Mapping|Sequence` for `dm-tree` to operate on them. + # TODO: support of gym.spaces.Tuple is expected for gym>=0.23.0. """ return tree.traverse( - lambda space: - _space_nested_raw(space.spaces) - if isinstance(space, gym.spaces.Dict) else None, + lambda space: _space_nested_raw(space.spaces) if isinstance( + space, (gym.spaces.Dict, gym.spaces.Tuple)) else None, space_nested) @@ -93,7 +93,7 @@ def sample(low: Union[float, np.ndarray] = -1.0, # Sample from normalized distribution. # Note that some distributions are not normalized by default if rg is None: - rg = np.random + rg = global_randstate distrib_fn = getattr(rg, dist) if dist == 'uniform': value = distrib_fn(low=-1.0, high=1.0, size=shape) @@ -111,9 +111,9 @@ def sample(low: Union[float, np.ndarray] = -1.0, def is_bounded(space_nested: gym.Space) -> bool: - """Check wether `gym.spaces.Space` has finite bounds. + """Check wether a `gym.Space` has finite bounds. - :param space: Gym.Space on which to operate. + :param space: `gym.Space` on which to operate. """ for space in tree.flatten(_space_nested_raw(space_nested)): is_bounded_fn = getattr(space, "is_bounded", None) @@ -122,34 +122,37 @@ def is_bounded(space_nested: gym.Space) -> bool: return True -def zeros(space_nested: gym.Space, - dtype: Optional[type] = None) -> Union[DataNested, int]: - """Allocate data structure from `gym.space.Space` and initialize it to zero. +def zeros(space: gym.Space, dtype: Optional[type] = None) -> DataNested: + """Allocate data structure from `gym.Space` and initialize it to zero. - :param space: Gym.Space on which to operate. + :param space: `gym.Space` on which to operate. :param dtype: Must be specified to overwrite original space dtype. """ - space_nested_raw = _space_nested_raw(space_nested) - values = [] - for space in tree.flatten(space_nested_raw): - if isinstance(space, gym.spaces.Box): - value = np.zeros(space.shape, dtype=dtype or space.dtype) - elif isinstance(space, gym.spaces.Discrete): - # Note that np.array of 0 dim is returned in order to be mutable - value = np.array(0, dtype=dtype or np.int64) - elif isinstance(space, gym.spaces.MultiDiscrete): - value = np.zeros_like(space.nvec, dtype=dtype or np.int64) - elif isinstance(space, gym.spaces.MultiBinary): - value = np.zeros(space.n, dtype=dtype or np.int8) - else: - raise NotImplementedError( - f"Space of type {type(space)} is not supported.") - values.append(value) - return tree.unflatten_as(space_nested_raw, values) + # Note that it is not possible to take advantage of dm-tree because the + # output type for collections (OrderedDict or Tuple) is not the same as + # the input one (gym.Space). This feature request would be too specific. + if isinstance(space, gym.spaces.Box): + return np.zeros(space.shape, dtype=dtype or space.dtype) + if isinstance(space, gym.spaces.Dict): + value = OrderedDict() + for field, subspace in dict.items(space.spaces): + value[field] = zeros(subspace, dtype=dtype) + return value + if isinstance(space, gym.spaces.Tuple): + return tuple(zeros(subspace, dtype=dtype) for subspace in space.spaces) + if isinstance(space, gym.spaces.Discrete): + # Note that np.array of 0 dim is returned in order to be mutable + return np.array(0, dtype=dtype or np.int64) + if isinstance(space, gym.spaces.MultiDiscrete): + return np.zeros_like(space.nvec, dtype=dtype or np.int64) + if isinstance(space, gym.spaces.MultiBinary): + return np.zeros(space.n, dtype=dtype or np.int8) + raise NotImplementedError( + f"Space of type {type(space)} is not supported.") def fill(data: DataNested, fill_value: float) -> None: - """Set every element of 'data' from `Gym.Space` to scalar 'fill_value'. + """Set every element of 'data' from `gym.Space` to scalar 'fill_value'. :param data: Data structure to update. :param fill_value: Value used to fill any scalar from the leaves. @@ -164,7 +167,7 @@ def fill(data: DataNested, fill_value: float) -> None: def set_value(data: DataNested, value: DataNested) -> None: - """Partially set 'data' from `Gym.Space` to 'value'. + """Partially set 'data' from `gym.Space` to 'value'. It avoids memory allocation, so that memory pointers of 'data' remains unchanged. A direct consequences, it is necessary to preallocate memory @@ -187,7 +190,7 @@ def set_value(data: DataNested, value: DataNested) -> None: def copy(data: DataNested) -> DataNested: - """Shallow copy recursively 'data' from `Gym.Space`, so that only leaves + """Shallow copy recursively 'data' from `gym.Space`, so that only leaves are still references. :param data: Hierarchical data structure to copy without allocation. @@ -197,13 +200,13 @@ def copy(data: DataNested) -> DataNested: def clip(space_nested: gym.Space, data: DataNested) -> DataNested: - """Clamp value from Gym.Space to make sure it is within bounds. + """Clamp value from `gym.Space` to make sure it is within bounds. - :param space: Gym.Space on which to operate. + :param space: `gym.Space` on which to operate. :param data: Data to clamp. """ return tree.map_structure( - lambda space, value: + lambda value, space: np.minimum(np.maximum(value, space.low), space.high) if isinstance(space, gym.spaces.Box) else value, - _space_nested_raw(space_nested), data) + data, _space_nested_raw(space_nested)) diff --git a/python/gym_jiminy/common/setup.py b/python/gym_jiminy/common/setup.py index b0ca5d4ad..d93b91c14 100644 --- a/python/gym_jiminy/common/setup.py +++ b/python/gym_jiminy/common/setup.py @@ -63,7 +63,7 @@ # - 0.18.0: dtype handling of flatten space # - >=0.18.0,<0.18.3 requires Pillow<8.0 to work, not compatible with # Python 3.9. - "gym>=0.18.3" + "gym>=0.18.3,<0.22.0" ], extras_require=extras, zip_safe=False diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/ant.py b/python/gym_jiminy/envs/gym_jiminy/envs/ant.py index 6c25f110f..e8ad88b7d 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/ant.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/ant.py @@ -60,11 +60,13 @@ def __init__(self, debug: bool = False, **kwargs) -> None: self.xpos_prev = 0.0 # Initialize base class - super().__init__(**{**dict( + super().__init__( simulator=simulator, - step_dt=STEP_DT, - enforce_bounded_spaces=False, - debug=debug), **kwargs}) + debug=debug, + **{**dict( + step_dt=STEP_DT, + enforce_bounded_spaces=False), + **kwargs}) def _neutral(self) -> np.ndarray: """ TODO: Write documentation. diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/anymal.py b/python/gym_jiminy/envs/gym_jiminy/envs/anymal.py index 91dc35d84..a1bb2a35a 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/anymal.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/anymal.py @@ -51,15 +51,17 @@ def __init__(self, debug: bool = False, **kwargs): urdf_path = os.path.join(data_root_dir, "anymal.urdf") # Initialize the walker environment - super().__init__(**{**dict( + super().__init__( urdf_path=urdf_path, mesh_path=data_root_dir, - simu_duration_max=SIMULATION_DURATION, - step_dt=STEP_DT, - reward_mixture=REWARD_MIXTURE, - std_ratio=STD_RATIO, - avoid_instable_collisions=False, - debug=debug), **kwargs}) + avoid_instable_collisions=True, + debug=debug, + **{**dict( + simu_duration_max=SIMULATION_DURATION, + step_dt=STEP_DT, + reward_mixture=REWARD_MIXTURE, + std_ratio=STD_RATIO), + **kwargs}) ANYmalPDControlJiminyEnv = build_pipeline(**{ diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/atlas.py b/python/gym_jiminy/envs/gym_jiminy/envs/atlas.py index 77aededa2..bc0cfbe4a 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/atlas.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/atlas.py @@ -4,12 +4,9 @@ from pkg_resources import resource_filename from typing import Any -import jiminy_py.core as jiminy -from jiminy_py.robot import load_hardware_description_file -from jiminy_py.simulator import Simulator -from gym_jiminy.common.envs import BaseJiminyEnv, WalkerJiminyEnv -from gym_jiminy.common.envs.env_locomotion import ( - F_PROFILE_WAVELENGTH, F_PROFILE_PERIOD) +from jiminy_py.core import build_models_from_urdf, build_reduced_models, Robot +from jiminy_py.robot import load_hardware_description_file, BaseJiminyRobot +from gym_jiminy.common.envs import WalkerJiminyEnv from gym_jiminy.common.controllers import PDController from gym_jiminy.common.pipeline import build_pipeline from gym_jiminy.toolbox.math import ConvexHull @@ -104,18 +101,20 @@ def __init__(self, debug: bool = False, **kwargs: Any) -> None: "gym_jiminy.envs", "data/bipedal_robots/atlas") urdf_path = os.path.join(data_root_dir, "atlas_v4.urdf") - # Initialize the walker environment directly - super().__init__(**{**dict( + # Initialize the walker environment + super().__init__( urdf_path=urdf_path, mesh_path=data_root_dir, - simu_duration_max=SIMULATION_DURATION, - step_dt=STEP_DT, - reward_mixture=REWARD_MIXTURE, - std_ratio=STD_RATIO, avoid_instable_collisions=True, - debug=debug), **kwargs}) - - # Remove unrelevant contact points + debug=debug, + **{**dict( + simu_duration_max=SIMULATION_DURATION, + step_dt=STEP_DT, + reward_mixture=REWARD_MIXTURE, + std_ratio=STD_RATIO), + **kwargs}) + + # Remove irrelevant contact points _cleanup_contact_points(self) def _neutral(self): @@ -146,10 +145,10 @@ def __init__(self, debug: bool = False, **kwargs: Any) -> None: # Load the full models pinocchio_model, collision_model, visual_model = \ - jiminy.build_models_from_urdf(urdf_path, - has_freeflyer=True, - build_visual_model=True, - mesh_package_dirs=[data_root_dir]) + build_models_from_urdf(urdf_path, + has_freeflyer=True, + build_visual_model=True, + mesh_package_dirs=[data_root_dir]) # Generate the reference configuration def joint_position_idx(joint_name): @@ -172,50 +171,39 @@ def joint_position_idx(joint_name): pinocchio_model.getJointId(joint_name) for joint_name in pinocchio_model.names[2:] if "_leg_" not in joint_name] - pinocchio_model, collision_model, visual_model = \ - jiminy.build_reduced_models(pinocchio_model, - collision_model, - visual_model, - joint_locked_indices, - qpos) + pinocchio_model, collision_model, visual_model = build_reduced_models( + pinocchio_model, + collision_model, + visual_model, + joint_locked_indices, + qpos) # Build the robot and load the hardware - robot = jiminy.Robot() - robot.initialize(pinocchio_model, collision_model, visual_model) - hardware_path = str( - Path(urdf_path).with_suffix('')) + '_hardware.toml' - config_path = str( - Path(urdf_path).with_suffix('')) + '_options.toml' - load_hardware_description_file(robot, hardware_path, True, False) - - # Instantiate a simulator and load the options - simulator = Simulator(robot) - simulator.import_options(config_path) - - # Set base class attributes manually - self.simu_duration_max = SIMULATION_DURATION - self.reward_mixture = { - k: v for k, v in REWARD_MIXTURE.items() if v > 0.0} - self.urdf_path = urdf_path - self.mesh_path = data_root_dir - self.hardware_path = hardware_path - self.config_path = config_path - self.std_ratio = { - k: v for k, v in STD_RATIO.items() if v > 0.0} - self.avoid_instable_collisions = True - self._f_xy_profile = [ - jiminy.PeriodicGaussianProcess( - F_PROFILE_WAVELENGTH, F_PROFILE_PERIOD), - jiminy.PeriodicGaussianProcess( - F_PROFILE_PERIOD, F_PROFILE_PERIOD)] - self._power_consumption_max = 0.0 - self._height_neutral = 0.0 - - # Initialize base environment - BaseJiminyEnv.__init__(self, simulator, **{**dict( - step_dt=STEP_DT, debug=debug), **kwargs}) - - # Remove unrelevant contact points + robot = BaseJiminyRobot() + Robot.initialize(robot, pinocchio_model, collision_model, visual_model) + robot._urdf_path_orig = urdf_path + hardware_path = str(Path(urdf_path).with_suffix('')) + '_hardware.toml' + load_hardware_description_file( + robot, + hardware_path, + avoid_instable_collisions=True, + verbose=debug) + + # Initialize the walker environment + super().__init__( + robot=robot, + urdf_path=urdf_path, + mesh_path=data_root_dir, + avoid_instable_collisions=True, + debug=debug, + **{**dict( + simu_duration_max=SIMULATION_DURATION, + step_dt=STEP_DT, + reward_mixture=REWARD_MIXTURE, + std_ratio=STD_RATIO), + **kwargs}) + + # Remove irrelevant contact points _cleanup_contact_points(self) diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py b/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py index cf2c937d2..5bae0f686 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py @@ -1,23 +1,28 @@ import os import math import numpy as np +from pathlib import Path from pkg_resources import resource_filename -from jiminy_py.core import ( - DistanceConstraint, JointConstraint, get_joint_type, joint_t) - -from pinocchio import neutral, SE3 - +from jiminy_py.core import (joint_t, + get_joint_type, + build_models_from_urdf, + build_reduced_models, + DistanceConstraint, + Robot) +from jiminy_py.robot import load_hardware_description_file, BaseJiminyRobot from gym_jiminy.common.envs import WalkerJiminyEnv from gym_jiminy.common.controllers import PDController from gym_jiminy.common.pipeline import build_pipeline +from pinocchio import neutral, SE3 + # Parameters of neutral configuration -DEFAULT_SAGITTAL_HIP_ANGLE = 30.0 / 180.0 * math.pi +DEFAULT_SAGITTAL_HIP_ANGLE = 23.0 / 180.0 * math.pi DEFAULT_KNEE_ANGLE = -65.0 / 180.0 * math.pi DEFAULT_ANKLE_ANGLE = 80.0 / 180.0 * math.pi -DEFAULT_TOE_ANGLE = -95.0 / 180.0 * math.pi +DEFAULT_TOE_ANGLE = -90.0 / 180.0 * math.pi # Default simulation duration (:float [s]) SIMULATION_DURATION = 20.0 @@ -58,21 +63,49 @@ def __init__(self, debug: bool = False, **kwargs): "gym_jiminy.envs", "data/bipedal_robots/cassie") urdf_path = os.path.join(data_root_dir, "cassie.urdf") + # Load the full models + pinocchio_model, collision_model, visual_model = \ + build_models_from_urdf(urdf_path, + has_freeflyer=True, + build_visual_model=True, + mesh_package_dirs=[data_root_dir]) + + # Fix passive rotary joints with spring. + # Alternatively, it would be more realistic to model them using the + # internal dynamics of the controller to add spring forces, but it + # would slow down the simulation. + qpos = neutral(pinocchio_model) + joint_locked_indices = [ + pinocchio_model.getJointId(joint_name) + for joint_name in ("knee_to_shin_right", "knee_to_shin_left")] + pinocchio_model, collision_model, visual_model = build_reduced_models( + pinocchio_model, collision_model, visual_model, + joint_locked_indices, qpos) + + # Build the robot and load the hardware + robot = BaseJiminyRobot() + Robot.initialize(robot, pinocchio_model, collision_model, visual_model) + robot._urdf_path_orig = urdf_path + hardware_path = str(Path(urdf_path).with_suffix('')) + '_hardware.toml' + load_hardware_description_file( + robot, + hardware_path, + avoid_instable_collisions=True, + verbose=debug) + # Initialize the walker environment - super().__init__(**{**dict( + super().__init__( + robot=robot, urdf_path=urdf_path, mesh_path=data_root_dir, - simu_duration_max=SIMULATION_DURATION, - step_dt=STEP_DT, - reward_mixture=REWARD_MIXTURE, - std_ratio=STD_RATIO, - avoid_instable_collisions=False, - debug=debug), **kwargs}) - - # Remove unrelevant contact points - self.robot.remove_contact_points([ - name for name in self.robot.contact_frames_names - if int(name.split("_")[-1]) in (0, 1, 4, 5)]) + avoid_instable_collisions=True, + debug=debug, + **{**dict( + simu_duration_max=SIMULATION_DURATION, + step_dt=STEP_DT, + reward_mixture=REWARD_MIXTURE, + std_ratio=STD_RATIO), + **kwargs}) # Add missing pushrod close kinematic chain constraint M_pushrod_tarsus_right = SE3( @@ -100,15 +133,10 @@ def __init__(self, debug: bool = False, **kwargs): pushrod_left.baumgarte_freq = 2.0 self.robot.add_constraint("pushrod_left", pushrod_left) - # Replace knee to shin spring by fixed joint constraint - right_spring_knee_to_shin = JointConstraint("knee_to_shin_right") - right_spring_knee_to_shin.baumgarte_freq = 20.0 - self.robot.add_constraint( - "right_spring_knee_to_shin", right_spring_knee_to_shin) - left_spring_knee_to_shin = JointConstraint("knee_to_shin_left") - left_spring_knee_to_shin.baumgarte_freq = 20.0 - self.robot.add_constraint( - "left_spring_knee_to_shin", left_spring_knee_to_shin) + # Remove irrelevant contact points + self.robot.remove_contact_points([ + name for name in self.robot.contact_frames_names + if int(name.split("_")[-1]) in (0, 1, 4, 5)]) def _neutral(self): def set_joint_rotary_position(joint_name, q_full, theta): diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/spotmicro.py b/python/gym_jiminy/envs/gym_jiminy/envs/spotmicro.py index a246d3108..64a3e10c5 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/spotmicro.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/spotmicro.py @@ -35,12 +35,14 @@ def __init__(self, debug: bool = False, **kwargs): urdf_path = os.path.join(data_root_dir, "spotmicro.urdf") # Initialize the walker environment - super().__init__(**{**dict( + super().__init__( urdf_path=urdf_path, mesh_path=data_root_dir, - simu_duration_max=SIMULATION_DURATION, - step_dt=STEP_DT, - reward_mixture=REWARD_MIXTURE, - std_ratio=STD_RATIO, - avoid_instable_collisions=False, - debug=debug), **kwargs}) + avoid_instable_collisions=True, + debug=debug, + **{**dict( + simu_duration_max=SIMULATION_DURATION, + step_dt=STEP_DT, + reward_mixture=REWARD_MIXTURE, + std_ratio=STD_RATIO), + **kwargs}) diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/__init__.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/__init__.py index 4ed15926e..8c681d143 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/__init__.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/__init__.py @@ -1,54 +1,11 @@ # pylint: disable=missing-module-docstring -from typing import Dict, Any - -import ray.tune.logger - from . import ppo from . import utilities from . import callbacks from . import curriculum -# Patch flatten dict method that is deep-copying the input dict in the original -# implementation instead of performing a shallow copy, which is slowing down -# down the optimization very significantly for no reason since tensorboard is -# already copying the data internally. -def _flatten_dict(dt: Dict[str, Any], - delimiter: str = "/", - prevent_delimiter: bool = False) -> Dict[str, Any]: - """Must be patched to use copy instead of deepcopy to prevent memory - allocation, significantly impeding computational efficiency of `TBXLogger`, - and slowing down the optimization by about 25%. - """ - dt = dt.copy() - if prevent_delimiter and any(delimiter in key for key in dt): - # Raise if delimiter is any of the keys - raise ValueError( - "Found delimiter `{}` in key when trying to flatten array." - "Please avoid using the delimiter in your specification.") - while any(isinstance(v, dict) for v in dt.values()): - remove = [] - add = {} - for key, value in dt.items(): - if isinstance(value, dict): - for subkey, v in value.items(): - if prevent_delimiter and delimiter in subkey: - # Raise if delimiter is in any of the subkeys - raise ValueError( - "Found delimiter `{}` in key when trying to " - "flatten array. Please avoid using the delimiter " - "in your specification.") - add[delimiter.join([key, str(subkey)])] = v - remove.append(key) - dt.update(add) - for k in remove: - del dt[k] - return dt - -ray.tune.logger.flatten_dict = _flatten_dict # noqa - - __all__ = [ "ppo", "utilities", diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/callbacks.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/callbacks.py index 598121e6b..23efd1d50 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/callbacks.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/callbacks.py @@ -1,11 +1,15 @@ """ TODO: Write documentation. """ -from typing import Type, Any +from typing import Any, Dict, Optional from ray.rllib.env import BaseEnv -from ray.rllib.evaluation import MultiAgentEpisode +from ray.rllib.policy import Policy +from ray.rllib.evaluation.episode import Episode +from ray.rllib.evaluation.worker_set import WorkerSet +from ray.rllib.evaluation.rollout_worker import RolloutWorker from ray.rllib.agents.trainer import Trainer from ray.rllib.agents.callbacks import DefaultCallbacks +from ray.rllib.utils.typing import PolicyID class MonitorInfoCallback(DefaultCallbacks): @@ -15,26 +19,40 @@ class MonitorInfoCallback(DefaultCallbacks): def on_episode_step(self, *, - episode: MultiAgentEpisode, + worker: RolloutWorker, + base_env: BaseEnv, + policies: Optional[Dict[PolicyID, Policy]] = None, + episode: Episode, **kwargs: Any) -> None: """ TODO: Write documentation. """ - super().on_episode_step(episode=episode, **kwargs) + super().on_episode_step(worker=worker, + base_env=base_env, + policies=policies, + episode=episode, + **kwargs) info = episode.last_info_for() if info is not None: for key, value in info.items(): + # TODO: This line cause memory to grow unboundely episode.hist_data.setdefault(key, []).append(value) def on_episode_end(self, *, + worker: RolloutWorker, base_env: BaseEnv, - episode: MultiAgentEpisode, + policies: Dict[PolicyID, Policy], + episode: Episode, **kwargs: Any) -> None: """ TODO: Write documentation. """ - super().on_episode_end(base_env=base_env, episode=episode, **kwargs) + super().on_episode_end(worker=worker, + base_env=base_env, + policies=policies, + episode=episode, + **kwargs) episode.custom_metrics["episode_duration"] = \ - base_env.get_unwrapped()[0].step_dt * episode.length + base_env.get_sub_environments()[0].step_dt * episode.length class CurriculumUpdateCallback(DefaultCallbacks): @@ -48,28 +66,17 @@ def on_train_result(self, """ TODO: Write documentation. """ super().on_train_result(trainer=trainer, result=result, **kwargs) - trainer.workers.foreach_worker( - lambda worker: worker.foreach_env( - lambda env: env.update(result))) - - -def build_callbacks(*callbacks: Type) -> DefaultCallbacks: - """Aggregate several callback mixin together. - .. note:: - Note that the order is important if several mixin are implementing the - same method. It follows the same precedence roles than usual multiple - inheritence, namely ordered from highest to lowest priority. + # Assertion(s) for type checker + workers = trainer.workers + assert isinstance(workers, WorkerSet) - :param callbacks: Sequence of callback objects. - """ - # TODO: Remove this method after release of ray 1.4.0 and use instead - # `ray.rllib.agents.callbacks.MultiCallbacks`. - return type("UnifiedCallbacks", callbacks, {}) + workers.foreach_worker( + lambda worker: worker.foreach_env( + lambda env: env.update(result))) __all__ = [ "MonitorInfoCallback", - "CurriculumUpdateCallback", - "build_callbacks" + "CurriculumUpdateCallback" ] diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/curriculum.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/curriculum.py index 2d2f6adba..50696fff4 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/curriculum.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/curriculum.py @@ -9,9 +9,13 @@ from ray.tune.logger import TBXLogger from ray.tune.result import TRAINING_ITERATION, TIMESTEPS_TOTAL from ray.rllib.env import BaseEnv -from ray.rllib.evaluation import MultiAgentEpisode +from ray.rllib.policy import Policy +from ray.rllib.evaluation.episode import Episode +from ray.rllib.evaluation.worker_set import WorkerSet +from ray.rllib.evaluation.rollout_worker import RolloutWorker from ray.rllib.agents.trainer import Trainer from ray.rllib.agents.callbacks import DefaultCallbacks +from ray.rllib.utils.typing import PolicyID from gym_jiminy.toolbox.wrappers.meta_envs import DataTreeT @@ -35,17 +39,22 @@ def __init__(self) -> None: def on_episode_end(self, *, + worker: RolloutWorker, base_env: BaseEnv, - episode: MultiAgentEpisode, + policies: Dict[PolicyID, Policy], + episode: Episode, **kwargs: Any) -> None: """ TODO: Write documentation. """ # Call base implementation - super().on_episode_end( - base_env=base_env, episode=episode, **kwargs) + super().on_episode_end(worker=worker, + base_env=base_env, + policies=policies, + episode=episode, + **kwargs) # Monitor episode duration for each gait - for env in base_env.get_unwrapped(): + for env in base_env.get_sub_environments(): # Gather the set of tasks. # It corresponds to all the leaves of the task decision tree. tasks: List[List[Any]] = [[]] @@ -196,8 +205,12 @@ def on_train_result(self, if task_branch_next: task_branches.append(task_branch_next) + # Assertion(s) for type checker + workers = trainer.workers + assert isinstance(workers, WorkerSet) + # Update envs accordingly - trainer.workers.foreach_worker( + workers.foreach_worker( lambda worker: worker.foreach_env( lambda env: env.task_tree_probas.update(task_tree_probas))) diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py index f3f3c98a6..bda6c1463 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py @@ -8,22 +8,26 @@ import gym import torch -from ray.rllib.models.modelv2 import ModelV2 +from ray.rllib.models.torch.torch_modelv2 import TorchModelV2 from ray.rllib.models.torch.torch_action_dist import ( TorchDistributionWrapper, TorchDiagGaussian) from ray.rllib.policy.policy import Policy from ray.rllib.policy.sample_batch import SampleBatch from ray.rllib.policy.view_requirement import ViewRequirement -from ray.rllib.utils.torch_ops import l2_loss +from ray.rllib.policy.torch_policy import EntropyCoeffSchedule, \ + LearningRateSchedule, TorchPolicy +from ray.rllib.utils.numpy import convert_to_numpy +from ray.rllib.utils.torch_utils import l2_loss from ray.rllib.utils.typing import TensorType, TrainerConfigDict -from ray.rllib.agents.ppo import DEFAULT_CONFIG, PPOTrainer +from ray.rllib.agents.ppo import ( + DEFAULT_CONFIG as _DEFAULT_CONFIG, PPOTrainer as _PPOTrainer) from ray.rllib.agents.ppo.ppo_torch_policy import ( - ppo_surrogate_loss, kl_and_loss_stats, setup_mixins, PPOTorchPolicy) + PPOTorchPolicy as _PPOTorchPolicy) -DEFAULT_CONFIG = PPOTrainer.merge_trainer_configs( - DEFAULT_CONFIG, +DEFAULT_CONFIG = _PPOTrainer.merge_trainer_configs( + _DEFAULT_CONFIG, { "enable_adversarial_noise": False, "spatial_noise_scale": 1.0, @@ -41,10 +45,16 @@ _allow_unknown_configs=True) -def get_action_mean(model: ModelV2, +def get_action_mean(model: TorchModelV2, dist_class: Type[TorchDistributionWrapper], action_logits: torch.Tensor) -> torch.Tensor: - """ TODO: Write documentation. + """Compute the mean value of the actions based on action distribution + logits and type of distribution. + + .. note: + It performs deterministic sampling for all distributions except + multivariate independent normal distribution, for which the mean can be + very efficiently extracted as a view of the logits. """ if issubclass(dist_class, TorchDiagGaussian): action_mean, _ = torch.chunk(action_logits, 2, dim=1) @@ -55,7 +65,7 @@ def get_action_mean(model: ModelV2, def get_adversarial_observation_sgld( - model: ModelV2, + model: TorchModelV2, dist_class: Type[TorchDistributionWrapper], train_batch: SampleBatch, noise_scale: float, @@ -63,7 +73,9 @@ def get_adversarial_observation_sgld( n_steps: int, action_true_mean: Optional[torch.Tensor] = None ) -> torch.Tensor: - """ TODO: Write documentation. + """Compute adversarial observation maximizing Mean Squared Error between + the original and the perturbed mean action using Stochastic gradient + Langevin dynamics algorithm (SGLD). """ # Compute mean field action for true observation if not provided if action_true_mean is None: @@ -133,346 +145,417 @@ def get_adversarial_observation_sgld( return observation_noisy -def ppo_init(policy: Policy, - obs_space: gym.spaces.Space, - action_space: gym.spaces.Space, - config: TrainerConfigDict) -> None: - """ TODO: Write documentation. +def _compute_mirrored_value(value: torch.Tensor, + space: gym.spaces.Space, + mirror_mat: Union[ + Dict[str, torch.Tensor], torch.Tensor] + ) -> torch.Tensor: + """Compute mirrored value from observation space based on provided + mirroring transformation. """ - # Call base implementation - setup_mixins(policy, obs_space, action_space, config) - - # Add previous observation in viewer requirements for CAPS loss computation - # TODO: Remove update of `policy.model.view_requirements` after ray fix - caps_view_requirements = { - "_prev_obs": ViewRequirement( - data_col="obs", - space=obs_space, - shift=-1, - used_for_compute_actions=False)} - policy.model.view_requirements.update(caps_view_requirements) - policy.view_requirements.update(caps_view_requirements) - - # Initialize extra loss - policy._loss_temporal_barrier_reg = 0.0 - policy._loss_symmetric_policy_reg = 0.0 - policy._loss_caps_temporal_reg = 0.0 - policy._loss_caps_spatial_reg = 0.0 - policy._loss_caps_global_reg = 0.0 - policy._loss_l2_reg = 0.0 - - # Extract original observation space - try: - observation_space = policy.observation_space.original_space - except AttributeError as e: - raise NotImplementedError( - "Only 'Dict' original observation space is supported.") from e - - # Transpose and convert to torch.Tensor the observation mirroring data - for field, mirror_mat in observation_space.mirror_mat.items(): - if not isinstance(mirror_mat, torch.Tensor): - mirror_mat = torch.from_numpy( - mirror_mat.T.copy()).to(dtype=torch.float32) - observation_space.mirror_mat[field] = mirror_mat - - # Transpose and convert to torch.Tensor the action mirroring data - action_space = policy.action_space - if not isinstance(action_space.mirror_mat, torch.Tensor): - action_mirror_mat = torch.from_numpy( - action_space.mirror_mat.T.copy()).to(dtype=torch.float32) - action_space.mirror_mat = action_mirror_mat - - -def ppo_loss(policy: Policy, - model: ModelV2, - dist_class: Type[TorchDistributionWrapper], - train_batch: SampleBatch - ) -> Union[TensorType, List[TensorType]]: - """ TODO: Write documentation. - """ - # Extract some proxies from convenience - observation_true = train_batch["obs"] - device = observation_true.device - - # Initialize the set of training batches to forward to the model - train_batches = {"true": train_batch} - - if policy.config["caps_temporal_reg"] > 0.0 or \ - policy.config["temporal_barrier_reg"] > 0.0: - # Shallow copy the original training batch - train_batch_copy = train_batch.copy(shallow=True) - - # Replace current observation by the previous one - observation_prev = train_batch["_prev_obs"] - train_batch_copy["obs"] = observation_prev - - # Append the training batches to the set - train_batches["prev"] = train_batch_copy - - if policy.config["caps_spatial_reg"] > 0.0 and \ - policy.config["enable_adversarial_noise"]: - # Shallow copy the original training batch - train_batch_copy = train_batch.copy(shallow=True) - - # Compute adversarial observation maximizing action difference - observation_worst = get_adversarial_observation_sgld( - model, dist_class, train_batch, - policy.config["spatial_noise_scale"], - policy.config["sgld_beta_inv"], - policy.config["sgld_n_steps"]) - - # Replace current observation by the adversarial one - train_batch_copy["obs"] = observation_worst - - # Append the training batches to the set - train_batches["worst"] = train_batch_copy - - if policy.config["caps_global_reg"] > 0.0 or \ - not policy.config["enable_adversarial_noise"]: - # Shallow copy the original training batch - train_batch_copy = train_batch.copy(shallow=True) - - # Generate noisy observation - observation_noisy = torch.normal( - observation_true, policy.config["spatial_noise_scale"]) - - # Replace current observation by the noisy one - train_batch_copy["obs"] = observation_noisy - - # Append the training batches to the set - train_batches["noisy"] = train_batch_copy - - if policy.config["symmetric_policy_reg"] > 0.0: - # Shallow copy the original training batch - train_batch_copy = train_batch.copy(shallow=True) - - # Compute mirrorred observation + def _update_flattened_slice(data: torch.Tensor, + shape: Tuple[int, ...], + mirror_mat: torch.Tensor) -> torch.Tensor: + """Mirror an array of flattened tensor using provided transformation + matrix. + """ + if len(shape) > 1: + data = data.reshape((-1, *shape)) \ + .swapaxes(1, 0) \ + .reshape((shape[0], -1)) + data_mirrored = mirror_mat @ data + return data_mirrored.reshape((shape[0], -1, shape[1])) \ + .swapaxes(1, 0) \ + .reshape((-1, *shape)) + return torch.mm(data, mirror_mat) + + if isinstance(mirror_mat, dict): offset = 0 - observation_mirror = torch.empty_like(observation_true) - observation_space = policy.observation_space.original_space - for field, mirror_mat in observation_space.mirror_mat.items(): - field_shape = observation_space[field].shape + value_mirrored = [] + for field, slice_mirror_mat in mirror_mat.items(): + field_shape = space.original_space[field].shape field_size = reduce(operator.mul, field_shape) slice_idx = slice(offset, offset + field_size) - - mirror_mat = mirror_mat.to(device) - observation_space.mirror_mat[field] = mirror_mat - - if len(field_shape) > 1: - observation_true_slice = observation_true[:, slice_idx] \ - .reshape((-1, field_shape[0], field_shape[1])) \ - .swapaxes(1, 0) \ - .reshape((field_shape[0], -1)) - observation_mirror_slice = mirror_mat @ observation_true_slice - observation_mirror[:, slice_idx] = observation_mirror_slice \ - .reshape((field_shape[0], -1, field_shape[1])) \ - .swapaxes(1, 0) \ - .reshape((-1, field_size)) - else: - torch.mm(observation_true[..., slice_idx], - mirror_mat, - out=observation_mirror[..., slice_idx]) - + slice_mirrored = _update_flattened_slice( + value[:, slice_idx], field_shape, slice_mirror_mat) + value_mirrored.append(slice_mirrored) offset += field_size + return torch.cat(value_mirrored, dim=1) + return _update_flattened_slice(value, space.shape, mirror_mat) + + +class PPOTorchPolicy(_PPOTorchPolicy): + """Add regularization losses on top of the original loss of PPO. + More specifically, it adds: + - CAPS regularization, which combines the spatial and temporal + difference betwen previous and current state + - Global regularization, which is the average norm of the action + - temporal barrier, which is exponential barrier loss when the + normalized action is above a threshold (much like interior point + methods). + - symmetry regularization, which is the error between actions and + symmetric actions associated with symmetric observations. + - L2 regularization of policy network weights + """ + def __init__(self, + obs_space: gym.spaces.Space, + action_space: gym.spaces.Space, + config: TrainerConfigDict) -> None: + """Initialize PPO Torch policy. - # Replace current observation by the mirrored one - train_batch_copy["obs"] = observation_mirror - - # Append the training batches to the set - train_batches["mirrored"] = train_batch_copy - - # Compute the action_logits for all training batches at onces - train_batch_all = {} - for k in ["obs"]: - train_batch_all[k] = torch.cat([ - s[k] for s in train_batches.values()], dim=0) - action_logits_all, _ = model(train_batch_all) - values_all = model.value_function() - action_logits = dict(zip(train_batches.keys(), torch.chunk( - action_logits_all, len(train_batches), dim=0))) - values = dict(zip(train_batches.keys(), torch.chunk( - values_all, len(train_batches), dim=0))) - - # Compute original ppo loss. - # pylint: disable=unused-argument,missing-function-docstring - class FakeModel: - """Fake model enabling doing all forward passes at once. + It extracts observation mirroring transforms for symmetry computations. """ - def __init__(self, - model: ModelV2, - action_logits: torch.Tensor, - value: torch.Tensor) -> None: - self._action_logits = action_logits - self._value = value - self._model = model - - def __getattr__(self, name: str) -> Any: - return getattr(self._model, name) - - def __call__(self, *args: Any, **kwargs: Any - ) -> Tuple[torch.Tensor, List[torch.Tensor]]: - return self._action_logits, [] - - def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor: - return self._value - - fake_model = FakeModel(model, action_logits["true"], values["true"]) - total_loss = ppo_surrogate_loss( - policy, fake_model, dist_class, train_batch) - - # Extract mean of predicted action from action_logits. - # No need to compute the perform model forward pass since the original - # PPO loss is already doing it, so just getting back the last ouput. - action_true_logits = action_logits["true"] - action_true_mean = get_action_mean(model, dist_class, action_true_logits) - - # Compute the mean action corresponding to the previous observation - if policy.config["caps_temporal_reg"] > 0.0 or \ - policy.config["temporal_barrier_reg"] > 0.0: - action_prev_logits = action_logits["prev"] - action_prev_mean = get_action_mean( - model, dist_class, action_prev_logits) - - # Compute the mean action corresponding to the noisy observation - if policy.config["caps_global_reg"] > 0.0 or \ - not policy.config["enable_adversarial_noise"]: - action_noisy_logits = action_logits["noisy"] - action_noisy_mean = get_action_mean( - model, dist_class, action_noisy_logits) + # pylint: disable=non-parent-init-called,super-init-not-called + + # Update default config wich provided partial config + config = dict(DEFAULT_CONFIG, **config) + + # Call base implementation. Note that `PPOTorchPolicy.__init__` is + # bypassed because it calls `_initialize_loss_from_dummy_batch` + # automatically, and mirroring matrices are not extracted at this + # point. It is not possible to extract them since `self.device` is set + # by `TorchPolicy.__init__`. + TorchPolicy.__init__( + self, + obs_space, + action_space, + config, + max_seq_len=config["model"]["max_seq_len"]) + + # TODO: Remove update of `policy.model.view_requirements` after ray fix + # https://github.com/ray-project/ray/pull/21043 + self.model.view_requirements["prev_obs"] = \ + self.view_requirements["prev_obs"] + + # Initialize mixins + EntropyCoeffSchedule.__init__(self, config["entropy_coeff"], + config["entropy_coeff_schedule"]) + LearningRateSchedule.__init__(self, config["lr"], + config["lr_schedule"]) + + # Current KL value + self.kl_coeff = self.config["kl_coeff"] + # Constant target value + self.kl_target = self.config["kl_target"] + + # Extract and convert observation and acrtion mirroring transform + self.obs_mirror_mat: Optional[Union[ + Dict[str, torch.Tensor], torch.Tensor]] = None + self.action_mirror_mat: Optional[Union[ + Dict[str, torch.Tensor], torch.Tensor]] = None + if config["symmetric_policy_reg"] > 0.0: + is_obs_dict = hasattr(obs_space, "original_space") + if is_obs_dict: + obs_space = obs_space.original_space + # Observation space + if is_obs_dict: + self.obs_mirror_mat = {} + for field, mirror_mat in obs_space.mirror_mat.items(): + obs_mirror_mat = torch.tensor(mirror_mat, + dtype=torch.float32, + device=self.device) + self.obs_mirror_mat[field] = obs_mirror_mat.T.contiguous() + else: + obs_mirror_mat = torch.tensor(obs_space.mirror_mat, + dtype=torch.float32, + device=self.device) + self.obs_mirror_mat = obs_mirror_mat.T.contiguous() + + # Action space + action_mirror_mat = torch.tensor(action_space.mirror_mat, + dtype=torch.float32, + device=self.device) + self.action_mirror_mat = action_mirror_mat.T.contiguous() + + # TODO: Don't require users to call this manually. + self._initialize_loss_from_dummy_batch() + + def _get_default_view_requirements(self) -> None: + """Add previous observation to view requirements for CAPS + regularization. + """ + view_requirements = super()._get_default_view_requirements() + view_requirements["prev_obs"] = ViewRequirement( + data_col=SampleBatch.OBS, + space=self.observation_space, + shift=-1, + used_for_compute_actions=False, + used_for_training=True) + return view_requirements - # Compute the mean action corresponding to the worst observation - if policy.config["caps_spatial_reg"] > 0.0 and \ - policy.config["enable_adversarial_noise"]: - action_worst_logits = action_logits["worst"] - action_worst_mean = get_action_mean( - model, dist_class, action_worst_logits) - - # Compute the mirrored mean action corresponding to the mirrored action - if policy.config["symmetric_policy_reg"] > 0.0: - action_mirror_logits = action_logits["mirrored"] - action_mirror_mean = get_action_mean( - model, dist_class, action_mirror_logits) - action_mirror_mat = policy.action_space.mirror_mat.to(device) - policy.action_space.mirror_mat = action_mirror_mat - action_mirror_mean = action_mirror_mean @ action_mirror_mat - - if policy.config["caps_temporal_reg"] > 0.0 or \ - policy.config["temporal_barrier_reg"] > 0.0: - # Compute action temporal delta - action_delta = (action_prev_mean - action_true_mean).abs() - - # Minimize the difference between the successive action mean - if policy.config["caps_temporal_reg"] > 0.0: - policy._loss_caps_temporal_reg = torch.mean(action_delta) - - # Add temporal smoothness loss to total loss - total_loss += policy.config["caps_temporal_reg"] * \ - policy._loss_caps_temporal_reg - - # Add temporal barrier loss to total loss: - # exp(scale * (err - thr)) - 1.0 if err > thr else 0.0 - if policy.config["temporal_barrier_reg"] > 0.0: - scale = policy.config["temporal_barrier_scale"] - threshold = policy.config["temporal_barrier_threshold"] - policy._loss_temporal_barrier_reg = torch.mean(torch.exp( - torch.clamp( - scale * (action_delta - threshold), min=0.0, max=5.0 - )) - 1.0) - - # Add spatial smoothness loss to total loss - total_loss += policy.config["temporal_barrier_reg"] * \ - policy._loss_temporal_barrier_reg - - if policy.config["caps_spatial_reg"] > 0.0: - # Minimize the difference between the original action mean and the - # perturbed one. - if policy.config["enable_adversarial_noise"]: - policy._loss_caps_spatial_reg = torch.mean( - torch.sum((action_worst_mean - action_true_mean) ** 2, dim=1)) - else: - policy._loss_caps_spatial_reg = torch.mean( - torch.sum((action_noisy_mean - action_true_mean) ** 2, dim=1)) - - # Add spatial smoothness loss to total loss - total_loss += policy.config["caps_spatial_reg"] * \ - policy._loss_caps_spatial_reg - - if policy.config["caps_global_reg"] > 0.0: - # Minimize the magnitude of action mean - policy._loss_caps_global_reg = torch.mean(action_noisy_mean ** 2) - - # Add global smoothness loss to total loss - total_loss += policy.config["caps_global_reg"] * \ - policy._loss_caps_global_reg - - if policy.config["symmetric_policy_reg"] > 0.0: - # Minimize the assymetry of policy output - policy._loss_symmetric_policy_reg = torch.mean( - (action_mirror_mean - action_true_mean) ** 2) - - # Add policy symmetry loss to total loss - total_loss += policy.config["symmetric_policy_reg"] * \ - policy._loss_symmetric_policy_reg - - if policy.config["l2_reg"] > 0.0: - # Add actor l2-regularization loss - l2_reg_loss = 0.0 - for name, params in model.named_parameters(): - if not name.endswith("bias"): - l2_reg_loss += l2_loss(params) - policy._loss_l2_reg = l2_reg_loss - - # Add l2-regularization loss to total loss - total_loss += policy.config["l2_reg"] * policy._loss_l2_reg - - return total_loss - - -def ppo_stats(policy: Policy, - train_batch: SampleBatch) -> Dict[str, TensorType]: - """ TODO: Write documentation. - """ - # Compute original stats report - stats_dict = kl_and_loss_stats(policy, train_batch) - - # Add spatial CAPS loss to the report - if policy.config["symmetric_policy_reg"] > 0.0: - stats_dict["symmetry"] = policy._loss_symmetric_policy_reg - if policy.config["temporal_barrier_reg"] > 0.0: - stats_dict["temporal_barrier"] = policy._loss_temporal_barrier_reg - if policy.config["caps_temporal_reg"] > 0.0: - stats_dict["temporal_smoothness"] = policy._loss_caps_temporal_reg - if policy.config["caps_spatial_reg"] > 0.0: - stats_dict["spatial_smoothness"] = policy._loss_caps_spatial_reg - if policy.config["caps_global_reg"] > 0.0: - stats_dict["global_smoothness"] = policy._loss_caps_global_reg - if policy.config["l2_reg"] > 0.0: - stats_dict["l2_reg"] = policy._loss_l2_reg - - return stats_dict - - -PPOTorchPolicy = PPOTorchPolicy.with_updates( - before_loss_init=ppo_init, - loss_fn=ppo_loss, - stats_fn=ppo_stats, - get_default_config=lambda: DEFAULT_CONFIG -) - - -def get_policy_class( - config: TrainerConfigDict) -> Optional[Type[Policy]]: - """ TODO: Write documentation. + def loss(self, + model: TorchModelV2, + dist_class: Type[TorchDistributionWrapper], + train_batch: SampleBatch) -> Union[TensorType, List[TensorType]]: + """Compute PPO loss with additional regulizations. + """ + with torch.no_grad(): + # Extract some proxies from convenience + observation_true = train_batch["obs"] + + # Initialize the set of training batches to forward to the model + train_batches = {"true": train_batch} + + if self.config["caps_temporal_reg"] > 0.0 or \ + self.config["temporal_barrier_reg"] > 0.0: + # Shallow copy the original training batch + train_batch_copy = train_batch.copy(shallow=True) + + # Replace current observation by the previous one + observation_prev = train_batch["prev_obs"] + train_batch_copy["obs"] = observation_prev + + # Append the training batches to the set + train_batches["prev"] = train_batch_copy + + if self.config["caps_spatial_reg"] > 0.0 and \ + self.config["enable_adversarial_noise"]: + # Shallow copy the original training batch + train_batch_copy = train_batch.copy(shallow=True) + + # Compute adversarial observation maximizing action difference + observation_worst = get_adversarial_observation_sgld( + model, dist_class, train_batch, + self.config["spatial_noise_scale"], + self.config["sgld_beta_inv"], + self.config["sgld_n_steps"]) + + # Replace current observation by the adversarial one + train_batch_copy["obs"] = observation_worst + + # Append the training batches to the set + train_batches["worst"] = train_batch_copy + + if self.config["caps_global_reg"] > 0.0 or \ + not self.config["enable_adversarial_noise"]: + # Shallow copy the original training batch + train_batch_copy = train_batch.copy(shallow=True) + + # Generate noisy observation + observation_noisy = torch.normal( + observation_true, self.config["spatial_noise_scale"]) + + # Replace current observation by the noisy one + train_batch_copy["obs"] = observation_noisy + + # Append the training batches to the set + train_batches["noisy"] = train_batch_copy + + if self.config["symmetric_policy_reg"] > 0.0: + # Shallow copy the original training batch + train_batch_copy = train_batch.copy(shallow=True) + + # Compute mirrorred observation + assert self.obs_mirror_mat is not None + observation_mirror = _compute_mirrored_value( + observation_true, + self.observation_space, + self.obs_mirror_mat) + + # Replace current observation by the mirrored one + train_batch_copy["obs"] = observation_mirror + + # Append the training batches to the set + train_batches["mirrored"] = train_batch_copy + + # Compute the action_logits for all training batches at onces + train_batch_all = { + "obs": torch.cat([ + s["obs"] for s in train_batches.values()], dim=0)} + action_logits_all, _ = model(train_batch_all) + values_all = model.value_function() + action_logits = dict(zip(train_batches.keys(), torch.chunk( + action_logits_all, len(train_batches), dim=0))) + values = dict(zip(train_batches.keys(), torch.chunk( + values_all, len(train_batches), dim=0))) + + # Compute original ppo loss. + # pylint: disable=unused-argument,missing-function-docstring + class FakeModel: + """Fake model enabling doing all forward passes at once. + """ + def __init__(self, + model: TorchModelV2, + action_logits: torch.Tensor, + value: torch.Tensor) -> None: + self._action_logits = action_logits + self._value = value + self._model = model + + def __getattr__(self, name: str) -> Any: + return getattr(self._model, name) + + def __call__( + self, *args: Any, **kwargs: Any + ) -> Tuple[torch.Tensor, List[torch.Tensor]]: + return self._action_logits, [] + + def value_function( + self, *args: Any, **kwargs: Any) -> torch.Tensor: + return self._value + + fake_model = FakeModel(model, action_logits["true"], values["true"]) + total_loss = super().loss(fake_model, dist_class, train_batch) + + # Extract mean of predicted action from action_logits. + # No need to compute the perform model forward pass since the original + # PPO loss is already doing it, so just getting back the last ouput. + action_true_logits = action_logits["true"] + action_true_mean = get_action_mean( + model, dist_class, action_true_logits) + + # Compute the mean action corresponding to the previous observation + if self.config["caps_temporal_reg"] > 0.0 or \ + self.config["temporal_barrier_reg"] > 0.0: + action_prev_logits = action_logits["prev"] + action_prev_mean = get_action_mean( + model, dist_class, action_prev_logits) + + # Compute the mean action corresponding to the noisy observation + if self.config["caps_global_reg"] > 0.0 or \ + not self.config["enable_adversarial_noise"]: + action_noisy_logits = action_logits["noisy"] + action_noisy_mean = get_action_mean( + model, dist_class, action_noisy_logits) + + # Compute the mean action corresponding to the worst observation + if self.config["caps_spatial_reg"] > 0.0 and \ + self.config["enable_adversarial_noise"]: + action_worst_logits = action_logits["worst"] + action_worst_mean = get_action_mean( + model, dist_class, action_worst_logits) + + # Compute the mirrored mean action corresponding to the mirrored action + if self.config["symmetric_policy_reg"] > 0.0: + assert self.action_mirror_mat is not None + action_mirror_logits = action_logits["mirrored"] + action_mirror_mean = get_action_mean( + model, dist_class, action_mirror_logits) + action_revert_mean = _compute_mirrored_value( + action_mirror_mean, + self.action_space, + self.action_mirror_mat) + + # Update total loss + stats = model.tower_stats + if self.config["caps_temporal_reg"] > 0.0 or \ + self.config["temporal_barrier_reg"] > 0.0: + # Compute action temporal delta + action_delta = (action_prev_mean - action_true_mean).abs() + + if self.config["caps_temporal_reg"] > 0.0: + # Minimize the difference between the successive action mean + caps_temporal_reg = torch.mean(action_delta) + + # Add temporal smoothness loss to total loss + stats["caps_temporal_reg"] = caps_temporal_reg + total_loss += \ + self.config["caps_temporal_reg"] * caps_temporal_reg + + if self.config["temporal_barrier_reg"] > 0.0: + # Add temporal barrier loss to total loss: + # exp(scale * (err - thr)) - 1.0 if err > thr else 0.0 + scale = self.config["temporal_barrier_scale"] + threshold = self.config["temporal_barrier_threshold"] + temporal_barrier_reg = torch.mean(torch.exp( + torch.clamp( + scale * (action_delta - threshold), min=0.0, max=5.0 + )) - 1.0) + + # Add spatial smoothness loss to total loss + stats["temporal_barrier_reg"] = temporal_barrier_reg + total_loss += \ + self.config["temporal_barrier_reg"] * temporal_barrier_reg + + if self.config["caps_spatial_reg"] > 0.0: + # Minimize the difference between the original action mean and the + # perturbed one. + if self.config["enable_adversarial_noise"]: + caps_spatial_reg = torch.mean(torch.sum( + (action_worst_mean - action_true_mean) ** 2, dim=1)) + else: + caps_spatial_reg = torch.mean(torch.sum( + (action_noisy_mean - action_true_mean) ** 2, dim=1)) + + # Add spatial smoothness loss to total loss + stats["caps_spatial_reg"] = caps_spatial_reg + total_loss += self.config["caps_spatial_reg"] * caps_spatial_reg + + if self.config["caps_global_reg"] > 0.0: + # Minimize the magnitude of action mean + caps_global_reg = torch.mean(action_noisy_mean ** 2) + + # Add global smoothness loss to total loss + stats["caps_global_reg"] = caps_global_reg + total_loss += self.config["caps_global_reg"] * caps_global_reg + + if self.config["symmetric_policy_reg"] > 0.0: + # Minimize the assymetry of self output + symmetric_policy_reg = torch.mean( + (action_revert_mean - action_true_mean) ** 2) + + # Add policy symmetry loss to total loss + stats["symmetric_policy_reg"] = symmetric_policy_reg + total_loss += \ + self.config["symmetric_policy_reg"] * symmetric_policy_reg + + if self.config["l2_reg"] > 0.0: + # Add actor l2-regularization loss + l2_reg = 0.0 + assert isinstance(model, torch.nn.Module) + for name, params in model.named_parameters(): + if not name.endswith("bias") and params.requires_grad: + l2_reg += l2_loss(params) + + # Add l2-regularization loss to total loss + stats["l2_reg"] = l2_reg + total_loss += self.config["l2_reg"] * l2_reg + + return total_loss + + def extra_grad_info(self, + train_batch: SampleBatch) -> Dict[str, TensorType]: + """Add regularization values to statistics. + """ + stats_dict = super().extra_grad_info(train_batch) + + if self.config["symmetric_policy_reg"] > 0.0: + stats_dict["symmetry"] = torch.mean( + torch.stack(self.get_tower_stats("symmetric_policy_reg"))) + if self.config["temporal_barrier_reg"] > 0.0: + stats_dict["temporal_barrier"] = torch.mean( + torch.stack(self.get_tower_stats("temporal_barrier_reg"))) + if self.config["caps_temporal_reg"] > 0.0: + stats_dict["temporal_smoothness"] = torch.mean( + torch.stack(self.get_tower_stats("caps_temporal_reg"))) + if self.config["caps_spatial_reg"] > 0.0: + stats_dict["spatial_smoothness"] = torch.mean( + torch.stack(self.get_tower_stats("caps_spatial_reg"))) + if self.config["caps_global_reg"] > 0.0: + stats_dict["global_smoothness"] = torch.mean( + torch.stack(self.get_tower_stats("caps_global_reg"))) + if self.config["l2_reg"] > 0.0: + stats_dict["l2_reg"] = torch.mean( + torch.stack(self.get_tower_stats("l2_reg"))) + + return convert_to_numpy(stats_dict) + + +class PPOTrainer(_PPOTrainer): + """Custom PPO Trainer with additional regularization losses on top of the + original surrogate loss. See `PPOTorchPolicy` for details. """ - if config["framework"] == "torch": - return PPOTorchPolicy - return None - + @classmethod + def get_default_config(cls) -> TrainerConfigDict: + """Returns a default configuration for the Trainer. + """ + return DEFAULT_CONFIG -PPOTrainer = PPOTrainer.with_updates( - default_config=DEFAULT_CONFIG, - get_policy_class=get_policy_class -) + def get_default_policy_class(self, + config: TrainerConfigDict) -> Type[Policy]: + if config["framework"] == "torch": + return PPOTorchPolicy + raise RuntimeError("The only framework supported is 'torch'.") __all__ = [ diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/utilities.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/utilities.py index aeb0c4d2d..b3e076181 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/utilities.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/utilities.py @@ -25,14 +25,20 @@ import ray.cloudpickle as pickle from ray import ray_constants from ray._private import services +from ray._private.gcs_utils import AvailableResources +from ray._private.test_utils import monitor_memory_usage from ray._raylet import GlobalStateAccessor from ray.exceptions import RayTaskError from ray.tune.logger import Logger, TBXLogger from ray.tune.utils.util import SafeFallbackEncoder from ray.rllib.policy import Policy -from ray.rllib.utils.filter import NoFilter +from ray.rllib.policy.torch_policy import TorchPolicy +from ray.rllib.policy.tf_policy import TFPolicy +from ray.rllib.utils.filter import NoFilter, MeanStdFilter from ray.rllib.agents.trainer import Trainer from ray.rllib.models.preprocessors import get_preprocessor +from ray.rllib.env.env_context import EnvContext +from ray.rllib.evaluation.worker_set import WorkerSet from jiminy_py.viewer import play_logs_files from gym_jiminy.common.utils import clip, DataNested @@ -129,7 +135,7 @@ def initialize(num_cpus: int, resources: Dict[str, int] = defaultdict(int) for info in global_state_accessor.get_all_available_resources(): # pylint: disable=no-member - message = ray.gcs_utils.AvailableResources.FromString(info) + message = AvailableResources.FromString(info) for field, capacity in message.resources_available.items(): resources[field] += capacity @@ -154,14 +160,12 @@ def initialize(num_cpus: int, num_cpus=num_cpus, # Number of GPUs assigned to each raylet num_gpus=num_gpus, - # Whether or not to execute the code serially (for debugging) - local_mode=debug, # Logging level logging_level=logging.DEBUG if debug else logging.ERROR, # Whether to redirect outputs from every worker to the driver log_to_driver=debug, **{**dict( # Whether to start Ray dashboard to monitor cluster status - include_dashboard=debug, + include_dashboard=False, # The host to bind the dashboard server to dashboard_host="0.0.0.0" ), **ray_init_kwargs}) @@ -181,6 +185,10 @@ def initialize(num_cpus: int, print(f"Started Tensorboard {url}.", f"Root directory: {log_root_path}") + # Monitor memory usage in debug + if debug: + monitor_memory_usage(print_interval_s=60) + # Define log filename interactively if requested if log_name == "": while True: @@ -228,6 +236,7 @@ def compute_action(policy: Policy, action. """ if policy.framework == 'torch': + assert isinstance(policy, TorchPolicy) input_dict = policy._lazy_tensor_dict(input_dict) with torch.no_grad(): policy.model.eval() @@ -249,6 +258,7 @@ def compute_action(policy: Policy, action_torch = action_dist.deterministic_sample() action = action_torch.cpu().numpy() elif tf.compat.v1.executing_eagerly(): + assert isinstance(policy, TFPolicy) action_logits, state = policy.model(input_dict) action_dist = policy.dist_class(action_logits, policy.model) if explore: @@ -261,10 +271,11 @@ def compute_action(policy: Policy, # placeholders to avoid creating new nodes to evalute computation # graph. It is several order of magnitude more efficient than calling # `action_logits, _ = model(input_dict).eval(session=policy._sess)`. + assert isinstance(policy, TFPolicy) feed_dict = {policy._input_dict[key]: value for key, value in input_dict.items() if key in policy._input_dict.keys()} - feed_dict[policy._is_exploring] = explore + feed_dict[policy._is_exploring] = np.array(True) action, *state = policy._sess.run( [policy._sampled_action] + policy._state_outputs, feed_dict=feed_dict) @@ -439,6 +450,7 @@ def train(train_agent: Trainer, the trained neural network model. """ # Get environment's reward threshold, if any + assert isinstance(train_agent.workers, WorkerSet) env_spec, *_ = [val for worker in train_agent.workers.foreach_worker( lambda worker: worker.foreach_env(lambda env: env.spec)) for val in worker] @@ -628,20 +640,23 @@ def test(test_agent: Trainer, """ # Instantiate the environment if not provided if test_env is None: - test_env = test_agent.env_creator( - {**test_agent.config["env_config"], **kwargs}) + test_env = test_agent.env_creator(EnvContext( + **test_agent.config["env_config"], **kwargs)) # Get policy model policy = test_agent.get_policy() # Get observation filter if any + assert isinstance(test_agent.workers, WorkerSet) obs_filter = test_agent.workers.local_worker().filters["default_policy"] if isinstance(obs_filter, NoFilter): obs_filter_fn = None - else: + elif isinstance(obs_filter, MeanStdFilter): obs_mean, obs_std = obs_filter.rs.mean, obs_filter.rs.std obs_filter_fn = \ lambda obs: (obs - obs_mean) / (obs_std + 1.0e-8) # noqa: E731 + else: + raise RuntimeError(f"Filter '{obs_filter.__class__}' not supported.") # Forward viewer keyword arguments if viewer_kwargs is not None: diff --git a/python/gym_jiminy/rllib/setup.py b/python/gym_jiminy/rllib/setup.py index c6704cdae..44132809e 100644 --- a/python/gym_jiminy/rllib/setup.py +++ b/python/gym_jiminy/rllib/setup.py @@ -30,7 +30,12 @@ packages=find_namespace_packages(), install_requires=[ f"gym_jiminy=={version}", - "ray[default,rllib]>=1.4.0", + # - <1.6.0: GPU detection must be patched to work + # - >=1.6.0: Cannot load checkpoints generated by Python < 3.8 using + # Python >= 3.8. + # - 1.9.0: Breaking changes + # - 1.10.0: Breaking changes + "ray[default,rllib]>=1.10.0", "plotext" ], zip_safe=False diff --git a/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py b/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py index 7214a8c2e..b96e037a5 100644 --- a/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py +++ b/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py @@ -1,5 +1,7 @@ """ TODO: Write documentation. """ +from typing import Optional + import numpy as np import numba as nb from numba.np.extensions import cross2d @@ -128,7 +130,7 @@ def __init__(self, points: np.ndarray) -> None: self._hull = None # Buffer to cache center computation - self._center = None + self._center: Optional[np.ndarray] = None @property def center(self) -> np.ndarray: diff --git a/python/gym_jiminy/unit_py/data/cassie_standing_panda3d_2.png b/python/gym_jiminy/unit_py/data/cassie_standing_panda3d_2.png index 7edc287fbca2a61e261a70a1fda2bcfed31ff527..2c91f8c097200d0faab97fd06cfe0ff10c078eb5 100644 GIT binary patch literal 6573 zcmds6i96J5`yXUULUD+iXt8Ej*|L<1hN#I>vP5oC?9Ib|#RHe^e(7Gf|coyIOC zS%za5nTcVBSsH%Ej|$LD?&~i06e)s6)SsLmC#aovjMuM*NRH!i}cbzmCc7 zdw$~SQ}bsCn$m2qTxO%;5Y#f~ahHABjKb&FtA^^yn!du7a;9Cadk?;dak*A-r6x5f z<-{Y`rjSqYqw6e?FA)#5^z{B$J}D+HU!oy;E#~lpIF|g9KUW~D?P#_=W!g1h{(u7u*>)SAL8N&2Uf0jZLoUhThVg} zO9?p4uog)4#Y0%!w)XKq&j7m>UbF5g6g(&{?k4hIVwgvnbwPGD*;%g(*Yg&-QL`LZTn%uI{6J{F63n*5g#qatk9F4Aqq3tQXC;oaaA- zOW>m%ddb+PJ&EzC9i5$ZEHPWc;BFFTEJ`=x$Hb0&>i>`_Fur%Lf^2J6ZgDv7^Wx%S zzF4hQIqjvH?1mSkxGrp&$XF7t*lP2x7(Di|K^nJ-Mm^HuSj;k|QUak2gA1uR8J?M$IocFV ze89%3(DdtL@<5cZ?dDU{&}r9s07d7!(y zdjb<;M|zhAZceymE$^!LHhr4`t2!$S?MLNn(ANhmF{Du{p@s2Dz_e*GKW2Vm;RcKl zPHxQVnb#(dCt036BN`X#v+*S(WT_eHbMtXyEjP6M~(eQ;Z0%wF>+D0q4>Nr8$hcw1I?`;JnmC;{qFT_s;rV)G>A> z;B3n+vU;3hKbnZWW0(*!uyF16CB)^3>8DaU)SuM?0s>ug_wT5qU&8FO*ESbq_dQJm z%X<6z7E)kwmrrzVwes#iFyKakK)doS^)Xd%Zn90Nk*m0qZKa z!B-2p5zV%hbyo6H``8`kP3(aO&^wPW`kaaiyu|EPaq-}VD*XOc+*}E+rGC5)CxTSl zr>m-}s-~8v%Uzgk8@l?}cGYA2ks2jag1=^u zb9KLKjaB*Ew{P`xn;abpUO(qu&U%fjtEz$kSxfaoYDZI(;U-_zZ%OkPJr}%OZc%C` zJ4weQ`g9qz@o&m@S9=a_H}{5Y-`=V$P9_m1b6qIWwr4!+#(;l-!UY8d zCPLg7sVv+D8P8u*e5mE$4&%|p@1XJtkte4dhvc-_`F2Q5 z(&p6^(X)v_qTg_ZjV^79?*>dt-Qy4v;0V$OLhY38>a(vmQYVJ4q5&|)!ti!pTPX=P z)Kx|jyn*`4PQ4S=LNAy!lnH>lv_09c@eU}9(n93;^=hQRim1=D0)Kw5_n86duXxbs zn-Z?>?9%BF*|a@ zFh~+H3JucU87P#^bTx!W#7*Q0&aY3q4tRwY6dD@p?k+3bCg*8?u7y4sG1D?0bR92% ze%-n%(Q`sLCt?ZT=;7xVqa%_GD{68A%R4L|$_rVprY9OxmMbhxy(rkJF-GL*(4eIJ!cI4Jty&z;yCTq zI9|EPf5V_T{1;XcN+S}9F)A8aXFXvus4&uqQB$Ho^pxa6Z`JLumg3k+W{wzBFI%n| zG#P7|86e8sy0mXo?-7gi$b3lIrLIxgT^sH7FO$)?SCq^b7C*9@1O z;6;1riX!CEOFBeP}cX`o=jlDHIzIjd|ZPf2Q)*T+sHc zl3KFESE@iO* zb9-%n4rTj76>stdI0Um1gEQ+m;EnY$mQ8`xJ@}QUh+=e)FfOUNMopar2q_(2MpxhU zniz9E9si2n`c`v&P_GfUJwa|JcUm^TL)=A3Bnw0zlakkGnRQ5^Eqwoelg#lZ+N8$v zT#nyYTfu29l!+~W)}$p;7^?}c$QjSm2y(+h36ggX{nohFswXB(2un7PUC`HRF*Pw! z7+tI0#DB7{?>MUR^YiD=AoDlYwe{{7ZA>CdOjDnA_x37e822fj*3{HIb0!78G~D&U zYA`&~p8Wl80?M~j#?Ai0A}DIe2xNPASkz1~VFI&dj_m2~o)zv1na4ya_v!j$^W#8)(HNV+Trosj=b%v9%bmBg z)p;~D5ZrgJpTf>HZ07|0C0{Di$9L{mYkRvP;o*1Z>Ws>1FI>Zhfi9)LPoYk8Cyyxo z6s<%n&{wF9lRBjDce}KSMq=U1W8nlvOOh-f?ZY1{fmEff^)#UYkaq(A$k2Z>IY}?l zHI&ib5+>-<21i#&&#=Vc1r)jcM1mZ75U&N(Iq0&#&9s&$>IIv}a6!Vh158Hv?QG^> z6Jsu7rJyNt1W!M&7;-VMRA;l+{D=xqYz@z)l!E(&ng0I%q1c59C@9CE$hy}q?a6y- zeN7_%a>3i_)$y^n7&F;9K9Gq zA-6tVlsg6O!~NZC-}dCMqSW34+>1(TnGdFF9ZRV0qHNa0DQ`LS!kqh(eR~G_hT(AN z;&Lv7t|q_ws5ySYLuY-XnIP~jI$JAm9dS-eOIl$_<NgO+Q;}+hu`Y-w3HuR{Q0uptm~Q?m=O9?OG-+@6I@(4F9GcwZjbZ${xO3d{ITxi zt#osDy3&)TL;HGppBY&_$ zaz|jJjY8VkN??hZW_`!Vd3ylN>mbk*;^IK%L}GF(%i?2W%bd?8#KwY7?sCBkKNT4X zraS16OJ|gol~q&{;i6FJN^h;DYR)N8hv6bnsAsL!*7Td|N7DdU*FkaFfA}n@pCEr* zRb@iwwUfK%b{b4fC+5Q)w=JUwpf5rAd~nG9O#Fb;)RLA{MLd&L`+))_J722%nSU|k z#?Di!*z$m7AUrbv&5(O>wl54OW5-@Te38^A12NX&$LZ8lUe4Q}|%-NE6w zq8&^ICoqfswF2niO^W?K$#v#N{7n$s-iv+CF9WL7!Mw<65Po)3)ONIgsl`E{cuQ%Fc`?nUV4YUro_39mY0@dI$xyD~GhqAWpa z%`B)J51Mo*4PISyEFC}0lbWr?p6c>V3gr(6P4n3-gQMNiVHYYIfTKY6dQIZcH>R>2 zzh&;v;B4_3eiT9Sz`j#EWD7=O>;X=qa$I0BN0cOUs0O%qSmNC$`&rSx43AuTlf%~XFuPYxyUoE zPd{L{elx*cyoG%|3f>NhpJaT@_9IAkQsMK=ru;5aho{&V2DM{nXjb&}_SzzF0V{za z{{A(JT1`J*WijhzTwL6SN3;^Z{%CkB);=E02_b}#$=k9iUiDK?#=KiFeuaH`2B9D4 z((H0{_urdrruS+Q&P@~TUCkd~=a$D#>Rg^ODZWuUM5$Fflza?@ZtG9a%~ee60-YO} zJHfl|+8vz%fk6ED)*DNV`($$%Xd_++a<#qjA0mXhfgF0O&T|j}stITv$zK<<_!!-4 zG%SN=sYd^N01?#hHfbcYGE(g_+@3}8`%{SvH1h93;3W_AqO`JlbTc>6lU8`QF)#Xz zGi+fydMrF>&|54#8uVF#FlN7kZ^~~m;9l}lt!{SW71b^oU$E(+UMLH zv^*C7!?1#eVgwrZMV~QmSgVUOjCA#n+QVfyqZAoweT|*VQ0AoGIp`3ees=g8CU?Y4 zT|2Cj6~{}icxAK3m)uHC7Dw`G#UO`Zr7^usmggTnhr{D)e$_uDkC_M{`;G`}E^DP! z_@BhFe7ydf4Qttd{_;g8Q~_HwB+@DJj>HCGLBHf?V*0mW<_i#dQ?9kp5(Sj_&6CC7 z^-&f6O-x~vW}MHK3LaEHdIa&J_qe3^yP31CJ6dOJ@yCjIz^t}eA3M(T3Z{4b@Ri+m zi@cB+pf1NGzzUrrA1WQt7Gi2%cFxmm!PKWBT}i1AKKcy!ItnSDSAOj= zlkUnqHd~8FURBWoSpHv>-U=s#6~4f`+@pcj{KBLQi=J1Jx?kQzFfZ*&SF7lK3uRYk zYUn`g+oek=Tgw3k(U8i*B=x4!)$OG@)RuQhNM7vL`STsF z?o8H{={4pVXn`G)?|zfg7mW&VBV2~PfV^bdeWi;(Dvw9I(21*6{K;R1PSSEGgi~F1 zxlDG4nvebO{@g1r&Qvzh&t-AZjJvdWPEk*fc_=d<9a%6|xO3I8ONomETBf|qS>CZZ zy$JN0@vh~+;GMpc^7!-w-c?LUk4SlQ`&konJ5uENhoa<^^tnTvie}}O`}~M%JzA`Y^Ce~ zjA{Ds7fPG%aV4`|m408}tvnD5my8XjaeUG{lx_Do-&7B;v`*1;o9$zf>i;?|9-AG4 zJuJ$PN^RK%+qK1-SU!o?RTxnF;~4)(KrOJd%AxlJ!Fi=(yG z6)?=#F1d34iDmNEOsh0W``$=WAo`|EA@L6L3Y%<#aaw@yt^tfp|IwF=JAzT7(JWOt k%n&R|i4FkDy$>d_ig*y-b14sUoApigX literal 6678 zcmds6i9eKU^dC!?Qs~~Kl4zBsY-O7=RKnb%J3@BK5ZQ{zI<$(2OekikED70-ZH%&J zFxeYp$WFs(Y%`Xb`8}h1?{EDJe(&e=e!TNO?>WzT&U2n~zUS-d6%)f9+xBgPKp;De zjV>S{5WXeeEzA$zTqrf7LLhso#uxOj-bF$<4 zM8#u^x9jP(szLb76YPz`eLkFn>YZ^1i?58_Dl0oC1mW*GAs`;d`^e-yzo7F5L{CpG zAUr(v$`;}G1HB?!#beu~Wo1u;S4))wzs7mu@_(KIfJ$i$ncw8_L-TY;Jknfj5 zuf5^nJ%fDwY73IGvag`Ov}Z{CE-Q-!BhO?Y!bX##;&HFRNEH&ouNDURHOapt-@z!) zDxNW==YaY{$Nq<$d6Bg20Zkgvlm@ii2X~VgMDU2Okg$>B5k0+=*ZGB8SbV?CxU=(* zU&9$Tzsbr{fjhL{Y!i>OFbWSxgX!;_llWyNZ)r1$e7^*r$;cKfvvYG1@O+D0+!4|u znM_7^!MOE?eI6x_6G+WL=D4-D!1kfDGK3I6zfzR(eC!Lnw+_YxM`70yg71}SidxwF z*W+Q=XrAE_s~nk)!30xXZB0$f+}_==l&L_tJh7N_6l0QYk&9>cam#{iwkndgnvWho z6byIs@R+?@5NrqL?scc?zJzDm7nquwGC8d+Ek-zsXPGs3G>2Io8EY`i(rXwOiKiMNTCWh+EmY657-}DYTUei8#{ZOncHZY4a zHyNm_u5KpCKqC^7)Wg{-fmo*d>D~|Z5n8|jk-^>}f5LMRIZH`B&xdDcXIZR@`UsWM zbH9H|;}CHxFk3=cD5`vD&i}dk=y33u&By9Fl$bxo@p47v)88p;J)wcv;k*5IbN<1f z%tYX-jrC(&8B8iG@p;(NyXBy>q{Yf;lvlWoNUr=kONP4){FxfA&Y3hlcI=q_y^%*} zNH+O>XPJZDGUQ<7E0gR5(SFL1GO_sj(Z&+IS}1&G;8sC?Kh4+Im)4h0sk(d1mqIF- zhHd-Jq#dKCJ=Iu{!C)932pn%yCn+T^8LogW+##j0bYMkj$-W~Dosi3>6S$n*f?A!- zQ)+525bhx~TuS?7gL#_u3ISqaT&7rMc3^#FPVwzx;t3mtm#yH_wIv-6Lm2}ttzxN|1L?bN>P)H|)nBs&XRbdQGyjP*(EWLPNI;1d35w8P3%(~!~0 z6mM*Das|DL7Iliyd9#!~sOKYSd+DXebra^rteu=jJH{yfMw-uUfJPHgtuI zF=_sE?rF=Qd`x#*m3L53l~FF3tMTow1cZB;b>kzWd<$A$Nr(HXeU0^7sBt0-%%R~E ziKkN91%yP~+S`|-SbFEnJ!ykUo>FUZsSj97KN^_ zT=%>5uJfiwO5@Y+L&o0VoQ+){nDb|N=vW}Ln6qtdVDvc0qMCa0Z6I(&nYC47dwaWt zvO@%Fcb;&59NM@u5sPrgQ4Cz{b`3|nLNO+pcZ@O_(`VK3oEGIUMs&ZeZ~M1z*GkB< zp+;vG+tlSVPuP_2k~#9rD|=viZkN)yBzM&6lkVxqj+eLw8ozCymC)Jz7l%v_)e-C+IVqPtRE4y zfu)f^oGTIc#a!GuEK(gXk4;StVGGeKmWR85c6HE~gz+D6>vdly zyQWb~-OMr(ffFw+dC84Nf9{~6qch;40gR?M7RjPrt@_-{nwp!VJ@D4~*q|Rd@|ig~ zIp{8sdj47j@fvgS#+^G0rQIeJCVn`#pGMMM_kg>BBbYMGnK9>Vt$T{_%@)+~9PsNj zkX(L@@*J#S00XFHw=6pksGmN4T35F=aB(t!o~0?D>Bpqv7vBbAB`-7Bw`l&=V`F2> zLHK)GIyym9m;g>Uv=)OHSZY8G0qOadNw#;stS!?B%lY1yO-!)99ojG96EoRqy4;P$ zG~XVTrDrmZ-#pnV$}X;{e}y=l`j8g3He>Jw*&Hj4pRh`tgZa7APZuiNyDAq;f&kIH z%kAI>%f>us_K|1&nwpw)FdY(>wv4Vnl{3*;AX-Aw4W1vuml0_1oxaHvukeHLJaOo5 zf$FjL=Sx3i6n-|~H8=3gVUBvhh5>wrE>)n_@sco=2ve?g!%-UL^b?Vdc5BDvI-(~p+?K#$}w!NBDo7CCa`Q$**^kwg$?$K-n9O^&&4MuRJR^2tM%E}og z4RRzK5BmH_2*9n4vI3_tNr-_;->H>3iWu&)+(tpTd6v@o)ta>*xieo=Y$g5Y2JQ|8 z!t6r0bkzEH0Zqc(aYkrJNQl3GIgTqyO=y(w!~S`)GQ++=^0F42m{u|Wc=2Pk=PFnH znzmZNW9og{i!}*pwI?;KU_uh&hX=P2-Bpbm`vj|HEX2phpCQQ|J2o)4@<}Q)>l75a zP9MIflKDWJRqMSm;09ZJCjtsrPO{+YW6c^TCY_oMe1DLn?X$Wyc0&$#);46ZE_*m}a=y$@B~Rvq)_JJ9dN;vmtSKf%(>sjOC@JYo zSJ|*_xUjnMT?nQ%fi~-Ub6L5uHzTE>s$Tb(s;YS7v?U*yR;v zr6E=6AiG?FCkL}9JFZYzmR!}Cm}TWk)@XDz$n5RjCvRGsx>@N5Om$$q!xmH4`>h7O z)<>h2={sKSZISwMx?^H)4(RK5ZlarIQA04lAs*oqD^kijaX?PFScn{Z(jsV!9 z3iSX*rXpwPKC5<|9q8rd1#0MX-L;$C&r{goj}>$`683CDLPF!UqHJuCySBGr?s?$q zTMkQ$%TyG4GFrvV%*^0+mN4VV(|O{zG|Ax-H)rRJfOhxzN>?lY4(%yWPWvP3+v-fFT?VASc`9#~x=aj{0T2~YOejWgHrNKZ>m z^-#Kt^5U|^0&1C1k==c?WtS4Gf_|I_xeJ;C3W{>o^jiVZ{f|=^0%*OR*P;h2j}!N6 zgbtOslAfo+T9_Ct_G7QRl!k|v&v(7sv|4l*fJRxkF!wlb0Y$xbjFaUvD;Tz^S*CK* zmq<<{tdtd;xYp%f7Ui%n>XTFO%B=Ra%-bMWDahXqUEMcXFu8?0s%0}kSa_9YyHFj% z_U%`)AHfmWb3;tNl! zjJf!(fB9P831YE(S@3k%F7HiikpqAIR9%F?1G|wj0_XdWnw@!~ub`Pr zLk_5)9EZU5(MJ!cX~Q-bLYjq-W=xvw--k>$#BIMl$R&DLuk?#L*jXJi6w1sbVH|&} zhCy|XD~<_8-nf4Kdg1zFKZ<6@{h8pfZ+xFq*>vI?bnR_g^={=eKHuw-V7|+amaJ?- z)z8us==;UnZ(q7}X}^|_V|evaouzPY(^hl1JZZ5}L@MTDzJ<-e$L4;-mWyOo6 zCv!mGc6APJ5_$bQwB+4`1w`VTr7HX)wpC39^crvgrdL@ftLpXD0{C}QoUni9%H!>= zE_ysUz`Sx)+*0+^833E?6Op&@TUGp?$)qoF%9Re^eiB%Pj2t84HpZc_<=#v-m3w-NcmIGEiE$M)f}e*TWWC|+aO z8_}caWVGEM?Tev(*7=D}&S*?L0d21Al$MeLz=55@omJ#4sc~C$@p@B? zl-Y|av+F$jI&bdU!W?Y_;L~nWIO33va^G)A*9{1=7y8n=rPw1VD5!9qw?AFmjQ0Zw z1ky%u_ffI^T5+Z45O+>qf13v~2j~I=T$baCRH6qLbG^^5ZBwrJejnI?BsVlPJl-J% z7MrdiVUY&XmU+GfrQ-G&fW+_|%3L{R?hNU9Elh?@Zk9pS5Zs5oaMIG~BygI|-*rw; z%z`Gdf2G#hDsj2$piuF-YvN5Y^8eI*CG1X^d4Swjz~Nxj@+9p z_p7nLZ8Z3%+2GQJQMAH-z*a;=&Z(GAnjCox-Oo!2?)0xW#+B>TcHB6WPQvJA&^uR^ zwZodku3T@NUEYMR`z|!%D`MOek%tc-My;&RqbSOQ(8zWr`Jy+2XryEQsziiDy3%>s z2PcS1Bg-nMoauRybn6WSs!j83NB#$k+;*2mmV?-NBSmM3+&pT8!(KOo)A|cwtb1ew zCV)twm_wl{QNg9y8kjYlWN-=n?Xo5njTpdr4^sn%@m|!biGo7=f^76h0%whc;w+X~ zd#)-zEK|!E6WY*eW89m3<=UY`%R_gSds1q-?D>G_%fZvC4p;INGM9>;ht9@A=~~$S zdY=z|W&y&4zP09aZo3%F`ywZg2OUbv>7KC#xSr~zNm`sJ*wObj&V}xCW`#~&ALy_3 z2b{u7OM5qs6k_j+!M*O@h)TV(?QwTqZ#T7Q74^mx{11>cmj@VxGss|qibh_RevPuvjM1T#u2 zC528-D-Yqk9pG*NhdA17y`>K@%t}%>v|~nBOJ-1vAm`vmBg|u@JhM%(I^+|YD7Pi< z0XK{!rz;z<1=G{hsLEv-%-5!k9yu;;EB=7R)fF?%fx~=nRe1!|gZjS-pZquo!|yI2 zvi@bgc#PFQeAwPp$M^o{BpmOZ&^{*xgQ%brB{4C8w`$pHzR5`Ct; zON&EzoHJeTsjAkBWe_IcZ z<|fq!Si?0V=G!ZPK?C%j+-b7iRi2fb{@pZ}=YQV%HW&UH`f$W?(%U?SE*J@p^cCyh z)nhMN{RVH=Cl-N4w(2`b$u<14(dp)wnrk|es(k;sSDQ>>8Z=L%d4Fyl@2k>tpIa03 zi+=pd84(iS-(S!F0yY%k`y?XnsH&|kkE!H2JEyXK>+QNLqF#M`;&E-#u}utpWAM&N zTkW_ea<3Lz;J!Fd2T1w}Db%!duycHse{3$^r?~K)FQRkX)?du!8c%!T!EM!QOMfLE zgg{l5GL+7{k$p#6crrPaW`|ZRwCw*25xH0A*vrogn|=eVYWfdrZ-|`smo0h1$i02{ zF`^d9Jij-I6)Mak3#&lpk%da;N$^P8O@5U3@Bu!^H`>XgYB1_c%ZckF?QM{3p64hTWr+L{_GtmK871cPaWZwYS6}c#g`r<4V^}f{1R$H!n;! zWAky None: # build from source after install `libatlas-base-dev` system # dependency. # 1.2.0 fixes `fmin_slsqp` optimizer returning wrong `imode` ouput. - "scipy>=1.2.0", + # 1.8.0: `scipy.spatial.qhull._Qhull` is no longer exposed. + "scipy>=1.2.0,<1.8.0", # Standard library to generate figures. "matplotlib", # Used internally to read HDF5 format log files. @@ -146,9 +147,9 @@ def finalize_options(self) -> None: # Check PEP8 conformance of Python native code "flake8", # Python linter - "pylint", + "pylint>=2.12.2", # Python static type checker - "mypy", + "mypy>=0.931", # Generate Python docs and render '.rst' nicely "sphinx", # 'Read the Docs' Sphinx docs style @@ -162,9 +163,10 @@ def finalize_options(self) -> None: # Bridge between doxygen and sphinx. Used to generate C++ API docs "breathe", # Repair wheels to embed shared libraries. - # Since 3.2.0, it is now possible to use custom patcher, and new - # manylinux_2_24 images are supported since 3.3.0 - "auditwheel>=3.3.0" + # - 3.2.0: enable defining custom patcher + # - 3.3.0: Support Python 3.9 and manylinux_2_24 images + # - 4.0.0: Many bug fixes, including RPATH of dependencies + "auditwheel>=4.0.0" ] }, zip_safe=False diff --git a/python/jiminy_py/src/jiminy_py/core/__init__.py b/python/jiminy_py/src/jiminy_py/core/__init__.py index 745e1f454..9eb9df90c 100644 --- a/python/jiminy_py/src/jiminy_py/core/__init__.py +++ b/python/jiminy_py/src/jiminy_py/core/__init__.py @@ -58,7 +58,7 @@ _module = _importlib.import_module(".".join((__name__, module_name))) _sys.modules[module_name] = _module -# Register pinocchio_pywrap to avoid importing bindings twise, which messes up +# Register pinocchio_pywrap to avoid importing bindings twice, which messes up # with boost python converters. In addition, submodules search path needs to be # fixed for releases older than 2.5.6. submodules = _inspect.getmembers( @@ -82,10 +82,6 @@ __all__.append(name) attrib.__module__ = __name__ -# Patch Pinocchio to avoid loading ground geometry in viewer, and force -# `np.ndarray` type for from/to Python matrix converters. -from . import _pinocchio_init # noqa - # Define helpers to build extension modules def get_cmake_module_path(): diff --git a/python/jiminy_py/src/jiminy_py/core/_pinocchio_init.py b/python/jiminy_py/src/jiminy_py/core/_pinocchio_init.py deleted file mode 100644 index 5c144e1a8..000000000 --- a/python/jiminy_py/src/jiminy_py/core/_pinocchio_init.py +++ /dev/null @@ -1,19 +0,0 @@ -import pinocchio as pin - - -# Use numpy array by default for Eigenpy and Pinocchio incidentally -__import__('eigenpy').switchToNumpyArray() - - -# Do not load the geometry of the ground is is not an actually geometry but -# rather a calculus artifact. -loadPrimitive_orig = \ - pin.visualize.gepetto_visualizer.GepettoVisualizer.loadPrimitive -def loadPrimitive(self, meshName, geometry_object): # noqa - if geometry_object.name == "ground": - return False - else: - return loadPrimitive_orig(self, meshName, geometry_object) - -pin.visualize.gepetto_visualizer.GepettoVisualizer.loadPrimitive = \ - loadPrimitive # noqa diff --git a/python/jiminy_py/src/jiminy_py/robot.py b/python/jiminy_py/src/jiminy_py/robot.py index 47b2f9f31..3729753ec 100644 --- a/python/jiminy_py/src/jiminy_py/robot.py +++ b/python/jiminy_py/src/jiminy_py/robot.py @@ -562,16 +562,16 @@ def load_hardware_description_file( elif body_name not in geometry_types['visual']['mesh']: logger.warning( "No visual mesh nor collision geometry associated with " - "the collision body. Fallback to adding a single contact " - "point at body frame.") + f"collision body '{body_name}'. Fallback to adding a single " + "contact point at body frame.") contact_frames_names.append(body_name) continue else: logger.warning( "No collision geometry associated with the collision " - "body. Fallback to replacing it by contact points at " - "the vertices of the minimal volume bounding box of the " - "available visual meshes.") + f"body '{body_name}'. Fallback to replacing it by contact " + "points at the vertices of the minimal volume bounding box of " + "the available visual meshes.") # Check if collision primitive box are available collision_boxes_size, collision_boxes_origin = [], [] diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index a3590804c..1e28babc5 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/python/jiminy_py/src/jiminy_py/simulator.py @@ -465,10 +465,6 @@ def render(self, """Render the current state of the simulation. One can display it or return an RGB array instead. - .. note:: - Gepetto-gui supports parallel rendering, which means that one can - display multiple simulations at the same time in different tabs. - :param return_rgb_array: Whether or not to return the current frame as an rgb array. :param width: Width of the returned RGB frame, if enabled. diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/index.html b/python/jiminy_py/src/jiminy_py/viewer/meshcat/index.html index 33d7279ac..5db761ce6 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/index.html +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/index.html @@ -34,10 +34,13 @@ } }; - // Connect the viewer to the existing server, using the - // usual websocket on desktop, though kernel communication - // in Google Colaboratory or Jupyter notebooks. + // Connect the viewer to the existing server, though direct ZMP + // websocket in standalone browser or though kernel communication + // in notebooks. try { + // Set externally by python in interactive mode + var ws_path = undefined; + if (typeof google !== 'undefined') { (async () => { viewer.connection = await google.colab.kernel.comms.open("meshcat", "meshcat:open"); @@ -58,20 +61,119 @@ viewer.handle_command_bytearray(new Uint8Array(message.buffers[0].buffer)); }); viewer.connection.on_close(function(message) { - viewer.connection = null; // The connection is no longer available console.log("connection to Jupyter kernel closed:", message); + viewer.connection = null; // The connection is no longer available }); } - else { - var ws_url = undefined; - viewer.connect(ws_url); + else if (ws_path !== undefined) { + // Connect to kernel socket manually if necessary, namely for + // VSCode notebooks and jupyterlab. + const ws_url = "ws" + window.parent.location.origin.substring(4) + ws_path; + viewer.connection = new window.WebSocket(ws_url); + + // Define UUID generation utility to identify the comm and messages + function uuid() { + return ([1e7]+-1e3+-4e3+-8e3+-1e11).replace(/[018]/g, c => + (c ^ crypto.getRandomValues(new Uint8Array(1))[0] & 15 >> c / 4).toString(16) + ); + }; + + // Define message deserialization method + var deserialize_array_buffer = function (buf) { + var data = new DataView(buf); + var nbufs = data.getUint32(0); + var offsets = []; + var i; + for (i = 1; i <= nbufs; i++) { + offsets.push(data.getUint32(i * 4)); + } + var json_bytes = new Uint8Array(buf.slice(offsets[0], offsets[1])); + var msg = JSON.parse( + (new TextDecoder('utf8')).decode(json_bytes) + ); + msg.buffers = []; + var start, stop; + for (i = 1; i < nbufs; i++) { + start = offsets[i]; + stop = offsets[i+1] || buf.byteLength; + msg.buffers.push(new DataView(buf.slice(start, stop))); + } + return msg; + }; + + // Create unique comm identifier + const comm_id = uuid(); + + // Monkey-patch send command + var send = viewer.connection.send; + viewer.connection.send = function(data, msg_type) { + var msg = { + header : { + date: new Date().toISOString(), + username : "meshcat", + msg_id : uuid(), + session : "000000000000", + version : "5.3", + msg_type : msg_type || "comm_msg" + }, + metadata : {}, + content : { + comm_id : comm_id, + target_name : "meshcat", + data : data, + }, + channel : 'shell', + buffers : [], + parent_header : {} + }; + send.call(this, JSON.stringify(msg)); + }; + + // Monkey-patch close command + var close = viewer.connection.close; + viewer.connection.close = function() { + // For some reason, `onclose` is never called, and + // calling the original `close` interferes with the + // send method and the message is never received. + viewer.connection.send("meshcat:close", "comm_close"); + }; + + // Define event handler + viewer.connection.onopen = function(event) { + console.log("connection to generic ipykernel:", viewer.connection); + viewer.connection.send("meshcat:open", "comm_open"); + }; + viewer.connection.onmessage = async function (event) { + var data = event.data; + if (data instanceof Blob) { + const reader = new FileReader(); + reader.addEventListener('loadend', () => { + var p = Promise.resolve(deserialize_array_buffer(reader.result)); + p.then(function(msg) { + if (msg.content.comm_id === comm_id) + { + viewer.handle_command_bytearray(new Uint8Array(msg.buffers[0].buffer)); + } + }); + }); + reader.readAsArrayBuffer(event.data); + } + }; + viewer.connection.onclose = function (message) { + console.log("connection to generic ipykernel closed:", message); + viewer.connection = null; // The connection is no longer available + }; + } + else + { + // Fallback to direct local communication through meshcat ZMQ socket + viewer.connect(); } } catch (e) { - console.info("Not connected to MeshCat server: ", e); + console.info("not connected to MeshCat server: ", e); } - // Replace the mesh grid by a filled checkerboard, similar to - // the one of Gepetto-gui. The paving size is 1m by 1m. + // Replace the mesh grid by tiles. The paving size is 1m by 1m. var segments = 20; var cmap = [new MeshCat.THREE.Color(0x222233), new MeshCat.THREE.Color(0xf2f2fe)]; var geometry = new MeshCat.THREE.PlaneBufferGeometry( @@ -96,8 +198,7 @@ viewer.scene_tree.find(["Axes", ""]).object.material.linewidth = 2.5 - // Update the "zoom" of the camera to match the behavior of - // Gepetto-gui, and update the default position of the camera. + // Update the "zoom" of the camera and its default position viewer.camera.fov = 30; viewer.camera.position.set(8.0, 1.2, 0); viewer.render(); @@ -143,7 +244,7 @@ // available. if (viewer.connection !== null) { - console.log("Closing connection..."); + console.log("closing connection..."); viewer.connection.close(); } return false; diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py index e714cefe9..5aad3d66c 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py @@ -153,6 +153,7 @@ def handle_comm(self, frames: Sequence[bytes]) -> None: self.send_scene(comm_id=comm_id) self.comm_pool.add(comm_id) if self.is_waiting_ready_msg: + # Send request for acknowledgment a-posteriori msg = umsgpack.packb({"type": "ready"}) self.forward_to_comm(comm_id, msg) elif cmd.startswith("close:"): diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py index 163e8e62e..8258d2b41 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py @@ -7,7 +7,6 @@ import pathlib import umsgpack import threading -import tornado.gen import tornado.ioloop from contextlib import redirect_stdout, redirect_stderr from typing import Optional, Sequence, Dict, Any @@ -113,7 +112,7 @@ def __call__(self, unsafe: bool = False) -> None: # New message: reading message without deserializing its # content at this point for efficiency. _, msg = self.__kernel.session.feed_identities( - args[1], copy=False) + args[-1], copy=False) msg = self.__kernel.session.deserialize( msg, content=False, copy=False) else: @@ -122,20 +121,29 @@ def __call__(self, unsafe: bool = False) -> None: if msg is not None and \ msg['header']['msg_type'].startswith('comm_'): + # Extract comm type and handler + comm_type = msg['header']['msg_type'] + comm_handler = getattr( + self.__kernel.comm_manager, comm_type) + + # Extract message content + content = self.__kernel.session.unpack(msg['content']) + data = content.get('data', '') + # Comm message. Analyzing message content to determine if # it is related to meshcat or not. - if msg['header']['msg_type'] == 'comm_close': + if comm_type == 'comm_close': # All comm_close messages are processed because Google # Colab API does not support sending data on close. - data = "meshcat:close" - else: - content = self.__kernel.session.unpack(msg['content']) - data = content.get('data', '') + msg['content'] = content + comm_handler(None, None, msg) + continue if isinstance(data, str) and data.startswith('meshcat:'): # Comm message related to meshcat. Processing it right # now and moving to the next message without puting it # back into the queue. - tornado.gen.maybe_future(dispatch(*args)) + msg['content'] = content + comm_handler(None, None, msg) continue # The message is not related to meshcat comm, so putting it @@ -192,7 +200,7 @@ def forward_comm_thread(): self.__comm_socket = context.socket(zmq.XREQ) self.__comm_socket.connect(comm_url) self.__comm_stream = ZMQStream(self.__comm_socket, self.__ioloop) - self.__comm_stream.on_recv(self.__forward_to_ipython) + self.__comm_stream.on_recv(self.__forward_to_ipykernel) self.__ioloop.start() self.__ioloop.close() self.__ioloop = None @@ -213,25 +221,25 @@ def __del__(self) -> None: def close(self) -> None: self.n_comm = 0 self.n_message = 0 - self.__kernel.comm_manager.unregister_target( - 'meshcat', self.__comm_register) + if 'meshcat' in self.__kernel.comm_manager.targets: + self.__kernel.comm_manager.unregister_target( + 'meshcat', self.__comm_register) self.__comm_stream.close(linger=5) self.__comm_socket.close(linger=5) self.__ioloop.add_callback(lambda: self.__ioloop.stop()) self.__thread.join() self.__thread = None - def __forward_to_ipython(self, frames: Sequence[bytes]) -> None: + def __forward_to_ipykernel(self, frames: Sequence[bytes]) -> None: comm_id, cmd = frames # There must be always two parts each messages comm_id = comm_id.decode() try: comm = self.__kernel.comm_manager.comms[comm_id] + comm.send(buffers=[cmd]) except KeyError: # The comm has probably been closed without the server knowing. # Sending the notification to the server to consider it as such. self.__comm_socket.send(f"close:{comm_id}".encode()) - else: - comm.send(buffers=[cmd]) def __comm_register(self, comm: 'ipykernel.comm.Comm', # noqa @@ -240,7 +248,7 @@ def __comm_register(self, # mechanism: if the main thread is already busy for some reason, for # instance waiting for a reply from the server ZMQ socket, then # `comm.on_msg` will NOT triggered automatically. It is only triggered - # automatically once every other tacks has been process. The workaround + # automatically once every other tasks has been process. The workaround # is to interleave blocking code with call of `kernel.do_one_iteration` # or `await kernel.process_one(wait=True)`. See Stackoverflow for ref: # https://stackoverflow.com/a/63666477/4820605 diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py index ef9d0ae3f..e7db5b8a6 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py @@ -25,7 +25,7 @@ TransparencyAttrib, OrthographicLens, ClockObject, Shader, ShaderAttrib, AntialiasAttrib, GraphicsPipe, WindowProperties, FrameBufferProperties, CollisionNode, CollisionRay, CollisionTraverser, CollisionHandlerQueue, - loadPrcFileData) + GraphicsOutput, loadPrcFileData) from direct.showbase.ShowBase import ShowBase from direct.gui.OnscreenImage import OnscreenImage from direct.gui.OnscreenText import OnscreenText @@ -36,6 +36,7 @@ import panda3d_viewer.viewer_proxy from panda3d_viewer import geometry from panda3d_viewer.viewer_errors import ViewerClosedError +from panda3d_viewer.viewer_config import ViewerConfig import hppfcl import pinocchio as pin @@ -259,8 +260,10 @@ def make_heightmap(heightmap: np.ndarray) -> Geom: class Panda3dApp(panda3d_viewer.viewer_app.ViewerApp): - def __init__(self, config) -> None: + def __init__(self, config: Optional[ViewerConfig] = None) -> None: # Enforce viewer configuration + if config is None: + config = ViewerConfig() config.set_window_size(*WINDOW_SIZE_DEFAULT) config.set_window_fixed(False) config.enable_antialiasing(True, multisamples=2) @@ -294,8 +297,11 @@ def keyboardInterruptHandler(signalNumber, stackFrame): self.taskMgr.keyboardInterruptHandler = keyboardInterruptHandler + # Disable the task manager loop for now. Only useful if onscreen. + self.shutdown() + # Active enhanced rendering if dedicated NVIDIA GPU is used. - # Note that shadow resolution larger than 1024 significatly affects + # Note that shadow resolution larger than 1024 significantly affects # the frame rate on Intel GPU chipsets: going from 1024 to 2048 makes # it drop from 60FPS to 30FPS. if self.win.gsg.driver_vendor.startswith('NVIDIA'): @@ -447,6 +453,9 @@ def keyboardInterruptHandler(signalNumber, stackFrame): self.show_grid(False) self.show_floor(True) + # Force rendering the scene to finalize initialization of the GSG + self.graphics_engine.render_frame() + def has_gui(self) -> bool: return any(isinstance(win, GraphicsWindow) for win in self.winList) @@ -504,6 +513,9 @@ def open_window(self) -> None: "Impossible to open graphical window. Make sure display is " "available on system.") + # Enable the task manager + self.restart() + def _open_offscreen_window(self, size: Optional[Tuple[int, int]] = None) -> None: """Create new completely independent offscreen buffer, rendering the @@ -521,7 +533,7 @@ def _open_offscreen_window(self, self.close_window(self.buff, keepCamera=False) # Set offscreen buffer frame properties - # Note that accumalator bits and back buffers is not supported by + # Note that accumulator bits and back buffers is not supported by # resizeable buffers. fbprops = FrameBufferProperties(self.win.getFbProperties()) fbprops.set_accum_bits(0) @@ -531,19 +543,21 @@ def _open_offscreen_window(self, winprops = WindowProperties() winprops.set_size(*size) - # Set offscreen buffer flags to enforce resizeable `GaphicsBuffer` + # Set offscreen buffer flags to enforce resizeable `GraphicsBuffer` flags = GraphicsPipe.BF_refuse_window | GraphicsPipe.BF_refuse_parasite flags |= GraphicsPipe.BF_resizeable - # Create new offscreen buffer. + # Create new offscreen buffer and attach a texture # Note that it is impossible to create resizeable buffer without an # already existing host for some reason... win = self.graphicsEngine.make_output( self.pipe, "offscreen_buffer", 0, fbprops, winprops, flags, self.win.get_gsg(), self.win) + texture = Texture() + win.addRenderTexture(texture, GraphicsOutput.RTMCopyRam) + self.buff = win # Append buffer to the list of windows managed by the ShowBase - self.buff = win self.winList.append(win) # Create 3D camera region for the scene. @@ -558,7 +572,7 @@ def _open_offscreen_window(self, self.offscreen_display_region.set_sort(5) self.offscreen_display_region.set_camera(self.offscreen_camera_2d) - # # Adjust aspect ratio + # Adjust aspect ratio self._adjust_offscreen_window_aspect_ratio() def _adjust_offscreen_window_aspect_ratio(self) -> None: @@ -602,7 +616,7 @@ def getSize(self, win: Optional[Any] = None) -> Tuple[int, int]: win = self.buff return super().getSize(win) - def getMousePos(self) -> Tuple[int, int]: + def getMousePos(self) -> Tuple[float, float]: """Get current mouse position. .. note:: @@ -671,7 +685,8 @@ def click_on_node(self) -> None: if self.picked_object is not None: self.highlight_node(*self.picked_object, True) - def move_orbital_camera_task(self, task: Any) -> None: + def move_orbital_camera_task(self, + task: Optional[Any] = None) -> Optional[int]: """Custom control of the camera to be more suitable for robotic application than the default one. """ @@ -736,7 +751,8 @@ def move_orbital_camera_task(self, task: Any) -> None: self.last_mouse_y = y # End task - return task.cont + if task is not None: + return task.cont def _make_light_ambient(self, color: Tuple3FType) -> NodePath: """Patched to fix wrong color alpha. @@ -1300,7 +1316,6 @@ def set_camera_lookat(self, def set_window_size(self, width: int, height: int) -> None: self.buff.setSize(width, height) self._adjust_offscreen_window_aspect_ratio() - self.step() # Update frame on-the-spot def set_framerate(self, framerate: Optional[float] = None) -> None: """Limit framerate of Panda3d to avoid consuming too much ressources. @@ -1326,6 +1341,10 @@ def save_screenshot(self, filename: Optional[str] = None) -> bool: template = 'screenshot-%Y-%m-%d-%H-%M-%S.png' filename = datetime.now().strftime(template) + # Refresh the scene to make sure it is perfectly up-to-date. + # It will take into account the updated position of the camera. + self.graphics_engine.render_frame() + # Capture frame as image image = PNMImage() if not self.buff.get_screenshot(image): @@ -1353,9 +1372,16 @@ def get_screenshot(self, framerate, as any other method relaying on low-level panda3d task scheduler. The framerate limit must be disable manually to avoid such limitation. + + :param requested_format: Desired export format (e.g. 'RGBA' or 'RGB') + :param raw: whether to return a raw memory view of bytes, of a + structured `np.ndarray` of uint8 with dimensions [W, H, D]. """ - # Capture frame as raw texture - texture = self.buff.get_screenshot() + # Refresh the scene + self.graphics_engine.render_frame() + + # Get frame as raw texture + texture = self.buff.get_texture() # Extract raw array buffer from texture image = texture.get_ram_image_as(requested_format) @@ -1439,34 +1465,12 @@ def append_frame(self, *args: Any, **kwargs: Any) -> None: def append_cylinder(self, *args: Any, **kwargs: Any) -> None: self._app.append_cylinder(*args, **kwargs) - def set_watermark(self, *args: Any, **kwargs: Any) -> None: - self._app.set_watermark(*args, **kwargs) - self._app.step() # Add watermark widget on-the-spot - - def set_legend(self, *args: Any, **kwargs: Any) -> None: - self._app.set_legend(*args, **kwargs) - self._app.step() # Add legend widget on-the-spot - - def set_clock(self, *args: Any, **kwargs: Any) -> None: - self._app.set_clock(*args, **kwargs) - self._app.step() # Add clock widget on-the-spot - - def set_window_size(self, *args: Any, **kwargs: Any) -> None: - self._app.set_window_size(*args, **kwargs) - self._app.step() # Update window size on-the-spot - - def save_screenshot(self, *args: Any, **kwargs: Any) -> bool: - # Refresh the scene if no graphical window is available - if not self._app.has_gui(): - self._app.step() - return self._app.save_screenshot(*args, **kwargs) + def save_screenshot(self, filename: Optional[str] = None): + return self._app.save_screenshot(filename) def get_screenshot(self, requested_format: str = 'RGBA', raw: bool = False) -> Union[np.ndarray, bytes]: - # Refresh the scene if no graphical window is available - if not self._app.has_gui(): - self._app.step() return self._app.get_screenshot(requested_format, raw) panda3d_viewer.viewer.Viewer = Panda3dViewer # noqa @@ -1480,7 +1484,7 @@ class Panda3dVisualizer(BaseVisualizer): Copyright (c) 2018-2020, INRIA """ # noqa: E501 def initViewer(self, - viewer: Optional[Panda3dViewer] = None, + viewer: Optional[Union[Panda3dViewer, Panda3dApp]] = None, loadModel: bool = False) -> None: """Init the viewer by attaching to / creating a GUI viewer. """ @@ -1579,7 +1583,7 @@ def loadViewerGeometryObject(self, geom_node = GeomNode('convex') geom_node.add_geom(obj) node = NodePath(geom_node) - self.viewer._app.append_node(*node_name, node) + self.viewer.append_node(*node_name, node) else: is_success = False else: diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_widget.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_widget.py index 81ff08843..bb018d6e1 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_widget.py +++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_widget.py @@ -3,16 +3,14 @@ from typing import Optional, Tuple, Any from matplotlib.backends.qt_compat import QtCore, QtWidgets, QtGui -from .panda3d_visualizer import Panda3dViewer - -FRAMERATE = 30 +from .panda3d_visualizer import Panda3dApp -Qt = QtCore.Qt +FRAMERATE = 30 -class Panda3dQWidget(Panda3dViewer, QtWidgets.QWidget): +class Panda3dQWidget(Panda3dApp, QtWidgets.QWidget): """An interactive panda3D QWidget. """ def __init__(self, parent: Optional[Any] = None) -> None: @@ -22,16 +20,13 @@ def __init__(self, parent: Optional[Any] = None) -> None: QtWidgets.QWidget.__init__(self, parent=parent) # Initialize Panda3D app - Panda3dViewer.__init__(self, window_type='offscreen') + Panda3dApp.__init__(self) # Only accept focus by clicking on widget - self.setFocusPolicy(Qt.ClickFocus) + self.setFocusPolicy(QtCore.Qt.ClickFocus) - # Enable mouse control + # Configure mouse control self.setMouseTracking(True) - self._app.getMousePos = self.getMousePos - self._app.taskMgr.add( - self._app.move_orbital_camera_task, "move_camera_task", sort=2) # Create painter to render "screenshot" from panda3d self.paint_surface = QtGui.QPainter() @@ -42,23 +37,27 @@ def __init__(self, parent: Optional[Any] = None) -> None: self.clock.timeout.connect(self.update) self.clock.start() - def destroy(self): - super(Panda3dViewer, self).destroy() - super(QtWidgets.QWidget, self).destroy() + def destroy(self) -> None: + Panda3dApp.destroy(self) + QtWidgets.QWidget.destroy(self) def close(self) -> bool: - super(Panda3dViewer, self).destroy() - return super(QtWidgets.QWidget, self).close() + Panda3dApp.destroy(self) + return QtWidgets.QWidget.close(self) def paintEvent(self, event: Any) -> None: """Pull the contents of the panda texture to the widget. """ + # Updating the pose of the camera + self.move_orbital_camera_task() + # Get raw image and convert it to Qt format. - # Note that `QImage` apparently does not manage the lifetime of the - # input data buffer, so it is necessary to keep it is local scope. + # Note that `QImage` does not manage the lifetime of the input data + # buffer, so it is necessary to keep it is local scope until the end of + # its drawning. data = self.get_screenshot('RGBA', raw=True) img = QtGui.QImage(data, - *self._app.buff.getSize(), + *self.buff.getSize(), QtGui.QImage.Format_RGBA8888).mirrored() # Render image on Qt widget @@ -72,7 +71,7 @@ def resizeEvent(self, event: Any) -> None: self.set_window_size( event.size().width(), event.size().height()) - def getMousePos(self) -> Tuple[int, int]: + def getMousePos(self) -> Tuple[float, float]: """ TODO: Write documentation. """ pos = self.mapFromGlobal(QtGui.QCursor().pos()) @@ -81,22 +80,22 @@ def getMousePos(self) -> Tuple[int, int]: def mousePressEvent(self, event: Any) -> None: """ TODO: Write documentation. """ - self._app.handle_key("mouse1", event.buttons() & Qt.LeftButton) - self._app.handle_key("mouse2", event.buttons() & Qt.MiddleButton) - self._app.handle_key("mouse3", event.buttons() & Qt.RightButton) + self.handle_key("mouse1", event.buttons() & QtCore.Qt.LeftButton) + self.handle_key("mouse2", event.buttons() & QtCore.Qt.MiddleButton) + self.handle_key("mouse3", event.buttons() & QtCore.Qt.RightButton) def mouseReleaseEvent(self, event: Any) -> None: """ TODO: Write documentation. """ - self._app.handle_key("mouse1", event.buttons() & Qt.LeftButton) - self._app.handle_key("mouse2", event.buttons() & Qt.MiddleButton) - self._app.handle_key("mouse3", event.buttons() & Qt.RightButton) + self.handle_key("mouse1", event.buttons() & QtCore.Qt.LeftButton) + self.handle_key("mouse2", event.buttons() & QtCore.Qt.MiddleButton) + self.handle_key("mouse3", event.buttons() & QtCore.Qt.RightButton) def wheelEvent(self, event: Any) -> None: """ TODO: Write documentation. """ delta = event.angleDelta().y() if delta > 0.0: - self._app.handle_key("wheelup", True) + self.handle_key("wheelup", True) else: - self._app.handle_key("wheeldown", True) + self.handle_key("wheeldown", True) diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py index ca851d7f7..bbadd5bb8 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/replay.py +++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py @@ -79,8 +79,8 @@ def play_trajectories(trajs_data: Union[ """Replay one or several robot trajectories in a viewer. The ratio between the replay and the simulation time is kept constant to - the desired ratio. One can choose between several backend (gepetto-gui or - meshcat). + the desired ratio. One can choose between several backend ('panda3d' or + 'meshcat'). .. note:: Replay speed is independent of the platform (windows, linux...) and @@ -172,9 +172,9 @@ def play_trajectories(trajs_data: Union[ input before starting to play the trajectories. Only available if `record_video_path` is None. Optional: False by default. - :param backend: Backend, one of 'panda3d', 'meshcat', or 'gepetto-gui'. If - `None`, the most appropriate backend will be selected - automatically, based on hardware and python environment. + :param backend: Backend, one of 'panda3d', 'meshcat'. If `None`, the most + appropriate backend will be selected automatically, based + on hardware and python environment. Optional: `None` by default. :param delete_robot_on_close: Whether or not to delete the robot from the viewer when closing it. @@ -425,8 +425,8 @@ def play_trajectories(trajs_data: Union[ # Disable framerate limit of Panda3d for efficiency if Viewer.backend.startswith('panda3d'): - framerate = viewer._backend_obj._app.get_framerate() - viewer._backend_obj._app.set_framerate(None) + framerate = viewer._backend_obj.get_framerate() + viewer._backend_obj.set_framerate(None) # Initialize video recording if Viewer.backend == 'meshcat': @@ -497,7 +497,7 @@ def play_trajectories(trajs_data: Union[ # Restore framerate limit of Panda3d if Viewer.backend.startswith('panda3d'): - viewer._backend_obj._app.set_framerate(framerate) + viewer._backend_obj.set_framerate(framerate) else: # Play trajectories with multithreading def replay_thread(viewer, *args): @@ -690,7 +690,7 @@ def _play_logs_files_entrypoint() -> None: "assuming the robot has a freeflyer.")) parser.add_argument( '-b', '--backend', default='panda3d', - help="Display backend (panda3d, meshcat, or gepetto-gui).") + help="Display backend ('panda3d' or 'meshcat').") parser.add_argument( '-m', '--mesh_package_dir', default=None, help="Fullpath location of mesh package directory.") @@ -713,10 +713,11 @@ def _play_logs_files_entrypoint() -> None: repeat = True viewers = None while repeat: - viewers = play_logs_files(**{**dict( - remove_widgets_overlay=False, - viewers=viewers), - **kwargs}) + viewers = play_logs_files( + viewers=viewers, + **{**dict( + remove_widgets_overlay=False), + **kwargs}) kwargs["start_paused"] = False if not hasattr(kwargs, "camera_xyzrpy"): kwargs["camera_xyzrpy"] = None diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 5414f6429..9329a40f3 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -13,9 +13,7 @@ import subprocess import webbrowser import multiprocessing -import xml.etree.ElementTree as ET from copy import deepcopy -from urllib.parse import urlparse from urllib.request import urlopen from functools import wraps, partial from bisect import bisect_right @@ -33,15 +31,13 @@ from panda3d_viewer.viewer_errors import ViewerClosedError import pinocchio as pin -from pinocchio import SE3, SE3ToXYZQUAT, SE3ToXYZQUATtuple +from pinocchio import SE3, SE3ToXYZQUAT from pinocchio.rpy import rpyToMatrix, matrixToRpy -from pinocchio.visualize import GepettoVisualizer from .. import core as jiminy from ..core import (ContactSensor as contact, discretize_heightmap) from ..state import State -from ..dynamics import XYZQuatToXYZRPY from .meshcat.utilities import interactive_mode from .meshcat.wrapper import MeshcatWrapper from .meshcat.meshcat_visualizer import MeshcatVisualizer @@ -52,7 +48,7 @@ Panda3dVisualizer) -DISPLAY_FRAMERATE = 30 +REPLAY_FRAMERATE = 40 CAMERA_INV_TRANSFORM_PANDA3D = rpyToMatrix(-np.pi/2, 0.0, 0.0) CAMERA_INV_TRANSFORM_MESHCAT = rpyToMatrix(-np.pi/2, 0.0, 0.0) @@ -95,11 +91,6 @@ def filter(self, record): # Determine set the of available backends backends_available = {'meshcat': MeshcatVisualizer, 'panda3d': Panda3dVisualizer} -if sys.platform.startswith('linux'): - import importlib - if (importlib.util.find_spec("gepetto") is not None and - importlib.util.find_spec("omniORB") is not None): - backends_available['gepetto-gui'] = GepettoVisualizer try: from .panda3d.panda3d_widget import Panda3dQWidget backends_available['panda3d-qt'] = Panda3dVisualizer @@ -131,12 +122,6 @@ def _get_backend_exceptions( """ if backend is None: backend = default_backend() - if backend == 'gepetto-gui': - import gepetto - import omniORB - return (omniORB.CORBA.COMM_FAILURE, - omniORB.CORBA.TRANSIENT, - gepetto.corbaserver.gepetto.Error) elif backend.startswith('panda3d'): return (ViewerClosedError,) else: @@ -154,8 +139,13 @@ def sleep(dt: float) -> None: :param dt: Sleep duration in seconds. """ + # A new high-precision cross-platform sleep method is now available + if sys.version_info >= (3, 11): + time.sleep(dt) + return + # Estimate of timer jitter depending on the operating system - if os.name == 'nt': + if sys.platform.startswith('win'): timer_jitter = 1e-2 else: timer_jitter = 1e-3 @@ -334,7 +324,7 @@ def __init__(self, `None` to use the unique lock of the current thread. Optional: `None` by default. :param backend: Name of the rendering backend to use. It can be either - 'panda3d', 'panda3d-qt', 'meshcat' or 'gepetto-gui'. + 'panda3d', 'panda3d-qt', 'meshcat'. None to keep using to one already running if any, or the default one otherwise. Note that the default is hardware and environment dependent. @@ -351,8 +341,7 @@ def __init__(self, Optional: False by default. :param robot_name: Unique robot name, to identify each robot. Optional: Randomly generated identifier by default. - :param scene_name: Scene name, used only with 'gepetto-gui' backend. It - must differ from the scene name. + :param scene_name: Scene name. Optional: 'world' by default. :param display_com: Whether or not to display the center of mass. Optional: Disabled by default. @@ -428,10 +417,7 @@ def __init__(self, else: backend = backend.lower() # Make sure backend's name is lowercase if backend not in backends_available: - if backend.startswith('gepetto'): - backend = 'gepetto-gui' - else: - raise ValueError("%s backend not available." % backend) + raise ValueError("%s backend not available." % backend) # Update the backend currently running, if any if Viewer.backend != backend and Viewer.is_alive(): @@ -447,11 +433,6 @@ def __init__(self, is_backend_running = True if not Viewer.is_open(): is_backend_running = False - if Viewer.backend == 'gepetto-gui': - try: - Viewer._backend_obj.gui.refresh() - except Viewer._backend_exceptions: - is_backend_running = False if not is_backend_running: Viewer._backend_obj = None Viewer._backend_proc = None @@ -600,18 +581,6 @@ def _setup(self, # Backup desired color self.robot_color = get_color_code(robot_color) - # Generate colorized URDF file if using gepetto-gui backend, since it - # is not supported by default, because of memory optimizations. - self.urdf_path = os.path.realpath(robot.urdf_path) - if Viewer.backend == 'gepetto-gui': - if self.robot_color is not None: - assert len(self.robot_color) == 4 - alpha = self.robot_color[3] - self.urdf_path = Viewer._get_colorized_urdf( - robot, self.robot_color[:3], self._tempdir) - else: - alpha = 1.0 - # Extract the right Pinocchio model if self.use_theoretical_model: pinocchio_model = robot.pinocchio_model_th @@ -632,36 +601,12 @@ def _setup(self, self._client.data = pinocchio_data self._client.collision_data = robot.collision_data - # Create the scene and load robot - if Viewer.backend == 'gepetto-gui': - # Initialize the viewer - self._client.initViewer(viewer=Viewer._backend_obj, - windowName=Viewer.window_name, - sceneName=self.scene_name, - loadModel=False) - - # Add missing scene elements - self._gui.addFloor('/'.join((self.scene_name, "floor"))) - self._gui.addLandmark(self.scene_name, 0.1) + # Initialize the viewer + self._client.initViewer(viewer=self._gui, loadModel=False) - # Load the robot - self._client.loadViewerModel(rootNodeName=self.robot_name) - - # Set robot transparency - try: - self._gui.setFloatProperty(robot_node_path, 'Alpha', alpha) - except Viewer._backend_exceptions: - # Old Gepetto versions do no have 'Alpha' attribute but - # 'Transparency'. - self._gui.setFloatProperty( - robot_node_path, 'Transparency', 1 - alpha) - else: - # Initialize the viewer - self._client.initViewer(viewer=self._gui, loadModel=False) - - # Load the robot - self._client.loadViewerModel( - rootNodeName=robot_node_path, color=self.robot_color) + # Create the scene and load robot + self._client.loadViewerModel( + rootNodeName=robot_node_path, color=self.robot_color) if Viewer.backend.startswith('panda3d'): # Add markers' display groups @@ -810,10 +755,6 @@ def get_force_scale(joint_idx: int) -> Tuple[float, float, float]: def open_gui(start_if_needed: bool = False) -> bool: """Open a new viewer graphical interface. - .. note:: - This method does nothing when using Gepetto-gui backend because - its lifetime is tied to the graphical interface. - .. note:: Only one graphical interface can be opened locally for efficiency. """ @@ -825,7 +766,7 @@ def open_gui(start_if_needed: bool = False) -> bool: if Viewer.has_gui(): return - if Viewer.backend in ['gepetto-gui', 'panda3d-qt']: + if Viewer.backend == 'panda3d-qt': # No instance is considered manager of the unique window pass elif Viewer.backend == 'panda3d': @@ -851,10 +792,26 @@ def open_gui(start_if_needed: bool = False) -> bool: # Provide websocket URL as fallback if needed. It would be # the case if the environment is not jupyter-notebook nor - # colab but rather japyterlab or vscode for instance. - web_url = f"ws://{urlparse(viewer_url).netloc}" + # colab but rather jupyterlab or vscode for instance. + from IPython import get_ipython + from notebook import notebookapp + kernel = get_ipython().kernel + conn_file = kernel.config['IPKernelApp']['connection_file'] + kernel_id = conn_file.split('-', 1)[1].split('.')[0] + server_pid = psutil.Process(os.getpid()).parent().pid + server_list = list(notebookapp.list_running_servers()) + try: + from jupyter_server import serverapp + server_list += list(serverapp.list_running_servers()) + except ImportError: + pass + for server_info in server_list: + if server_info['pid'] == server_pid: + break + ws_path = (f"{server_info['base_url']}api/kernels/{kernel_id}" + f"/channels?token={server_info['token']}") html_content = html_content.replace( - "var ws_url = undefined;", f'var ws_url = "{web_url}";') + "var ws_path = undefined;", f'var ws_path = "{ws_path}";') if interactive_mode() == 1: # Embed HTML in iframe on Jupyter, since it is not @@ -905,8 +862,8 @@ def has_gui() -> bool: comm_manager = Viewer._backend_obj.comm_manager if comm_manager is not None: ack = Viewer._backend_obj.wait(require_client=False) - Viewer._has_gui = any([ - msg == "meshcat:ok" for msg in ack.split(",")]) + Viewer._has_gui = any( + msg == "ok" for msg in ack.split(",")) return Viewer._has_gui return False @@ -916,10 +873,6 @@ def has_gui() -> bool: def wait(require_client: bool = False) -> None: """Wait for all the meshes to finish loading in every clients. - .. note:: - It is a non-op for `gepetto-gui` since it works in synchronous - mode. - :param require_client: Wait for at least one client to be available before checking for mesh loading. """ @@ -972,14 +925,10 @@ def close(self=None) -> None: Viewer.detach_camera() Viewer.remove_camera_motion() if Viewer.is_alive(): - if Viewer.backend in ('meshcat', 'panda3d-qt'): + if Viewer.backend in 'meshcat': Viewer._backend_obj.close() - if Viewer.backend == 'meshcat': - recorder_proc = Viewer._backend_obj.recorder.proc - _ProcessWrapper(recorder_proc).kill() - if Viewer.backend == 'panda3d': - Viewer._backend_obj._app.stop() - Viewer._backend_proc.wait(timeout=0.1) + elif Viewer.backend.startswith('panda3d'): + Viewer._backend_obj.stop() Viewer._backend_proc.kill() atexit.unregister(Viewer.close) Viewer._backend_obj = None @@ -1027,106 +976,13 @@ def close(self=None) -> None: # At this point, consider the viewer has been closed, no matter what self.__is_open = False - @staticmethod - def _get_colorized_urdf(robot: jiminy.Model, - rgb: Tuple3FType, - output_root_path: Optional[str] = None) -> str: - """Generate a unique colorized URDF for a given robot model. - - .. note:: - Multiple identical URDF model of different colors can be loaded in - Gepetto-viewer this way. - - :param robot: jiminy.Model already initialized for the desired URDF. - :param rgb: RGB code defining the color of the model. It is the same - for each link. - :param output_root_path: Root directory of the colorized URDF data. - Optional: temporary directory by default. - - :returns: Full path of the colorized URDF file. - """ - # Get the URDF path and mesh directory search paths if any - urdf_path = robot.urdf_path - mesh_package_dirs = robot.mesh_package_dirs - - # Make sure the robot is associated with an existing URDF - if not urdf_path: - raise RuntimeError( - "Impossible to call this method if the robot is not " - "associated with any URDF.") - - # Define color tag and string representation - color_tag = " ".join(map(str, list(rgb) + [1.0])) - color_str = "_".join(map(str, list(rgb) + [1.0])) - - # Create the output directory - if output_root_path is None: - output_root_path = tempfile.mkdtemp() - colorized_data_dir = os.path.join( - output_root_path, f"colorized_urdf_rgb_{color_str}") - os.makedirs(colorized_data_dir, exist_ok=True) - colorized_urdf_path = os.path.join( - colorized_data_dir, os.path.basename(urdf_path)) - - # Parse the URDF file - tree = ET.parse(robot.urdf_path) - root = tree.getroot() - - # Update mesh fullpath and material color for every visual - for visual in root.iterfind('./link/visual'): - # Get mesh full path - for geom in visual.iterfind('geometry'): - # Get mesh path if any, otherwise skip the geometry - mesh_descr = geom.find('mesh') - if mesh_descr is None: - continue - mesh_fullpath = mesh_descr.get('filename') - - # Make sure mesh path is fully qualified and exists - mesh_realpath = None - if mesh_fullpath.startswith('package://'): - for mesh_dir in mesh_package_dirs: - mesh_searchpath = os.path.join( - mesh_dir, mesh_fullpath[10:]) - if os.path.exists(mesh_searchpath): - mesh_realpath = mesh_searchpath - break - else: - mesh_realpath = mesh_fullpath - assert mesh_realpath is not None, ( - f"Invalid mesh path '{mesh_fullpath}'.") - - # Copy original meshes to temporary directory - colorized_mesh_fullpath = os.path.join( - colorized_data_dir, mesh_realpath[1:]) - colorized_mesh_path = os.path.dirname(colorized_mesh_fullpath) - if not os.access(colorized_mesh_path, os.F_OK): - os.makedirs(colorized_mesh_path) - shutil.copy2(mesh_realpath, colorized_mesh_fullpath) - - # Update mesh fullpath - geom.find('mesh').set('filename', mesh_realpath) - - # Override color tag, remove existing one, if any - material = visual.find('material') - if material is not None: - visual.remove(material) - material = ET.SubElement(visual, 'material', name='') - ET.SubElement(material, 'color', rgba=color_tag) - - # Write on disk the generated URDF file - tree = ET.ElementTree(root) - tree.write(colorized_urdf_path) - - return colorized_urdf_path - @staticmethod @__with_lock def __connect_backend(start_if_needed: bool = False, open_gui: Optional[bool] = None, close_at_exit: bool = True, timeout: int = 2000) -> None: - """Get a pointer to the running process of Gepetto-Viewer. + """Get the running process of backend client. This method can be used to open a new process if necessary. @@ -1139,63 +995,9 @@ def __connect_backend(start_if_needed: bool = False, :param close_at_exit: Terminate backend server at Python exit. Optional: True by default - :returns: Pointer to the running Gepetto-viewer Client and its PID. + :returns: Pointer to the running backend Client and its PID. """ - if Viewer.backend == 'gepetto-gui': - from gepetto.corbaserver.client import Client as gepetto_client - - if open_gui is not None and not open_gui: - logger.warning( - "This option is not available for Gepetto-gui.") - open_gui = False - - def _gepetto_client_connect(get_proc_info=False): - nonlocal close_at_exit - - # Get the existing Gepetto client - client = gepetto_client() - - # Try to fetch the list of scenes to make sure that the Gepetto - # client is responding. - client.gui.getSceneList() - - # Get the associated process information if requested - if not get_proc_info: - return client - proc = [p for p in psutil.process_iter() - if p.cmdline() and 'gepetto-gui' in p.cmdline()[0]][0] - return client, _ProcessWrapper(proc, close_at_exit) - - try: - client, proc = _gepetto_client_connect(get_proc_info=True) - except Viewer._backend_exceptions: - try: - client, proc = _gepetto_client_connect(get_proc_info=True) - except Viewer._backend_exceptions: - if start_if_needed: - FNULL = open(os.devnull, 'w') - proc = subprocess.Popen( - ['gepetto-gui'], shell=False, stdout=FNULL, - stderr=FNULL) - proc = _ProcessWrapper(proc, close_at_exit) - # Must try at least twice for robustness - is_connected = False - for _ in range(max(2, int(timeout / 200))): - time.sleep(0.2) - try: - client = _gepetto_client_connect() - is_connected = True - continue - except Viewer._backend_exceptions: - pass - if not is_connected: - raise RuntimeError( - "Impossible to open Gepetto-viewer.") - else: - raise RuntimeError( - "No backend server to connect to but " - "'start_if_needed' is set to False") - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): # handle default argument(s) if open_gui is None: open_gui = True @@ -1210,20 +1012,21 @@ def _gepetto_client_connect(get_proc_info=False): # Note that it fallbacks to software rendering if necessary. if Viewer.backend == 'panda3d-qt': client = Panda3dQWidget() + proc = _ProcessWrapper(client, close_at_exit) else: client = Panda3dViewer(window_type='onscreen', window_title=Viewer.window_name) - client.gui = client # The gui is the client itself for now + proc = _ProcessWrapper(client._app, close_at_exit) - # Extract low-level process to monitor health - proc = _ProcessWrapper(client._app, close_at_exit) + # The gui is the client itself + client.gui = client else: # handle default argument(s) if open_gui is None: open_gui = True # List of connections likely to correspond to Meshcat servers - meshcat_candidate_conn = [] + meshcat_candidate_conn = {} for pid in psutil.pids(): try: proc = psutil.Process(pid) @@ -1234,7 +1037,7 @@ def _gepetto_client_connect(get_proc_info=False): cmdline = proc.cmdline() if cmdline and ('python' in cmdline[0].lower() or 'meshcat' in cmdline[-1]): - meshcat_candidate_conn.append(conn) + meshcat_candidate_conn[pid] = conn except (psutil.AccessDenied, psutil.ZombieProcess, psutil.NoSuchProcess): @@ -1254,7 +1057,7 @@ def _gepetto_client_connect(get_proc_info=False): # Use the first port responding to zmq request, if any zmq_url = None context = zmq.Context.instance() - for conn in meshcat_candidate_conn: + for pid, conn in meshcat_candidate_conn.items(): try: # Note that the timeout must be long enough to give enough # time to the server to respond, but not to long to avoid @@ -1264,7 +1067,7 @@ def _gepetto_client_connect(get_proc_info=False): continue zmq_url = f"tcp://127.0.0.1:{port}" zmq_socket = context.socket(zmq.REQ) - zmq_socket.RCVTIMEO = 250 # millisecond + zmq_socket.RCVTIMEO = 200 # millisecond zmq_socket.connect(zmq_url) zmq_socket.send(b"url") response = zmq_socket.recv().decode("utf-8") @@ -1285,7 +1088,7 @@ def _gepetto_client_connect(get_proc_info=False): # Create a meshcat server if needed and connect to it client = MeshcatWrapper(zmq_url) if client.server_proc is None: - proc = psutil.Process(conn.pid) + proc = psutil.Process(pid) else: proc = client.server_proc proc = _ProcessWrapper(proc, close_at_exit) @@ -1310,7 +1113,7 @@ def _gepetto_client_connect(get_proc_info=False): @__must_be_open @__with_lock def _delete_nodes_viewer(nodes_path: Sequence[str]) -> None: - """Delete a 'node' in Gepetto-viewer. + """Delete an object or a group of objects in the scene. .. note:: Be careful, one must specify the full path of a node, including all @@ -1319,10 +1122,7 @@ def _delete_nodes_viewer(nodes_path: Sequence[str]) -> None: :param nodes_path: Full path of the node to delete """ - if Viewer.backend == 'gepetto-gui': - for node_path in nodes_path: - Viewer._backend_obj.gui.deleteNode(node_path, True) - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): for node_path in nodes_path: try: Viewer._backend_obj.gui.remove_group(node_path) @@ -1342,10 +1142,7 @@ def set_watermark(img_fullpath: Optional[str] = None, .. note:: The relative width and height cannot exceed 20% of the visible - area, othewise it will be rescaled. - - .. note:: - Gepetto-gui is not supported by this method and will never be. + area, otherwise it will be rescaled. :param img_fullpath: Full path of the image to use as watermark. Meshcat supports format '.png', '.jpeg' or 'svg', @@ -1359,10 +1156,7 @@ def set_watermark(img_fullpath: Optional[str] = None, image manually. Optional: None by default. """ - if Viewer.backend == 'gepetto-gui': - logger.warning( - "Adding watermark is not available for Gepetto-gui.") - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): Viewer._backend_obj.gui.set_watermark(img_fullpath, width, height) else: width = width or DEFAULT_WATERMARK_MAXSIZE[0] @@ -1380,9 +1174,6 @@ def set_legend(labels: Optional[Sequence[str]] = None) -> None: Make sure to have specified different colors for each robot on the scene, since it will be used as marker on the legend. - .. note:: - Gepetto-gui is not supported by this method and will never be. - :param labels: Sequence of strings whose length must be consistent with the number of robots on the scene. None to disable. Optional: None by default. @@ -1391,9 +1182,7 @@ def set_legend(labels: Optional[Sequence[str]] = None) -> None: if labels is not None: assert len(labels) == len(Viewer._backend_robot_colors) - if Viewer.backend == 'gepetto-gui': - logger.warning("Adding legend is not available for Gepetto-gui.") - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): if labels is None: items = None else: @@ -1443,11 +1232,7 @@ def get_camera_transform(self) -> Tuple[Tuple3FType, Tuple3FType]: this method is valid as long as the user does not move the camera manually using mouse camera control. """ - if Viewer.backend == 'gepetto-gui': - xyzquat = self._gui.getCameraTransform(self._client.windowID) - xyzrpy = XYZQuatToXYZRPY(xyzquat) - xyz, rpy = xyzrpy[:3], xyzrpy[3:] - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): xyz, quat = self._gui.get_camera_transform() rot = pin.Quaternion(*quat).matrix() rpy = matrixToRpy(rot @ CAMERA_INV_TRANSFORM_PANDA3D.T) @@ -1522,11 +1307,7 @@ def set_camera_transform(self, return self.set_camera_transform(position, rotation) # Perform the desired transformation - if Viewer.backend == 'gepetto-gui': - H_abs = SE3(rotation_mat, position) - self._gui.setCameraTransform( - self._client.windowID, SE3ToXYZQUAT(H_abs).tolist()) - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): rotation_panda3d = pin.Quaternion( rotation_mat @ CAMERA_INV_TRANSFORM_PANDA3D).coeffs() self._gui.set_camera_transform(position, rotation_panda3d) @@ -1783,34 +1564,15 @@ def capture_frame(self, raw_data: bool = False) -> Union[np.ndarray, bytes]: """Take a snapshot and return associated data. - :param width: Width for the image in pixels (not available with - Gepetto-gui for now). None to keep unchanged. + :param width: Width for the image in pixels. None to keep unchanged. Optional: Kept unchanged by default. - :param height: Height for the image in pixels (not available with - Gepetto-gui for now). None to keep unchanged. + :param height: Height for the image in pixels. None to keep unchanged. Optional: Kept unchanged by default. :param raw_data: Whether to return a 2D numpy array, or the raw output from the backend (the actual type may vary). """ # Check user arguments - if Viewer.backend == 'gepetto-gui' and ( - width is not None or height is not None): - logger.warning( - "Specifying window size is not available for Gepetto-gui.") - - if raw_data: - raise NotImplementedError( - "Raw data mode is not available for Gepetto-gui.") - - if Viewer.backend == 'gepetto-gui': - # It is not possible to capture frame directly using gepetto-gui, - # and it is not able to save the frame if the file does not have - # ".png" extension. - with tempfile.NamedTemporaryFile(suffix=".png") as f: - self.save_frame(f.name) - img_obj = Image.open(f.name) - rgba_array = np.array(img_obj) - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): # Resize window if size has changed _width, _height = self._gui.getSize() if width is None: @@ -1857,17 +1619,13 @@ def save_frame(self, """Save a snapshot in png format. :param image_path: Fullpath of the image (.png extension is mandatory) - :param width: Width for the image in pixels (not available with - Gepetto-gui for now). None to keep unchanged. + :param width: Width for the image in pixels. None to keep unchanged. Optional: Kept unchanged by default. - :param height: Height for the image in pixels (not available with - Gepetto-gui for now). None to keep unchanged. + :param height: Height for the image in pixels. None to keep unchanged. Optional: Kept unchanged by default. """ image_path = str(pathlib.Path(image_path).with_suffix('.png')) - if Viewer.backend == 'gepetto-gui': - self._gui.captureFrame(self._client.windowID, image_path) - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): _width, _height = self._gui.getSize() if width is None: width = _width @@ -2225,15 +1983,7 @@ def refresh(self, self._client.model, self._client.data, model, data) # Render new geometries placements - if Viewer.backend == 'gepetto-gui': - for geom_model, geom_data, model_type in zip( - model_list, data_list, model_type_list): - self._gui.applyConfigurations( - [self._client.getViewerNodeName(geom, model_type) - for geom in geom_model.geometryObjects], - [SE3ToXYZQUATtuple(geom_data.oMg[i]) - for i in range(len(geom_model.geometryObjects))]) - elif Viewer.backend.startswith('panda3d'): + if Viewer.backend.startswith('panda3d'): for geom_model, geom_data, model_type in zip( model_list, data_list, model_type_list): pose_dict = {} @@ -2292,10 +2042,6 @@ def refresh(self, self._gui.set_materials(self._markers_group, material_dict) self._gui.set_scales(self._markers_group, scale_dict) - # Refreshing viewer backend manually if necessary - if Viewer.backend == 'gepetto-gui': - self._gui.refresh() - # Wait for the backend viewer to finish rendering if requested if wait: Viewer.wait(require_client=False) @@ -2442,7 +2188,7 @@ def replay(self, self.display(q, v, xyz_offset, update_hook_t, wait) # Sleep for a while if computing faster than display framerate - sleep(1.0 / DISPLAY_FRAMERATE - (time.time() - time_prev)) + sleep(1.0 / REPLAY_FRAMERATE - (time.time() - time_prev)) # Update time in simulation, taking into account speed ratio time_prev = time.time() diff --git a/python/jiminy_pywrap/include/jiminy/python/Utilities.h b/python/jiminy_pywrap/include/jiminy/python/Utilities.h index e5299c45a..12ab76389 100644 --- a/python/jiminy_pywrap/include/jiminy/python/Utilities.h +++ b/python/jiminy_pywrap/include/jiminy/python/Utilities.h @@ -398,7 +398,7 @@ namespace python { bp::dict configPyDict; AppendBoostVariantToPython visitor(copy); - for (auto const & pair : config) // Structure binding is not supported by gcc<7.3 + for (auto const & pair : config) // Structured binding is not supported by gcc<7.3 { auto const & key = std::get<0>(pair); auto const & value = std::get<1>(pair); configPyDict[key] = boost::apply_visitor(visitor, value); diff --git a/python/jiminy_pywrap/src/Helpers.cc b/python/jiminy_pywrap/src/Helpers.cc index 9074ae283..31ce55786 100644 --- a/python/jiminy_pywrap/src/Helpers.cc +++ b/python/jiminy_pywrap/src/Helpers.cc @@ -8,8 +8,6 @@ #include "jiminy/core/utilities/Json.h" #include "jiminy/core/utilities/Random.h" -#include "pinocchio/algorithm/model.hpp" - #include /* Note that it is necessary to import eigenpy to get access to the converters. @@ -119,13 +117,19 @@ namespace python auto jointsToLock = convertFromPython >(jointsToLockPy); pinocchio::Model reducedModel; pinocchio::GeometryModel reducedCollisionModel, reducedVisualModel; - pinocchio::buildReducedModel(model, collisionModel, jointsToLock, - referenceConfiguration, reducedModel, - reducedCollisionModel); + buildReducedModel(model, + collisionModel, + jointsToLock, + referenceConfiguration, + reducedModel, + reducedCollisionModel); reducedModel = pinocchio::Model(); - pinocchio::buildReducedModel(model, visualModel, jointsToLock, - referenceConfiguration, reducedModel, - reducedVisualModel); + buildReducedModel(model, + visualModel, + jointsToLock, + referenceConfiguration, + reducedModel, + reducedVisualModel); return bp::make_tuple(reducedModel, reducedCollisionModel, reducedVisualModel); } diff --git a/unit/CMakeLists.txt b/unit/CMakeLists.txt index 207e75fd7..cbfc275ea 100755 --- a/unit/CMakeLists.txt +++ b/unit/CMakeLists.txt @@ -15,8 +15,13 @@ set(UNIT_TEST_FILES # Create the unit test executable add_executable(${PROJECT_NAME} ${UNIT_TEST_FILES}) +# Add tests with CTest +gtest_discover_tests(${PROJECT_NAME}) + # Add definition of unit test data folder -add_definitions("-DUNIT_TEST_DATA_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/data/\"") +target_compile_definitions(${PROJECT_NAME} PUBLIC + "-DUNIT_TEST_DATA_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/data/\"" +) # Link with Jiminy core library target_link_libraries(${PROJECT_NAME} ${LIBRARY_NAME}_core) @@ -33,5 +38,3 @@ target_link_libraries(${PROJECT_NAME} "${CMAKE_THREAD_LIBS_INIT}") set_property(TARGET ${PROJECT_NAME} PROPERTY MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>DLL" ) -include(CTest) -enable_testing()