From 60f60ec941eb00cb77d936e13445e91152961035 Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Fri, 19 Apr 2024 19:02:59 +0200 Subject: [PATCH] Jiminy release 1.8.4 (#769) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * [core] Fix bias/noise check for EncoderSensor. (#744) * [core] Fix some symbols not exported by mistake. (#742) * [core] Catch exception during stepper integration. (#747) * [core] Clarify terminology: 'Rigid' by 'Mechanical', 'Original' by 'Theoretical', 'Actual' by 'Extended'. (#753) (#762) (#763) * [core] More appropriate tolerance when checking GCD to avoid false positive. (#753) * [core] User is now systematically responsible for serializing constants before telemetry registration. (#753) * [core] Various cleanup and minor improvements. (#753) * [core] Add 'JIMINY' namespace to macros. (#755) * [core] Add methods to get / set all simulation options at once. (#760) * [core] More robust set/get options logics. (#761) * [core] Add backlash support at motor-level. (#767) * [core|python/simulator] Cleanup get/set options for multi-robot simulations. (#758) * [core/python] Faster 'array_copyto'. (#740) * [core/python] Do not export bindings. (#742) * [core/python] Expose more convenience getters. (#748) * [core/python] Add method to query heightmaps on multiple positions at once. (#750) * [python/viewer] Fix arrow maker for negative length with panda3d backend. * [python/viewer] Fix arrow marker for 'anchor_top=True' with panda3d. * [python/viewer] Fix viewer automatic closing during repley not working. (#756) * [python/simulator] First-class multi-robot support. (#754) (#766) * [python/plot] Add multi-robot support. (#764) * [gym/common] Fix corrupted robot proxy if model changes. (#746) * [gym/common] Introduce new first-class quantity manager. (#749) (#751) (#752) (#756) * [gym/common] Initialize sensor measurements at init. (#750) * [gym/common] Plot state, action and features of all pipeline blocks. (#750) * [gym/common] PD controller now satisfies position, velocity and acceleration bounds. (#750) (#765) (#768) * [gym/common] Allows PD control with position action. (#750) * [gym/common] Remove PD target dead band. (#750) * [gym/common] Decouple PD controller action order adapter from acceleration integration. (#764) * [gym/common] Refactor 'compute_command' to operate in-place. (#764) * [gym/rllib] Migrate from ray[rllib]==2.5.0 to 2.9.*. (#739) * [misc] Fix documentation formating. * [misc] Add YAPF formatting configuration (opt-in). (#739) * [misc] More robust build scripts. (#739) * [misc] Allow failure when installing gym_jiminy add-on modules. (#739) * [misc] Run all unit tests for gym jiminy base modules on Python 3.12 and Debug. (#739) * [misc] Support Numpy 2.0.0b1. (#740) * [misc] Upgrade C++ dependencies. (#741) * [misc] Fix compilation on Windows ARM64. (#746) * [misc] Fix numpy version requirement. (#750) * [misc] Update documentation. (#750) --------- Co-authored-by: Alexis Duburcq Co-authored-by: Grégoire Roussel Co-authored-by: Amandine --- .github/workflows/linux.yml | 76 +- .github/workflows/macos.yml | 48 +- .github/workflows/manylinux.yml | 7 +- .github/workflows/ubuntu.yml | 33 +- .github/workflows/win.yml | 48 +- .gitignore | 1 + .style.yapf | 22 + CMakeLists.txt | 18 +- INSTALL.md | 6 +- build_tools/build_install_deps_unix.sh | 83 +- build_tools/build_install_deps_windows.ps1 | 155 +-- build_tools/cmake/base.cmake | 17 +- build_tools/cmake/setupPython.cmake | 36 +- build_tools/easy_install_deps_ubuntu.sh | 6 +- .../patch_deps_unix/boost-python.patch | 19 + .../patch_deps_windows/boost-python.patch | 19 + build_tools/patch_deps_windows/eigenpy.patch | 4 +- build_tools/patch_deps_windows/hppfcl.patch | 8 +- .../patch_deps_windows/pinocchio.patch | 6 +- build_tools/stub_gen.py | 6 +- .../double_pendulum/double_pendulum.cc | 1 + .../external_project/double_pendulum.cc | 33 +- .../core/constraints/abstract_constraint.h | 6 +- .../core/constraints/distance_constraint.h | 9 +- .../core/constraints/frame_constraint.h | 9 +- .../core/constraints/joint_constraint.h | 9 +- .../core/constraints/sphere_constraint.h | 9 +- .../core/constraints/wheel_constraint.h | 9 +- .../jiminy/core/control/abstract_controller.h | 26 +- .../core/control/abstract_controller.hxx | 33 +- .../jiminy/core/control/controller_functor.h | 2 +- .../core/control/controller_functor.hxx | 6 +- core/include/jiminy/core/engine/engine.h | 29 +- core/include/jiminy/core/fwd.h | 15 +- .../jiminy/core/hardware/abstract_motor.h | 25 +- .../jiminy/core/hardware/abstract_sensor.h | 13 +- .../jiminy/core/hardware/abstract_sensor.hxx | 70 +- .../jiminy/core/hardware/basic_sensors.h | 56 +- core/include/jiminy/core/macros.h | 24 +- core/include/jiminy/core/robot/model.h | 113 +- .../robot/pinocchio_overload_algorithms.h | 2 +- core/include/jiminy/core/robot/robot.h | 44 +- .../jiminy/core/solver/constraint_solvers.h | 2 +- .../jiminy/core/stepper/abstract_stepper.h | 30 +- .../core/stepper/runge_kutta_dopri_stepper.h | 7 +- .../jiminy/core/telemetry/telemetry_data.h | 10 +- .../jiminy/core/telemetry/telemetry_data.hxx | 6 +- .../core/telemetry/telemetry_recorder.h | 4 +- .../jiminy/core/telemetry/telemetry_sender.h | 12 +- core/include/jiminy/core/utilities/helpers.h | 84 +- .../include/jiminy/core/utilities/helpers.hxx | 7 +- core/include/jiminy/core/utilities/json.hxx | 8 +- .../include/jiminy/core/utilities/pinocchio.h | 15 +- core/include/jiminy/core/utilities/random.h | 6 +- core/src/constraints/abstract_constraint.cc | 10 +- core/src/constraints/distance_constraint.cc | 6 +- core/src/constraints/frame_constraint.cc | 4 +- core/src/constraints/joint_constraint.cc | 2 +- core/src/constraints/sphere_constraint.cc | 4 +- core/src/constraints/wheel_constraint.cc | 4 +- core/src/control/abstract_controller.cc | 87 +- core/src/engine/engine.cc | 767 ++++++++------ core/src/hardware/abstract_motor.cc | 129 ++- core/src/hardware/abstract_sensor.cc | 41 +- core/src/hardware/basic_motors.cc | 29 +- core/src/hardware/basic_sensors.cc | 140 ++- core/src/io/abstract_io_device.cc | 16 +- core/src/io/file_device.cc | 32 +- core/src/io/json_loader.cc | 2 +- core/src/io/memory_device.cc | 2 +- core/src/robot/model.cc | 976 +++++++++--------- core/src/robot/robot.cc | 674 ++++++------ core/src/stepper/abstract_stepper.cc | 49 +- core/src/stepper/euler_explicit_stepper.cc | 5 +- core/src/stepper/runge_kutta_dopri_stepper.cc | 62 +- core/src/telemetry/telemetry_data.cc | 4 +- core/src/telemetry/telemetry_recorder.cc | 20 +- core/src/utilities/helpers.cc | 66 +- core/src/utilities/json.cc | 16 +- core/src/utilities/pinocchio.cc | 172 ++- core/src/utilities/random.cc | 30 +- core/unit/engine_sanity_check.cc | 21 +- core/unit/model_test.cc | 16 +- .../atlas/atlas_v4_options.toml | 1 - .../bipedal_robots/cassie/cassie_options.toml | 1 - data/bipedal_robots/digit/digit_options.toml | 1 - .../anymal/anymal_options.toml | 1 - data/toys_models/ant/ant_options.toml | 1 - docs/spec/src/tlmc_format_specification.md | 4 +- python/CMakeLists.txt | 13 +- .../gym_jiminy/common/bases/__init__.py | 6 + .../common/gym_jiminy/common/bases/blocks.py | 4 +- .../gym_jiminy/common/bases/interfaces.py | 52 +- .../gym_jiminy/common/bases/pipeline.py | 90 +- .../gym_jiminy/common/bases/quantity.py | 454 ++++++++ .../gym_jiminy/common/blocks/__init__.py | 3 +- .../common/blocks/deformation_estimator.py | 220 ++-- .../common/blocks/motor_safety_limit.py | 33 +- .../proportional_derivative_controller.py | 490 +++++---- .../common/gym_jiminy/common/envs/generic.py | 185 ++-- .../gym_jiminy/common/envs/locomotion.py | 34 +- .../gym_jiminy/common/quantities/__init__.py | 15 + .../gym_jiminy/common/quantities/generic.py | 276 +++++ .../common/quantities/locomotion.py | 125 +++ .../gym_jiminy/common/quantities/manager.py | 164 +++ .../gym_jiminy/common/utils/__init__.py | 2 + .../common/gym_jiminy/common/utils/math.py | 55 +- .../common/gym_jiminy/common/utils/misc.py | 6 +- .../common/wrappers/observation_stack.py | 4 +- python/gym_jiminy/common/setup.py | 2 +- .../envs/gym_jiminy/envs/acrobot.py | 12 +- python/gym_jiminy/envs/gym_jiminy/envs/ant.py | 31 +- .../gym_jiminy/envs/gym_jiminy/envs/anymal.py | 19 +- .../gym_jiminy/envs/gym_jiminy/envs/atlas.py | 42 +- .../envs/gym_jiminy/envs/cartpole.py | 10 +- .../gym_jiminy/envs/gym_jiminy/envs/cassie.py | 19 +- .../gym_jiminy/envs/gym_jiminy/envs/digit.py | 19 +- .../gym_jiminy/examples/quantity_benchmark.py | 61 ++ .../reinforcement_learning/rllib/ant_td3.py | 72 -- .../rllib/cartpole_ppo.py | 183 ---- .../rllib/tools/fcnet.py | 330 ------ .../rllib/acrobot_ppo.py | 42 +- .../stable_baselines3/acrobot_ppo.py | 2 +- .../stable_baselines3/cartpole_ppo.py | 2 +- .../stable_baselines3/tools/utilities.py | 0 .../tianshou/acrobot_ppo.py | 0 .../tianshou/cartpole_ppo.py | 0 .../tianshou/tools/fcnet.py | 0 .../tianshou/tools/utilities.py | 0 .../torchrl/acrobot_ppo.py | 0 .../gym_jiminy/rllib/gym_jiminy/rllib/ppo.py | 15 +- python/gym_jiminy/rllib/setup.py | 11 +- .../unit_py/data/anymal_pipeline.json | 18 +- .../unit_py/data/anymal_pipeline.toml | 12 +- .../unit_py/test_deformation_estimator.py | 47 +- .../unit_py/test_pipeline_control.py | 134 ++- .../unit_py/test_pipeline_design.py | 40 +- python/gym_jiminy/unit_py/test_quantities.py | 137 +++ .../unit_py/test_training_toys_models.py | 7 + .../box_uneven_ground_impulse_contact.py | 6 +- .../examples/constraint_fixed_frame.py | 8 +- python/jiminy_py/examples/constraint_wheel.py | 4 +- python/jiminy_py/examples/double_pendulum.py | 2 +- python/jiminy_py/examples/sphere_rolling.py | 4 +- python/jiminy_py/examples/tutorial.ipynb | 4 +- python/jiminy_py/setup.py | 50 +- python/jiminy_py/src/jiminy_py/dynamics.py | 49 +- python/jiminy_py/src/jiminy_py/log.py | 163 ++- python/jiminy_py/src/jiminy_py/plot.py | 25 +- python/jiminy_py/src/jiminy_py/robot.py | 40 +- python/jiminy_py/src/jiminy_py/simulator.py | 609 +++++++---- .../src/jiminy_py/viewer/__init__.py | 4 +- .../viewer/meshcat/meshcat_visualizer.py | 4 +- .../viewer/panda3d/panda3d_visualizer.py | 35 +- .../jiminy_py/src/jiminy_py/viewer/replay.py | 10 +- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 60 +- .../unit_py/data/simple_pendulum.urdf | 9 + python/jiminy_py/unit_py/test_dense_pole.py | 28 +- python/jiminy_py/unit_py/test_flexible_arm.py | 10 +- .../jiminy_py/unit_py/test_foot_pendulum.py | 90 ++ python/jiminy_py/unit_py/test_simple_mass.py | 9 +- .../jiminy_py/unit_py/test_simple_pendulum.py | 203 +++- python/jiminy_py/unit_py/test_simulator.py | 84 +- python/jiminy_py/unit_py/test_utils.py | 71 ++ python/jiminy_py/unit_py/utilities.py | 52 +- python/jiminy_pywrap/CMakeLists.txt | 2 +- .../include/jiminy/python/compatibility.h | 2 +- .../include/jiminy/python/constraints.h | 4 +- .../include/jiminy/python/controllers.h | 4 +- .../include/jiminy/python/engine.h | 8 +- .../include/jiminy/python/generators.h | 2 +- .../include/jiminy/python/helpers.h | 2 +- .../include/jiminy/python/motors.h | 4 +- .../include/jiminy/python/robot.h | 4 +- .../include/jiminy/python/sensors.h | 6 +- .../include/jiminy/python/utilities.h | 110 +- python/jiminy_pywrap/src/controllers.cc | 62 +- python/jiminy_pywrap/src/engine.cc | 63 +- python/jiminy_pywrap/src/functors.cc | 22 + python/jiminy_pywrap/src/generators.cc | 6 +- python/jiminy_pywrap/src/helpers.cc | 300 +++--- python/jiminy_pywrap/src/motors.cc | 7 +- python/jiminy_pywrap/src/robot.cc | 176 ++-- python/jiminy_pywrap/src/sensors.cc | 7 +- 184 files changed, 6549 insertions(+), 4120 deletions(-) create mode 100644 .style.yapf create mode 100644 python/gym_jiminy/common/gym_jiminy/common/bases/quantity.py create mode 100644 python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py create mode 100644 python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py create mode 100644 python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py create mode 100644 python/gym_jiminy/common/gym_jiminy/common/quantities/manager.py create mode 100644 python/gym_jiminy/examples/quantity_benchmark.py delete mode 100644 python/gym_jiminy/examples/reinforcement_learning/rllib/ant_td3.py delete mode 100644 python/gym_jiminy/examples/reinforcement_learning/rllib/cartpole_ppo.py delete mode 100644 python/gym_jiminy/examples/reinforcement_learning/rllib/tools/fcnet.py rename python/gym_jiminy/examples/{reinforcement_learning => }/rllib/acrobot_ppo.py (93%) rename python/gym_jiminy/examples/{reinforcement_learning => }/stable_baselines3/acrobot_ppo.py (98%) rename python/gym_jiminy/examples/{reinforcement_learning => }/stable_baselines3/cartpole_ppo.py (97%) rename python/gym_jiminy/examples/{reinforcement_learning => }/stable_baselines3/tools/utilities.py (100%) rename python/gym_jiminy/examples/{reinforcement_learning => }/tianshou/acrobot_ppo.py (100%) rename python/gym_jiminy/examples/{reinforcement_learning => }/tianshou/cartpole_ppo.py (100%) rename python/gym_jiminy/examples/{reinforcement_learning => }/tianshou/tools/fcnet.py (100%) rename python/gym_jiminy/examples/{reinforcement_learning => }/tianshou/tools/utilities.py (100%) rename python/gym_jiminy/examples/{reinforcement_learning => }/torchrl/acrobot_ppo.py (100%) create mode 100644 python/gym_jiminy/unit_py/test_quantities.py create mode 100644 python/jiminy_py/unit_py/test_foot_pendulum.py create mode 100644 python/jiminy_py/unit_py/test_utils.py diff --git a/.github/workflows/linux.yml b/.github/workflows/linux.yml index 625565c8a..f2fc114a9 100644 --- a/.github/workflows/linux.yml +++ b/.github/workflows/linux.yml @@ -66,17 +66,19 @@ jobs: sudo apt install -y gdb gnupg curl wget build-essential cmake doxygen graphviz texlive-latex-base "${PYTHON_EXECUTABLE}" -m pip install setuptools wheel "pip>=20.3" - "${PYTHON_EXECUTABLE}" -m pip install "numpy>=1.21,<2.0" git config --global advice.detachedHead false - - name: Build jiminy dependencies - run: | - BUILD_TYPE="${{ matrix.BUILD_TYPE }}" ./build_tools/build_install_deps_unix.sh - name: Install pre-compiled binaries for additional gym-jiminy dependencies - if: matrix.PYTHON_VERSION != '3.12' + if: matrix.PYTHON_VERSION != '3.13' run: | "${PYTHON_EXECUTABLE}" -m pip install "torch" -f https://download.pytorch.org/whl/torch - "${PYTHON_EXECUTABLE}" -m pip install "gymnasium>=0.26,<0.29" "importlib-metadata>=3.3.0" + "${PYTHON_EXECUTABLE}" -m pip install "gymnasium>=0.28,<1.0" "stable_baselines3>=2.0" + - name: Install latest numpy version at build-time for run-time binary compatibility + run: | + "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy>=1.24,<2.0" + - name: Build jiminy dependencies + run: | + BUILD_TYPE="${{ matrix.BUILD_TYPE }}" ./build_tools/build_install_deps_unix.sh ##################################################################################### @@ -84,14 +86,15 @@ jobs: run: | unset Boost_ROOT - mkdir "$RootDir/build" - cd "$RootDir/build" - cmake "$RootDir" -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" \ + mkdir "${RootDir}/build" + cd "${RootDir}/build" + cmake "${RootDir}" -Wdev \ + -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" \ -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" \ -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE -DBoost_USE_STATIC_LIBS=ON \ -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \ -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON \ - -DINSTALL_GYM_JIMINY=${{ (matrix.PYTHON_VERSION == '3.12' && 'OFF') || 'ON' }} \ + -DINSTALL_GYM_JIMINY=${{ (matrix.PYTHON_VERSION == '3.13' && 'OFF') || 'ON' }} \ -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS}" -DCMAKE_BUILD_TYPE="${{ matrix.BUILD_TYPE }}" make install -j2 @@ -103,9 +106,9 @@ jobs: run: | "$InstallDir/bin/jiminy_double_pendulum" - mkdir -p "$RootDir/core/examples/external_project/build" - cd "$RootDir/core/examples/external_project/build" - cmake "$RootDir/core/examples/external_project/" -DCMAKE_INSTALL_PREFIX="$InstallDir" \ + mkdir -p "${RootDir}/core/examples/external_project/build" + cd "${RootDir}/core/examples/external_project/build" + cmake "${RootDir}/core/examples/external_project/" -DCMAKE_INSTALL_PREFIX="$InstallDir" \ -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \ -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS}" -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" make install @@ -118,46 +121,43 @@ jobs: run: | ctest --output-on-failure --test-dir "${RootDir}/build/core/unit" - "${PYTHON_EXECUTABLE}" -m unittest discover -s "$RootDir/python/jiminy_py/unit_py" -v + "${PYTHON_EXECUTABLE}" -m unittest discover -s "${RootDir}/python/jiminy_py/unit_py" -v - - name: Run unit tests for gym_jiminy - if: matrix.BUILD_TYPE == 'Release' && matrix.PYTHON_VERSION != '3.12' + - name: Run unit tests for gym jiminy base module run: | - cd "$RootDir/python/gym_jiminy/examples/reinforcement_learning/rllib" - gdb -batch -ex "run" -ex "bt" --args "${PYTHON_EXECUTABLE}" acrobot_ppo.py 2>&1 | \ - grep -v ^"No stack."$ && exit 1 - - # FIXME: Must install stable_baselines3 after running rllib test - # because they require a different version of `gymnasium`. - "${PYTHON_EXECUTABLE}" -m pip install "stable_baselines3>=2.0" + if [[ "${{ matrix.BUILD_TYPE }}" == 'Debug' ]] ; then + export JIMINY_BUILD_DEBUG= + fi + "${PYTHON_EXECUTABLE}" -m unittest discover -s "${RootDir}/python/gym_jiminy/unit_py" -v - "${PYTHON_EXECUTABLE}" -m unittest discover -s "$RootDir/python/gym_jiminy/unit_py" -v + - name: Run examples for gym jiminy add-on modules + if: matrix.BUILD_TYPE != 'Debug' && matrix.PYTHON_VERSION != '3.12' + run: | + cd "${RootDir}/python/gym_jiminy/examples/rllib" + "${PYTHON_EXECUTABLE}" acrobot_ppo.py - name: Python static type checker - if: matrix.PYTHON_VERSION == '3.8' && matrix.BUILD_TYPE == 'Release' + if: matrix.PYTHON_VERSION == '3.8' && matrix.BUILD_TYPE != 'Debug' run: | # Generate stubs - stubgen -p jiminy_py -o $RootDir/build/pypi/jiminy_py/src - "${PYTHON_EXECUTABLE}" "$RootDir/build_tools/stub_gen.py" \ - -o $RootDir/build/stubs --ignore-invalid=all jiminy_py - cp $RootDir/build/stubs/jiminy_py-stubs/core/__init__.pyi \ - $RootDir/build/pypi/jiminy_py/src/jiminy_py/core/core.pyi + stubgen -p jiminy_py -o ${RootDir}/build/pypi/jiminy_py/src + "${PYTHON_EXECUTABLE}" "${RootDir}/build_tools/stub_gen.py" \ + -o ${RootDir}/build/stubs --ignore-invalid=all jiminy_py + cp ${RootDir}/build/stubs/jiminy_py-stubs/core/__init__.pyi \ + ${RootDir}/build/pypi/jiminy_py/src/jiminy_py/core/core.pyi # Re-install jiminy with stubs - cd "$RootDir/build" + cd "${RootDir}/build" make install - # FIXME: Force re-installing the right version of gymnasium - "${PYTHON_EXECUTABLE}" -m pip install "gymnasium>=0.28,<0.29" - # mypy is not able to follow `.pth` when checking directories or files. # This bug may be fixed in the future. The only workaround is to set `MYPYPATH` manually. # Anyway, the stubs generated for eigenpy, hppfcl and pinocchio they are incomplete and # even partially wrong, so it is better to ignore them for now. # See: https://github.com/python/mypy/issues/14848. - cd "$RootDir/python/jiminy_py/" - mypy --config-file="$RootDir/.mypy.ini" -p src + cd "${RootDir}/python/jiminy_py/" + mypy --config-file="${RootDir}/.mypy.ini" -p src for name in "common" "envs" "toolbox" "rllib"; do - cd "$RootDir/python/gym_jiminy/$name" - mypy --config-file="$RootDir/.mypy.ini" -p gym_jiminy + cd "${RootDir}/python/gym_jiminy/$name" + mypy --config-file="${RootDir}/.mypy.ini" -p gym_jiminy done diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index b938eab44..dc4608b19 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -68,17 +68,19 @@ jobs: echo "InstallDir=${GITHUB_WORKSPACE}/install" >> $GITHUB_ENV "${PYTHON_EXECUTABLE}" -m pip install setuptools wheel "pip>=20.3" - "${PYTHON_EXECUTABLE}" -m pip install "numpy>=1.21,<2.0" "${PYTHON_EXECUTABLE}" -m pip install delocate twine + - name: Install pre-compiled binaries for additional gym-jiminy dependencies + if: matrix.PYTHON_VERSION != '3.13' + run: | + "${PYTHON_EXECUTABLE}" -m pip install "torch" -f https://download.pytorch.org/whl/torch + "${PYTHON_EXECUTABLE}" -m pip install "gymnasium>=0.28,<1.0" "stable_baselines3>=2.0" + - name: Install latest numpy version at build-time for run-time binary compatibility + run: | + "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy>=1.24,<2.0" - name: Build jiminy dependencies run: | OSX_DEPLOYMENT_TARGET=${OSX_DEPLOYMENT_TARGET} OSX_ARCHITECTURES=${OSX_ARCHITECTURES} \ BUILD_TYPE="${{ matrix.BUILD_TYPE }}" ./build_tools/build_install_deps_unix.sh - - name: Install pre-compiled binaries for additional gym-jiminy dependencies - if: matrix.PYTHON_VERSION != '3.12' - run: | - "${PYTHON_EXECUTABLE}" -m pip install "torch" -f https://download.pytorch.org/whl/torch - "${PYTHON_EXECUTABLE}" -m pip install "gymnasium>=0.26,<0.29" "importlib-metadata>=3.3.0" ##################################################################################### @@ -90,7 +92,8 @@ jobs: mkdir "${RootDir}/build" cd "${RootDir}/build" export LD_LIBRARY_PATH="${InstallDir}/lib/:/usr/local/lib" - cmake "${RootDir}" -DCMAKE_INSTALL_PREFIX="${InstallDir}" -DCMAKE_PREFIX_PATH="${InstallDir}" \ + cmake "${RootDir}" -Wdev \ + -DCMAKE_INSTALL_PREFIX="${InstallDir}" -DCMAKE_PREFIX_PATH="${InstallDir}" \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" \ -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ @@ -99,7 +102,7 @@ jobs: -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \ -DBoost_USE_STATIC_LIBS=ON -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \ -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON \ - -DINSTALL_GYM_JIMINY=${{ (matrix.PYTHON_VERSION == '3.12' && 'OFF') || 'ON' }} \ + -DINSTALL_GYM_JIMINY=${{ (matrix.PYTHON_VERSION == '3.13' && 'OFF') || 'ON' }} \ -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS}" -DCMAKE_BUILD_TYPE="${{ matrix.BUILD_TYPE }}" make -j2 @@ -140,7 +143,7 @@ jobs: -p "macosx_${OSX_DEPLOYMENT_TARGET//./_}_${WHEEL_ARCH}" "${RootDir}/build/wheelhouse/"*.whl "${PYTHON_EXECUTABLE}" -m pip install --force-reinstall --no-deps "${RootDir}/build/wheelhouse/"*.whl - name: Upload the wheel for Linux of jiminy_py - if: matrix.BUILD_TYPE == 'Release' + if: matrix.BUILD_TYPE != 'Debug' uses: actions/upload-artifact@v4 with: name: wheelhouse-${{ matrix.PYTHON_VERSION }} @@ -175,28 +178,25 @@ jobs: cd "${RootDir}/python/jiminy_py/unit_py" "${PYTHON_EXECUTABLE}" -m unittest discover -v - - name: Run unit tests for gym_jiminy - if: matrix.BUILD_TYPE != 'Release' + - name: Run unit tests for gym jiminy base module run: | - # Disabling `test_pipeline_control.py` on MacOS because `test_pid_standing` is failing - # for 'panda3d-sync' backend due to meshes still loading at screenshot time. - cd "${RootDir}/python/gym_jiminy/unit_py" - "${PYTHON_EXECUTABLE}" -m unittest test_pipeline_design.py -v + export LD_LIBRARY_PATH="${InstallDir}/lib/:/usr/local/lib" + + # FIXME: Disabling `test_pipeline_control.py` on MacOS because `test_pid_standing` is + # failing for 'panda3d-sync' backend due to meshes still loading at screenshot time. + if [[ "${{ matrix.BUILD_TYPE }}" == 'Debug' ]] ; then + export JIMINY_BUILD_DEBUG= + fi + "${PYTHON_EXECUTABLE}" -m unittest discover -s "${RootDir}/python/gym_jiminy/unit_py" -v - - name: Run unit tests for gym jiminy - if: matrix.BUILD_TYPE == 'Release' && matrix.PYTHON_VERSION != '3.12' + - name: Run examples for gym jiminy add-on modules + if: matrix.BUILD_TYPE != 'Debug' && matrix.PYTHON_VERSION != '3.12' run: | export LD_LIBRARY_PATH="${InstallDir}/lib/:/usr/local/lib" - cd "${RootDir}/python/gym_jiminy/examples/reinforcement_learning/rllib" + cd "${RootDir}/python/gym_jiminy/examples/rllib" "${PYTHON_EXECUTABLE}" acrobot_ppo.py - # FIXME: Must install stable_baselines3 after running rllib test - # because they require a different version of `gymnasium`. - "${PYTHON_EXECUTABLE}" -m pip install "stable_baselines3>=2.0" - - "${PYTHON_EXECUTABLE}" -m unittest discover -s "${RootDir}/python/gym_jiminy/unit_py" -v - ######################################################################################### publish-pypi-macos: diff --git a/.github/workflows/manylinux.yml b/.github/workflows/manylinux.yml index 5d58ab3f4..a561bfb1c 100644 --- a/.github/workflows/manylinux.yml +++ b/.github/workflows/manylinux.yml @@ -62,8 +62,10 @@ jobs: echo "InstallDir=${GITHUB_WORKSPACE}/install" >> $GITHUB_ENV "${PYTHON_EXECUTABLE}" -m pip install setuptools wheel "pip>=20.3" - "${PYTHON_EXECUTABLE}" -m pip install "numpy>=1.21,<2.0" "${PYTHON_EXECUTABLE}" -m pip install twine cmake + - name: Install latest numpy version at build-time for run-time binary compatibility + run: | + "${PYTHON_EXECUTABLE}" -m pip install --upgrade "numpy>=1.24,<2.0" - name: Build project dependencies run: | ./build_tools/build_install_deps_unix.sh @@ -78,7 +80,8 @@ jobs: mkdir "$RootDir/build" cd "$RootDir/build" export LD_LIBRARY_PATH="$InstallDir/lib:$InstallDir/lib64:/usr/local/lib" - cmake "$RootDir" -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" \ + cmake "$RootDir" -Wdev \ + -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_VERBOSE_MAKEFILE=ON \ -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" \ -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \ diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 72ce96e7b..e247da210 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -76,12 +76,12 @@ jobs: sudo apt install ninja-build fi "${PYTHON_EXECUTABLE}" -m pip install "torch" -f https://download.pytorch.org/whl/torch - "${PYTHON_EXECUTABLE}" -m pip install "gymnasium>=0.26,<0.29" "importlib-metadata>=3.3.0" + "${PYTHON_EXECUTABLE}" -m pip install "gymnasium>=0.28,<1.0" "stable_baselines3>=2.0" ##################################################################################### - name: PEP8 Code Style Check - if: matrix.OS == 'ubuntu-20.04' && matrix.BUILD_TYPE == 'Release' && matrix.COMPILER == 'gcc' + if: matrix.OS == 'ubuntu-20.04' && matrix.BUILD_TYPE != 'Debug' && matrix.COMPILER == 'gcc' run: | "${PYTHON_EXECUTABLE}" -m pip install flake8 flake8 --ignore=E121,E126,E123,E226,E241,E266,E402,E741,F405,W504 \ @@ -94,7 +94,9 @@ jobs: mkdir "${RootDir}/build" "${InstallDir}" cd "${RootDir}/build" - cmake "${RootDir}" -G "${{ matrix.GENERATOR }}" -DCMAKE_EXE_LINKER_FLAGS="${{ matrix.LINKER_FLAGS }}" \ + cmake "${RootDir}" -Wdev -G "${{ matrix.GENERATOR }}" \ + -DCMAKE_EXE_LINKER_FLAGS="${{ matrix.LINKER_FLAGS }}" \ + -DCMAKE_SHARED_LINKER_FLAGS="${{ matrix.LINKER_FLAGS }}" \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ -DCMAKE_PREFIX_PATH="/opt/openrobots/" -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DBoost_NO_SYSTEM_PATHS=OFF \ @@ -120,7 +122,9 @@ jobs: RootDir="${RootDir}/core/examples/external_project/" mkdir -p "${RootDir}/build" cd "${RootDir}/build" - cmake "${RootDir}" -G "${{ matrix.GENERATOR }}" -DCMAKE_EXE_LINKER_FLAGS="${{ matrix.LINKER_FLAGS }}" \ + cmake "${RootDir}" -G "${{ matrix.GENERATOR }}" \ + -DCMAKE_EXE_LINKER_FLAGS="${{ matrix.LINKER_FLAGS }}" \ + -DCMAKE_SHARED_LINKER_FLAGS="${{ matrix.LINKER_FLAGS }}" \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ -DCMAKE_PREFIX_PATH="/opt/openrobots/" -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \ @@ -139,26 +143,23 @@ jobs: "${PYTHON_EXECUTABLE}" -m unittest discover -s "${RootDir}/python/jiminy_py/unit_py" -v - - name: Run unit tests for gym_jiminy - if: matrix.BUILD_TYPE != 'Release' + - name: Run unit tests for gym jiminy base module run: | - cd "${RootDir}/python/gym_jiminy/unit_py" - "${PYTHON_EXECUTABLE}" -m unittest test_pipeline_control.py test_pipeline_design.py -v + if [[ "${{ matrix.BUILD_TYPE }}" == 'Debug' ]] ; then + export JIMINY_BUILD_DEBUG= + fi + "${PYTHON_EXECUTABLE}" -m unittest discover -s "${RootDir}/python/gym_jiminy/unit_py" -v - - name: Run unit tests for gym_jiminy - if: matrix.BUILD_TYPE == 'Release' + - name: Run examples for gym jiminy add-on modules + if: matrix.BUILD_TYPE != 'Debug' run: | - cd "${RootDir}/python/gym_jiminy/examples/reinforcement_learning/rllib" + cd "${RootDir}/python/gym_jiminy/examples/rllib" "${PYTHON_EXECUTABLE}" acrobot_ppo.py - "${PYTHON_EXECUTABLE}" -m pip install "stable_baselines3>=2.0" - - "${PYTHON_EXECUTABLE}" -m unittest discover -s "${RootDir}/python/gym_jiminy/unit_py" -v - ##################################################################################### - name: Python linter - if: matrix.OS == 'ubuntu-20.04' && matrix.BUILD_TYPE == 'Release' && matrix.COMPILER == 'gcc' + if: matrix.OS == 'ubuntu-20.04' && matrix.BUILD_TYPE != 'Debug' && matrix.COMPILER == 'gcc' run: | cd "${RootDir}/python/jiminy_py/" pylint --rcfile="${RootDir}/.pylintrc" "src/" diff --git a/.github/workflows/win.yml b/.github/workflows/win.yml index 3233bc585..3e276f967 100644 --- a/.github/workflows/win.yml +++ b/.github/workflows/win.yml @@ -56,21 +56,27 @@ jobs: run: | git config --global advice.detachedHead false python -m pip install setuptools wheel "pip>=20.3" - python -m pip install "numpy>=1.21,<2.0" python -m pip install pefile machomachomangler + - name: Install pre-compiled binaries for additional gym-jiminy dependencies + if: matrix.PYTHON_VERSION != '3.13' + run: | + python -m pip install "torch" -f https://download.pytorch.org/whl/torch + python -m pip install "gymnasium>=0.28,<1.0" "stable_baselines3>=2.0" + - name: Install latest numpy version at build-time for run-time binary compatibility + run: | + python -m pip install --upgrade "numpy>=1.24,<2.0" - name: Build jiminy dependencies run: | ${env:BUILD_TYPE} = "${{ matrix.BUILD_TYPE }}" & "./workspace/build_tools/build_install_deps_windows.ps1" - - name: Install pre-compiled binaries for additional gym-jiminy dependencies - if: matrix.PYTHON_VERSION != '3.11' && matrix.PYTHON_VERSION != '3.12' - run: | - python -m pip install "torch" -f https://download.pytorch.org/whl/torch - python -m pip install "gymnasium>=0.26,<0.29" "importlib-metadata>=3.3.0" ##################################################################################### - name: Configure and build jiminy + # FIXME: CMake exits with non-zero code if an error occurred in 'execute_process'. + # Indeed, such errors are expected to occur if some gym_jiminy add-on cannot be + # installed because of missing pre-compiled dependency on Pypi. + continue-on-error: true run: | $ErrorActionPreference = "Stop" Set-PSDebug -Trace 1 @@ -86,7 +92,7 @@ jobs: New-Item -ItemType "directory" -Force -Path "$RootDir/build" } Set-Location -Path $RootDir/build - cmake "$RootDir" -G "${env:GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` + cmake "$RootDir" -Wdev -G "${env:GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=OFF -DCMAKE_VERBOSE_MAKEFILE=ON ` @@ -94,7 +100,7 @@ jobs: -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE ` -DBoost_USE_STATIC_LIBS=ON -DPYTHON_REQUIRED_VERSION="${{ matrix.PYTHON_VERSION }}" ` -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON ` - -DINSTALL_GYM_JIMINY=${{ ((matrix.PYTHON_VERSION == '3.11' || matrix.PYTHON_VERSION == '3.12') && 'OFF') || 'ON' }} ` + -DINSTALL_GYM_JIMINY=${{ ((matrix.PYTHON_VERSION == '3.13') && 'OFF') || 'ON' }} ` -DCMAKE_CXX_FLAGS="${env:CMAKE_CXX_FLAGS} $( ) -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DBOOST_CORE_USE_GENERIC_CMATH $( ) -DEIGENPY_STATIC -DURDFDOM_STATIC -DHPP_FCL_STATIC -DPINOCCHIO_STATIC" @@ -141,7 +147,7 @@ jobs: python -m pip install --force-reinstall --no-deps $wheel_path } - name: Upload the wheel for Windows of jiminy_py - if: matrix.BUILD_TYPE == 'Release' + if: matrix.BUILD_TYPE != 'Debug' uses: actions/upload-artifact@v4 with: name: wheelhouse-${{ matrix.PYTHON_VERSION }} @@ -182,28 +188,26 @@ jobs: python -m unittest discover -s "${RootDir}/python/jiminy_py/unit_py" -v - - name: Run unit tests for gym_jiminy - if: matrix.BUILD_TYPE != 'Release' + - name: Run unit tests for gym jiminy base module run: | $RootDir = "${env:GITHUB_WORKSPACE}/workspace" -replace '\\', '/' - Set-Location -Path "$RootDir/python/gym_jiminy/unit_py" - python -m unittest test_pipeline_control.py test_pipeline_design.py -v + if ("${{ matrix.BUILD_TYPE }}" -eq "Debug") { + ${env:JIMINY_BUILD_DEBUG} = 1 + } + python -m unittest discover -s "$RootDir/python/gym_jiminy/unit_py" -v - - name: Running unit tests for gym_jiminy - if: matrix.BUILD_TYPE == 'Release' && matrix.PYTHON_VERSION != '3.11' && matrix.PYTHON_VERSION != '3.12' + - name: Run examples for gym jiminy add-on modules + if: matrix.BUILD_TYPE != 'Debug' && matrix.PYTHON_VERSION != '3.11' && matrix.PYTHON_VERSION != '3.12' run: | $RootDir = "${env:GITHUB_WORKSPACE}/workspace" -replace '\\', '/' - Set-Location -Path "$RootDir/python/gym_jiminy/examples/reinforcement_learning/rllib" + # FIXME: Python 3.11 was not supported by ray on Windows until very recently. + # It has been fixed on master but not on the latest available release (2.93). + # See: https://github.com/ray-project/ray/pull/42097 + Set-Location -Path "$RootDir/python/gym_jiminy/examples/rllib" python acrobot_ppo.py - # FIXME: Must install stable_baselines3 after running rllib test - # because they require a different version of `gymnasium`. - python -m pip install "stable_baselines3>=2.0" - - python -m unittest discover -s "$RootDir/python/gym_jiminy/unit_py" -v - ######################################################################################### publish-pypi-win10: diff --git a/.gitignore b/.gitignore index afdb50304..dc132985d 100644 --- a/.gitignore +++ b/.gitignore @@ -10,3 +10,4 @@ install build build_tools/cmake/*cppad* **/log +**/*.trace diff --git a/.style.yapf b/.style.yapf new file mode 100644 index 000000000..f14008645 --- /dev/null +++ b/.style.yapf @@ -0,0 +1,22 @@ +# options: https://github.com/google/yapf +# Do not enable auto-formatting for now because it does not support: +# * splitting all arguments on separated lines if it does not fit on one line +# AND adding a single line is not enough. +# * Prefer splitting top-level over low-level (eg add new line instead of +# splitting typing annotations) +# In addition, there are several annoying issues: +# * Inconsistent formatting from time to time +# * Inline comments are messing up with formatting + +[style] + +based_on_style = google +column_limit = 79 + +coalesce_brackets = True +blank_lines_between_top_level_imports_and_variables = 2 +spaces_around_power_operator = True +# split_all_top_level_comma_separated_values = True +split_arguments_when_comma_terminated = True +split_before_expression_after_opening_paren = True +split_before_named_assigns = False diff --git a/CMakeLists.txt b/CMakeLists.txt index e0cda7484..a35b03d5e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,19 @@ cmake_minimum_required(VERSION 3.12.4) # Set the build version (specify a tweak version to indicated post-release if needed) -set(BUILD_VERSION 1.8.3) +set(BUILD_VERSION 1.8.4) + +# MSVC runtime library flags are defined by 'CMAKE_MSVC_RUNTIME_LIBRARY' +if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.15.7) + cmake_policy(SET CMP0091 NEW) + set(CMAKE_POLICY_DEFAULT_CMP0091 NEW) +endif() + +# Set find_package strategy to look for both upper-case and case-preserved variables +if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.27.0) + cmake_policy(SET CMP0144 NEW) + set(CMAKE_POLICY_DEFAULT_CMP0144 NEW) +endif() # Set compatibility set(COMPATIBILITY_VERSION SameMinorVersion) @@ -63,6 +75,8 @@ if(MSVC) set(CMAKE_CXX_FLAGS_DEBUG "/Zi /Od") set(CMAKE_CXX_FLAGS_RELEASE "/DNDEBUG /O2 /Ob3") set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELEASE} /Zi") + set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS} /DEBUG:FULL /INCREMENTAL:NO /OPT:REF /OPT:ICF") + set(CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO "${CMAKE_SHARED_LINKER_FLAGS_DEBUG}") set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS} /DEBUG:FULL /INCREMENTAL:NO /OPT:REF /OPT:ICF") set(CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO "${CMAKE_SHARED_LINKER_FLAGS_DEBUG}") else() @@ -70,6 +84,8 @@ else() set(CMAKE_CXX_FLAGS_DEBUG "-DBOOST_PYTHON_DEBUG -g -O0 -fno-omit-frame-pointer") set(CMAKE_CXX_FLAGS_RELEASE "-DNDEBUG -O3 -Wfatal-errors -Werror") set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELEASE} -g -fno-omit-frame-pointer -fno-sanitize-recover") + set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS} -fno-omit-frame-pointer") + set(CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO "${CMAKE_SHARED_LINKER_FLAGS_DEBUG}") set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS} -fno-omit-frame-pointer") set(CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO "${CMAKE_SHARED_LINKER_FLAGS_DEBUG}") endif() diff --git a/INSTALL.md b/INSTALL.md index e8ce76292..9bb43f892 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -50,7 +50,7 @@ python -m pip install --prefer-binary gym-jiminy[all] ## Excluding dependencies on Ubuntu 18+ -First, one must install the pre-compiled libraries of the dependencies. Most of them are available on `robotpkg` APT repository. Just run the bash script to install them automatically for Ubuntu 18 and upward. It should be straightforward to adapt it to any other distribution for which `robotpkg` is available. Note that if you plan to use a virtual environment (`venv`, `pyenv`, ...), you must install the Python dependencies manually using `pip`, i.e. `"wheel", "numpy>=1.21,<2.0"`. +First, one must install the pre-compiled libraries of the dependencies. Most of them are available on `robotpkg` APT repository. Just run the bash script to install them automatically for Ubuntu 18 and upward. It should be straightforward to adapt it to any other distribution for which `robotpkg` is available. Note that if you plan to use a virtual environment (`venv`, `pyenv`, ...), you must install the Python dependencies manually using `pip`, i.e. `"wheel", "numpy>=1.24,<2.0"`. ```bash sudo env "PATH=$PATH" ./build_tools/easy_install_deps_ubuntu.sh @@ -79,7 +79,7 @@ make install -j2 ```bash sudo apt install -y gnupg curl wget build-essential cmake doxygen graphviz -python -m pip install "wheel", "numpy>=1.21,<2.0" +python -m pip install "wheel", "numpy>=1.24,<2.0" ``` ### Jiminy dependencies build and install @@ -121,7 +121,7 @@ You have to preinstall by yourself the (free) MSVC 2019 toolchain. Then, install `setuptools`, `wheel` and `numpy`. ```powershell -python -m pip install setuptools wheel "numpy>=1.21,<2.0" +python -m pip install setuptools wheel "numpy>=1.24,<2.0" ``` ### Jiminy dependencies build and install diff --git a/build_tools/build_install_deps_unix.sh b/build_tools/build_install_deps_unix.sh index e52ea7fc1..7c4355300 100755 --- a/build_tools/build_install_deps_unix.sh +++ b/build_tools/build_install_deps_unix.sh @@ -10,24 +10,34 @@ if [ -z ${BUILD_TYPE} ]; then fi ### Set the macos sdk version min if undefined -if [ -z ${OSX_DEPLOYMENT_TARGET} ]; then - OSX_DEPLOYMENT_TARGET="10.15" - echo "OSX_DEPLOYMENT_TARGET is unset. Defaulting to '${OSX_DEPLOYMENT_TARGET}'." -fi - -### Set the build architecture if undefined -if [ -z ${OSX_ARCHITECTURES} ]; then - OSX_ARCHITECTURES="arm64" - echo "OSX_ARCHITECTURES is unset. Defaulting to '${OSX_ARCHITECTURES}'." +if [ "$(uname -s)" = "Darwin" ]; then + if [ -z ${OSX_DEPLOYMENT_TARGET} ]; then + OSX_DEPLOYMENT_TARGET="10.15" + echo "OSX_DEPLOYMENT_TARGET is unset. Defaulting to '${OSX_DEPLOYMENT_TARGET}'." + fi + + ### Set the build architecture if undefined + if [ -z ${OSX_ARCHITECTURES} ]; then + OSX_ARCHITECTURES="x86_64;arm64" + echo "OSX_ARCHITECTURES is unset. Defaulting to '${OSX_ARCHITECTURES}'." + fi fi ### Set the compiler(s) if undefined if [ -z ${CMAKE_C_COMPILER} ]; then - CMAKE_C_COMPILER="$(which gcc)" + if [ "$(uname -s)" = "Darwin" ]; then + CMAKE_C_COMPILER="$(which clang)" + else + CMAKE_C_COMPILER="$(which gcc)" + fi echo "CMAKE_C_COMPILER is unset. Defaulting to '${CMAKE_C_COMPILER}'." fi if [ -z ${CMAKE_CXX_COMPILER} ]; then - CMAKE_CXX_COMPILER="$(which g++)" + if [ "$(uname -s)" = "Darwin" ]; then + CMAKE_CXX_COMPILER="$(which clang++)" + else + CMAKE_CXX_COMPILER="$(which g++)" + fi echo "CMAKE_CXX_COMPILER is unset. Defaulting to '${CMAKE_CXX_COMPILER}'." fi @@ -62,7 +72,10 @@ if [ -z ${PYTHON_EXECUTABLE} ]; then fi ### Configure site-packages pythonic "symlink" pointing to install directory -PYTHON_USER_SITELIB="$("${PYTHON_EXECUTABLE}" -c 'import sysconfig; print(sysconfig.get_path("purelib"))')" || true +PYTHON_USER_SITELIB="$("${PYTHON_EXECUTABLE}" -c 'import sysconfig; print(sysconfig.get_path("purelib"))')" +if [ ! -w "$PYTHON_USER_SITELIB" ]; then + PYTHON_USER_SITELIB="$("${PYTHON_EXECUTABLE}" -m site --user-site)" +fi PYTHON_VERSION="$(${PYTHON_EXECUTABLE} -c "import sysconfig; print(sysconfig.get_config_var('py_version_short'))")" mkdir -p "${PYTHON_USER_SITELIB}" echo "${InstallDir}/lib/python${PYTHON_VERSION}/site-packages" > "${PYTHON_USER_SITELIB}/install_site.pth" @@ -87,6 +100,8 @@ unset Boost_ROOT if [ ! -d "${RootDir}/boost" ]; then git clone --depth 1 https://github.com/boostorg/boost.git "${RootDir}/boost" fi +rm -rf "${RootDir}/boost/build" +mkdir -p "${RootDir}/boost/build" cd "${RootDir}/boost" git reset --hard git fetch origin "boost-1.76.0" && git checkout --force FETCH_HEAD || true @@ -101,6 +116,8 @@ git apply --reject --whitespace=fix "${RootDir}/build_tools/patch_deps_unix/boos if [ ! -d "${RootDir}/eigen3" ]; then git clone --depth 1 https://gitlab.com/libeigen/eigen.git "${RootDir}/eigen3" fi +rm -rf "${RootDir}/eigen3/build" +mkdir -p "${RootDir}/eigen3/build" cd "${RootDir}/eigen3" git reset --hard git fetch origin "3.4.0" && git checkout --force FETCH_HEAD || true @@ -109,9 +126,11 @@ git fetch origin "3.4.0" && git checkout --force FETCH_HEAD || true if [ ! -d "${RootDir}/eigenpy" ]; then git clone --depth 1 https://github.com/stack-of-tasks/eigenpy.git "${RootDir}/eigenpy" fi +rm -rf "${RootDir}/eigenpy/build" +mkdir -p "${RootDir}/eigenpy/build" cd "${RootDir}/eigenpy" git reset --hard -git fetch origin "v3.3.0" && git checkout --force FETCH_HEAD || true +git fetch origin "v3.4.0" && git checkout --force FETCH_HEAD || true git submodule --quiet foreach --recursive git reset --quiet --hard git submodule --quiet update --init --recursive --depth 1 --jobs 8 git apply --reject --whitespace=fix "${RootDir}/build_tools/patch_deps_unix/eigenpy.patch" @@ -120,6 +139,8 @@ git apply --reject --whitespace=fix "${RootDir}/build_tools/patch_deps_unix/eige if [ ! -d "${RootDir}/tinyxml" ]; then git clone --depth 1 https://github.com/robotology-dependencies/tinyxml.git "${RootDir}/tinyxml" fi +rm -rf "${RootDir}/tinyxml/build" +mkdir -p "${RootDir}/tinyxml/build" cd "${RootDir}/tinyxml" git reset --hard git fetch origin "master" && git checkout --force FETCH_HEAD || true @@ -129,6 +150,8 @@ git apply --reject --whitespace=fix "${RootDir}/build_tools/patch_deps_unix/tiny if [ ! -d "${RootDir}/console_bridge" ]; then git clone --depth 1 https://github.com/ros/console_bridge.git "${RootDir}/console_bridge" fi +rm -rf "${RootDir}/console_bridge/build" +mkdir -p "${RootDir}/console_bridge/build" cd "${RootDir}/console_bridge" git reset --hard git fetch origin "0.3.2" && git checkout --force FETCH_HEAD || true @@ -137,6 +160,8 @@ git fetch origin "0.3.2" && git checkout --force FETCH_HEAD || true if [ ! -d "${RootDir}/urdfdom_headers" ]; then git clone --depth 1 https://github.com/ros/urdfdom_headers.git "${RootDir}/urdfdom_headers" fi +rm -rf "${RootDir}/urdfdom_headers/build" +mkdir -p "${RootDir}/urdfdom_headers/build" cd "${RootDir}/urdfdom_headers" git reset --hard git fetch origin "1.0.4" && git checkout --force FETCH_HEAD || true @@ -145,6 +170,8 @@ git fetch origin "1.0.4" && git checkout --force FETCH_HEAD || true if [ ! -d "${RootDir}/urdfdom" ]; then git clone --depth 1 https://github.com/ros/urdfdom.git "${RootDir}/urdfdom" fi +rm -rf "${RootDir}/urdfdom/build" +mkdir -p "${RootDir}/urdfdom/build" cd "${RootDir}/urdfdom" git reset --hard git fetch origin "1.0.3" && git checkout --force FETCH_HEAD || true @@ -154,6 +181,8 @@ git apply --reject --whitespace=fix "${RootDir}/build_tools/patch_deps_unix/urdf if [ ! -d "${RootDir}/cppad" ]; then git clone --depth 1 https://github.com/coin-or/CppAD.git "${RootDir}/cppad" fi +rm -rf "${RootDir}/cppad/build" +mkdir -p "${RootDir}/cppad/build" cd "${RootDir}/cppad" git reset --hard git fetch origin "20240000.2" && git checkout --force FETCH_HEAD || true @@ -162,6 +191,8 @@ git fetch origin "20240000.2" && git checkout --force FETCH_HEAD || true if [ ! -d "${RootDir}/cppadcodegen" ]; then git clone --depth 1 https://github.com/joaoleal/CppADCodeGen.git "${RootDir}/cppadcodegen" fi +rm -rf "${RootDir}/cppadcodegen/build" +mkdir -p "${RootDir}/cppadcodegen/build" cd "${RootDir}/cppadcodegen" git reset --hard git fetch origin "v2.4.3" && git checkout --force FETCH_HEAD || true @@ -170,6 +201,8 @@ git fetch origin "v2.4.3" && git checkout --force FETCH_HEAD || true if [ ! -d "${RootDir}/assimp" ]; then git clone --depth 1 https://github.com/assimp/assimp.git "${RootDir}/assimp" fi +rm -rf "${RootDir}/assimp/build" +mkdir -p "${RootDir}/assimp/build" cd "${RootDir}/assimp" git reset --hard git fetch origin "v5.2.5" && git checkout --force FETCH_HEAD || true @@ -177,22 +210,25 @@ git fetch origin "v5.2.5" && git checkout --force FETCH_HEAD || true ### Checkout hpp-fcl if [ ! -d "${RootDir}/hpp-fcl" ]; then git clone --depth 1 https://github.com/humanoid-path-planner/hpp-fcl.git "${RootDir}/hpp-fcl" - git config --global url."https://".insteadOf git:// fi +rm -rf "${RootDir}/hpp-fcl/build" +mkdir -p "${RootDir}/hpp-fcl/build" cd "${RootDir}/hpp-fcl" git reset --hard -git fetch origin "v2.4.1" && git checkout --force FETCH_HEAD || true +git fetch origin "v2.4.4" && git checkout --force FETCH_HEAD || true git submodule --quiet foreach --recursive git reset --quiet --hard git submodule --quiet update --init --recursive --depth 1 --jobs 8 git apply --reject --whitespace=fix "${RootDir}/build_tools/patch_deps_unix/hppfcl.patch" cd "${RootDir}/hpp-fcl/third-parties/qhull" +rm -rf "${RootDir}/hpp-fcl/third-parties/qhull/build" git fetch origin "v8.0.2" && git checkout --force FETCH_HEAD || true ### Checkout pinocchio and its submodules if [ ! -d "${RootDir}/pinocchio" ]; then git clone --depth 1 https://github.com/stack-of-tasks/pinocchio.git "${RootDir}/pinocchio" - git config --global url."https://".insteadOf git:// fi +rm -rf "${RootDir}/pinocchio/build" +mkdir -p "${RootDir}/pinocchio/build" cd "${RootDir}/pinocchio" git reset --hard git fetch origin "v2.7.0" && git checkout --force FETCH_HEAD || true @@ -238,7 +274,7 @@ else echo "Build type '${BUILD_TYPE}' not supported." >&2 exit 1 fi -CMAKE_CXX_FLAGS_B2="-fPIC -std=c++11" +CMAKE_CXX_FLAGS_B2="-fPIC -std=c++17" if [ "${OSTYPE//[0-9.]/}" == "darwin" ]; then CMAKE_CXX_FLAGS_B2="${CMAKE_CXX_FLAGS_B2} -mmacosx-version-min=${OSX_DEPLOYMENT_TARGET}" fi @@ -269,7 +305,6 @@ mkdir -p "${RootDir}/boost/build" #################################### Build and install eigen3 ########################################## -mkdir -p "${RootDir}/eigen3/build" cd "${RootDir}/eigen3/build" cmake "${RootDir}/eigen3" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -279,7 +314,6 @@ make install -j2 ################################### Build and install eigenpy ########################################## -mkdir -p "${RootDir}/eigenpy/build" cd "${RootDir}/eigenpy/build" cmake "${RootDir}/eigenpy" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -297,7 +331,6 @@ make install -j2 ################################## Build and install tinyxml ########################################### -mkdir -p "${RootDir}/tinyxml/build" cd "${RootDir}/tinyxml/build" cmake "${RootDir}/tinyxml" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -311,7 +344,6 @@ make install -j2 ############################## Build and install console_bridge ######################################## -mkdir -p "${RootDir}/console_bridge/build" cd "${RootDir}/console_bridge/build" cmake "${RootDir}/console_bridge" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -324,7 +356,6 @@ make install -j2 ############################### Build and install urdfdom_headers ###################################### -mkdir -p "${RootDir}/urdfdom_headers/build" cd "${RootDir}/urdfdom_headers/build" cmake "${RootDir}/urdfdom_headers" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -333,7 +364,6 @@ make install -j2 ################################## Build and install urdfdom ########################################### -mkdir -p "${RootDir}/urdfdom/build" cd "${RootDir}/urdfdom/build" cmake "${RootDir}/urdfdom" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -346,7 +376,6 @@ make install -j2 ################################### Build and install CppAD ########################################## -mkdir -p "${RootDir}/cppad/build" cd "${RootDir}/cppad/build" cmake "${RootDir}/cppad" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -359,7 +388,6 @@ make install -j2 ################################### Build and install CppADCodeGen ########################################## -mkdir -p "${RootDir}/cppadcodegen/build" cd "${RootDir}/cppadcodegen/build" cmake "${RootDir}/cppadcodegen" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -374,7 +402,6 @@ make install -j2 # C flag 'HAVE_HIDDEN' must be specified to hide internal symbols of zlib that may not be exposed at # runtime causing undefined symbol error when loading hpp-fcl shared library. -mkdir -p "${RootDir}/assimp/build" cd "${RootDir}/assimp/build" cmake "${RootDir}/assimp" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -404,7 +431,6 @@ cmake "${RootDir}/hpp-fcl/third-parties/qhull" -Wno-dev -DCMAKE_CXX_STANDARD=17 -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 -mkdir -p "${RootDir}/hpp-fcl/build" cd "${RootDir}/hpp-fcl/build" cmake "${RootDir}/hpp-fcl" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ @@ -425,8 +451,7 @@ make install -j2 ################################# Build and install Pinocchio ########################################## -### Build and install pinocchio, finally ! -mkdir -p "${RootDir}/pinocchio/build" +### Build and install pinocchio cd "${RootDir}/pinocchio/build" cmake "${RootDir}/pinocchio" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ diff --git a/build_tools/build_install_deps_windows.ps1 b/build_tools/build_install_deps_windows.ps1 index 0d6c3eb59..f774d980b 100644 --- a/build_tools/build_install_deps_windows.ps1 +++ b/build_tools/build_install_deps_windows.ps1 @@ -23,6 +23,17 @@ if (-not (Test-Path env:BUILD_TYPE)) { ${BUILD_TYPE} = "${env:BUILD_TYPE}" } +### Set the CPU architecture +$ARCHITECTURE = ( & "${PYTHON_EXECUTABLE}" -c "import platform ; print(platform.machine(), end='')" ) +if (("x86_64", "AMD64").contains(${ARCHITECTURE})) { + $ARCHITECTURE = "x64" +} elseif (${ARCHITECTURE} -eq "ARM64") { + $ARCHITECTURE = "ARM64" +} else { + Write-Error "Architecture '${ARCHITECTURE}' not supported." -ErrorAction:Stop +} +Write-Output "Architecture: ${ARCHITECTURE}" + ### Set the default generate if undefined. # The appropriate toolset will be selected automatically by cmake. if (-not (Test-Path env:GENERATOR)) { @@ -81,6 +92,10 @@ if (Test-Path env:Boost_ROOT) { if (-not (Test-Path -PathType Container "$RootDir/boost")) { git clone --depth=1 https://github.com/boostorg/boost.git "$RootDir/boost" } +if (Test-Path -PathType Container "$RootDir/boost/build") { + Remove-Item -Recurse -Force -Path "$RootDir/boost/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/boost/build" Push-Location -Path "$RootDir/boost" git reset --hard git fetch origin "boost-1.78.0" @@ -96,6 +111,10 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/eigen3")) { git clone --depth=1 https://gitlab.com/libeigen/eigen.git "$RootDir/eigen3" } +if (Test-Path -PathType Container "$RootDir/eigen3/build") { + Remove-Item -Recurse -Force -Path "$RootDir/eigen3/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/eigen3/build" Push-Location -Path "$RootDir/eigen3" git fetch origin eeacbd26c8838869a491ee89ab5cf0fe7dac016f git checkout --force FETCH_HEAD @@ -106,13 +125,16 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/eigenpy")) { git clone --depth=1 https://github.com/stack-of-tasks/eigenpy.git "$RootDir/eigenpy" } +if (Test-Path -PathType Container "$RootDir/eigenpy/build") { + Remove-Item -Recurse -Force -Path "$RootDir/eigenpy/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/eigenpy/build" Push-Location -Path "$RootDir/eigenpy" git reset --hard -git fetch origin "v3.3.0" +git fetch origin "v3.4.0" git checkout --force FETCH_HEAD git submodule --quiet foreach --recursive git reset --quiet --hard git submodule --quiet update --init --recursive --depth 1 --jobs 8 -dos2unix "$RootDir/build_tools/patch_deps_windows/eigenpy.patch" git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/eigenpy.patch" Pop-Location @@ -120,6 +142,10 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/tinyxml")) { git clone --depth=1 https://github.com/robotology-dependencies/tinyxml.git "$RootDir/tinyxml" } +if (Test-Path -PathType Container "$RootDir/tinyxml/build") { + Remove-Item -Recurse -Force -Path "$RootDir/tinyxml/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/tinyxml/build" Push-Location -Path "$RootDir/tinyxml" git reset --hard git fetch origin "master" @@ -130,6 +156,10 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/console_bridge")) { git clone --depth=1 https://github.com/ros/console_bridge.git "$RootDir/console_bridge" } +if (Test-Path -PathType Container "$RootDir/console_bridge/build") { + Remove-Item -Recurse -Force -Path "$RootDir/console_bridge/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/console_bridge/build" Push-Location -Path "$RootDir/console_bridge" git reset --hard git fetch origin "1.0.2" @@ -140,6 +170,10 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/urdfdom_headers")) { git clone --depth=1 https://github.com/ros/urdfdom_headers.git "$RootDir/urdfdom_headers" } +if (Test-Path -PathType Container "$RootDir/urdfdom_headers/build") { + Remove-Item -Recurse -Force -Path "$RootDir/urdfdom_headers/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/urdfdom_headers/build" Push-Location -Path "$RootDir/urdfdom_headers" git reset --hard git fetch origin "1.0.5" @@ -150,11 +184,14 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/urdfdom")) { git clone --depth=1 https://github.com/ros/urdfdom.git "$RootDir/urdfdom" } +if (Test-Path -PathType Container "$RootDir/urdfdom/build") { + Remove-Item -Recurse -Force -Path "$RootDir/urdfdom/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/urdfdom/build" Push-Location -Path "$RootDir/urdfdom" git reset --hard git fetch origin "3.0.0" git checkout --force FETCH_HEAD -dos2unix "$RootDir/build_tools/patch_deps_windows/urdfdom.patch" git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/urdfdom.patch" Pop-Location @@ -162,6 +199,10 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/cppad")) { git clone --depth=1 https://github.com/coin-or/CppAD.git "$RootDir/cppad" } +if (Test-Path -PathType Container "$RootDir/cppad/build") { + Remove-Item -Recurse -Force -Path "$RootDir/cppad/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/cppad/build" Push-Location -Path "$RootDir/cppad" git reset --hard git fetch origin "20240000.2" @@ -172,6 +213,10 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/cppadcodegen")) { git clone --depth=1 https://github.com/joaoleal/CppADCodeGen.git "$RootDir/cppadcodegen" } +if (Test-Path -PathType Container "$RootDir/cppadcodegen/build") { + Remove-Item -Recurse -Force -Path "$RootDir/cppadcodegen/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/cppadcodegen/build" Push-Location -Path "$RootDir/cppadcodegen" git reset --hard git fetch origin "v2.4.3" @@ -182,6 +227,10 @@ Pop-Location if (-not (Test-Path -PathType Container "$RootDir/assimp")) { git clone --depth=1 https://github.com/assimp/assimp.git "$RootDir/assimp" } +if (Test-Path -PathType Container "$RootDir/assimp/build") { + Remove-Item -Recurse -Force -Path "$RootDir/assimp/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/assimp/build" Push-Location -Path "$RootDir/assimp" git reset --hard git fetch origin "v5.2.5" @@ -191,17 +240,23 @@ Pop-Location ### Checkout hpp-fcl, then apply some patches if (-not (Test-Path -PathType Container "$RootDir/hpp-fcl")) { git clone --depth=1 https://github.com/humanoid-path-planner/hpp-fcl.git "$RootDir/hpp-fcl" - git config --global url."https://".insteadOf git:// } +if (Test-Path -PathType Container "$RootDir/hpp-fcl/build") { + Remove-Item -Recurse -Force -Path "$RootDir/hpp-fcl/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/hpp-fcl/build" Push-Location -Path "$RootDir/hpp-fcl" git reset --hard -git fetch origin "v2.4.1" +git fetch origin "v2.4.4" git checkout --force FETCH_HEAD git submodule --quiet foreach --recursive git reset --quiet --hard git submodule --quiet update --init --recursive --depth 1 --jobs 8 -dos2unix "$RootDir/build_tools/patch_deps_windows/hppfcl.patch" git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/hppfcl.patch" Pop-Location +if (Test-Path -PathType Container "$RootDir/hpp-fcl/third-parties/qhull/build") { + Remove-Item -Recurse -Force -Path "$RootDir/hpp-fcl/third-parties/qhull/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/hpp-fcl/third-parties/qhull/build" Push-Location -Path "$RootDir/hpp-fcl/third-parties/qhull" git fetch origin "v8.0.2" git checkout --force FETCH_HEAD @@ -210,15 +265,17 @@ Pop-Location ### Checkout pinocchio and its submodules, then apply some patches if (-not (Test-Path -PathType Container "$RootDir/pinocchio")) { git clone --depth=1 https://github.com/stack-of-tasks/pinocchio.git "$RootDir/pinocchio" - git config --global url."https://".insteadOf git:// } +if (Test-Path -PathType Container "$RootDir/pinocchio/build") { + Remove-Item -Recurse -Force -Path "$RootDir/pinocchio/build" +} +New-Item -ItemType "directory" -Force -Path "$RootDir/pinocchio/build" Push-Location -Path "$RootDir/pinocchio" git reset --hard git fetch origin "v2.7.0" git checkout --force FETCH_HEAD git submodule --quiet foreach --recursive git reset --quiet --hard git submodule --quiet update --init --recursive --depth 1 --jobs 8 -dos2unix "$RootDir/build_tools/patch_deps_windows/pinocchio.patch" git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/pinocchio.patch" Pop-Location @@ -259,36 +316,38 @@ if (${BUILD_TYPE} -eq "Release") { } else { Write-Error "Build type '${BUILD_TYPE}' not supported." -ErrorAction:Stop } -if (-not (Test-Path -PathType Container "$RootDir/boost/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/boost/build" + +if (${ARCHITECTURE} -eq "x64") { + $ArchiB2 = "x86" +} elseif (${ARCHITECTURE} -eq "ARM64") { + $ArchiB2 = "arm" } # Compiling everything with static linkage except Boost::Python ./b2.exe --prefix="$InstallDir" --build-dir="$RootDir/boost/build" ` --with-chrono --with-timer --with-date_time --with-system --with-test ` --with-filesystem --with-atomic --with-serialization --with-thread ` - --build-type=minimal architecture=x86 address-model=64 ` - threading=single @DebugOptionsB2 --layout=system --lto=off ` - link=static runtime-link=shared ` - cxxflags="-std=c++11 ${CMAKE_CXX_FLAGS}" linkflags="${CMAKE_SHARED_LINKER_FLAGS}" ` + --build-type=minimal --layout=system --lto=off ` + architecture=${ArchiB2} address-model=64 @DebugOptionsB2 ` + threading=single link=static runtime-link=shared ` + cxxflags="-std=c++11 ${CMAKE_CXX_FLAGS}" ` + linkflags="${CMAKE_SHARED_LINKER_FLAGS}" ` variant="$BuildTypeB2" install -q -d0 -j2 ./b2.exe --prefix="$InstallDir" --build-dir="$RootDir/boost/build" ` --with-python ` - --build-type=minimal architecture=x86 address-model=64 ` - threading=single @DebugOptionsB2 --layout=system --lto=off ` - link=shared runtime-link=shared ` - cxxflags="-std=c++11 ${CMAKE_CXX_FLAGS}" linkflags="${CMAKE_SHARED_LINKER_FLAGS}" ` + --build-type=minimal --layout=system --lto=off ` + architecture=${ArchiB2} address-model=64 @DebugOptionsB2 ` + threading=single link=shared runtime-link=shared ` + cxxflags="-std=c++11 ${CMAKE_CXX_FLAGS}" ` + linkflags="${CMAKE_SHARED_LINKER_FLAGS}" ` variant="$BuildTypeB2" install -q -d0 -j2 Pop-Location #################################### Build and install eigen3 ########################################## -if (-not (Test-Path -PathType Container "$RootDir/eigen3/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/eigen3/build" -} Push-Location -Path "$RootDir/eigen3/build" -cmake "$RootDir/eigen3" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/eigen3" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DBUILD_TESTING=OFF -DEIGEN_BUILD_PKGCONFIG=OFF cmake --build . --target INSTALL --config "${BUILD_TYPE}" --parallel 2 @@ -296,11 +355,8 @@ Pop-Location ################################### Build and install eigenpy ########################################## -if (-not (Test-Path -PathType Container "$RootDir/eigenpy/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/eigenpy/build" -} Push-Location -Path "$RootDir/eigenpy/build" -cmake "$RootDir/eigenpy" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/eigenpy" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=OFF -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" ` @@ -315,11 +371,8 @@ Pop-Location ################################## Build and install tinyxml ########################################### -if (-not (Test-Path -PathType Container "$RootDir/tinyxml/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/tinyxml/build" -} Push-Location -Path "$RootDir/tinyxml/build" -cmake "$RootDir/tinyxml" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/tinyxml" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DBUILD_SHARED_LIBS=OFF -DCMAKE_SHARED_LINKER_FLAGS="${CMAKE_SHARED_LINKER_FLAGS}" ` @@ -329,11 +382,8 @@ Pop-Location ############################## Build and install console_bridge ######################################## -if (-not (Test-Path -PathType Container "$RootDir/console_bridge/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/console_bridge/build" -} Push-Location -Path "$RootDir/console_bridge/build" -cmake "$RootDir/console_bridge" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/console_bridge" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DBUILD_SHARED_LIBS=OFF -DCMAKE_SHARED_LINKER_FLAGS="${CMAKE_SHARED_LINKER_FLAGS}" ` @@ -343,22 +393,16 @@ Pop-Location ############################### Build and install urdfdom_headers ###################################### -if (-not (Test-Path -PathType Container "$RootDir/urdfdom_headers/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/urdfdom_headers/build" -} Push-Location -Path "$RootDir/urdfdom_headers/build" -cmake "$RootDir/urdfdom_headers" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/urdfdom_headers" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" cmake --build . --target INSTALL --config "${BUILD_TYPE}" --parallel 2 Pop-Location ################################## Build and install urdfdom ########################################### -if (-not (Test-Path -PathType Container "$RootDir/urdfdom/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/urdfdom/build" -} Push-Location -Path "$RootDir/urdfdom/build" -cmake "$RootDir/urdfdom" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/urdfdom" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DCMAKE_FIND_PACKAGE_PREFER_CONFIG=ON -DBUILD_TESTING=OFF -DBUILD_SHARED_LIBS=OFF ` @@ -369,11 +413,8 @@ Pop-Location ################################### Build and install CppAD ########################################## -if (-not (Test-Path -PathType Container "$RootDir/cppad/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/cppad/build" -} Push-Location -Path "$RootDir/cppad/build" -cmake "$RootDir/cppad" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/cppad" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DCMAKE_SHARED_LINKER_FLAGS="${CMAKE_SHARED_LINKER_FLAGS}" -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS}" @@ -382,11 +423,8 @@ Pop-Location ################################### Build and install CppADCodeGen ########################################## -if (-not (Test-Path -PathType Container "$RootDir/cppadcodegen/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/cppadcodegen/build" -} Push-Location -Path "$RootDir/cppadcodegen/build" -cmake "$RootDir/cppadcodegen" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/cppadcodegen" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DGOOGLETEST_GIT=ON -DCMAKE_SHARED_LINKER_FLAGS="${CMAKE_SHARED_LINKER_FLAGS}" ` @@ -396,11 +434,8 @@ Pop-Location ###################################### Build and install assimp ######################################## -if (-not (Test-Path -PathType Container "$RootDir/assimp/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/assimp/build" -} Push-Location -Path "$RootDir/assimp/build" -cmake "$RootDir/assimp" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/assimp" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DASSIMP_BUILD_ASSIMP_TOOLS=OFF -DASSIMP_BUILD_ZLIB=OFF -DASSIMP_BUILD_TESTS=OFF ` @@ -417,7 +452,7 @@ Pop-Location # add the desired flag at the end of CMAKE_CXX_FLAGS ("/MT", "/MD"...). It will take precedence over # any existing flag if any. Push-Location -Path "$RootDir/hpp-fcl/third-parties/qhull/build" -cmake "$RootDir/hpp-fcl/third-parties/qhull" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/hpp-fcl/third-parties/qhull" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON ` -DCMAKE_SHARED_LINKER_FLAGS="${CMAKE_SHARED_LINKER_FLAGS}" ` @@ -426,11 +461,8 @@ cmake --build . --target INSTALL --config "${BUILD_TYPE}" --parallel 2 Pop-Location ### Build hpp-fcl -if (-not (Test-Path -PathType Container "$RootDir/hpp-fcl/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/hpp-fcl/build" -} Push-Location -Path "$RootDir/hpp-fcl/build" -cmake "$RootDir/hpp-fcl" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/hpp-fcl" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=OFF -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" ` @@ -447,11 +479,8 @@ Pop-Location ################################ Build and install Pinocchio ########################################## ### Build and install pinocchio, finally ! -if (-not (Test-Path -PathType Container "$RootDir/pinocchio/build")) { - New-Item -ItemType "directory" -Force -Path "$RootDir/pinocchio/build" -} Push-Location -Path "$RootDir/pinocchio/build" -cmake "$RootDir/pinocchio" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM=x64 ` +cmake "$RootDir/pinocchio" -Wno-dev -G "${GENERATOR}" -DCMAKE_GENERATOR_PLATFORM="${ARCHITECTURE}" ` -DCMAKE_POLICY_DEFAULT_CMP0091=NEW -DCMAKE_MSVC_RUNTIME_LIBRARY="MultiThreaded$<$:Debug>DLL" ` -DCMAKE_CXX_STANDARD=17 -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="$InstallDir" ` -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=OFF -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" ` diff --git a/build_tools/cmake/base.cmake b/build_tools/cmake/base.cmake index 4454b6fa6..13043f179 100644 --- a/build_tools/cmake/base.cmake +++ b/build_tools/cmake/base.cmake @@ -1,16 +1,6 @@ # Minimum version required cmake_minimum_required(VERSION 3.12.4) -# MSVC runtime library flags are defined by 'CMAKE_MSVC_RUNTIME_LIBRARY' -if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.15.7) - cmake_policy(SET CMP0091 NEW) -endif() - -# Set find_package strategy to look for both upper-case and case-preserved variables -if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.27.0) - cmake_policy(SET CMP0144 NEW) -endif() - # Forces GCC/Clang compilers to enable color diagnostics. # CMake versions 3.24 and below do not support this option, so we have # to invoke the color diagnostics flags manually. @@ -25,17 +15,14 @@ endif() # Check if network is available before compiling external projects if(WIN32) - find_program(HAS_PING "ping") -endif() -if(HAS_PING) + set(BUILD_OFFLINE 0) +else() unset(BUILD_OFFLINE) unset(BUILD_OFFLINE CACHE) execute_process(COMMAND bash -c "if ping -q -c 1 -W 1 8.8.8.8 ; then echo 0; else echo 1; fi" OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE BUILD_OFFLINE) -else() - set(BUILD_OFFLINE 0) endif() if(${BUILD_OFFLINE}) message(WARNING "No internet connection. Not building external projects.") diff --git a/build_tools/cmake/setupPython.cmake b/build_tools/cmake/setupPython.cmake index 6d0eae51b..ad053e700 100644 --- a/build_tools/cmake/setupPython.cmake +++ b/build_tools/cmake/setupPython.cmake @@ -55,7 +55,8 @@ else() OUTPUT_VARIABLE HAS_NO_WRITE_PERMISSION_ON_PYTHON_SYS_SITELIB) endif() -set(PYTHON_INSTALL_FLAGS " --no-warn-script-location --prefer-binary ") +# Libraries not distributed as wheel must be explicitly whitelisted via '--no-binary'. +set(PYTHON_INSTALL_FLAGS " --no-warn-script-location --only-binary :all:") if(${HAS_NO_WRITE_PERMISSION_ON_PYTHON_SYS_SITELIB}) set(PYTHON_INSTALL_FLAGS "${PYTHON_INSTALL_FLAGS} --user ") message(STATUS "No right on Python system site-packages: ${Python_SYS_SITELIB}.\n" @@ -113,9 +114,10 @@ if(${Boost_MINOR_VERSION} GREATER_EQUAL 67) set(Boost_USE_STATIC_LIBS OFF) set(Boost_LIB_PREFIX "") unset(Boost_LIBRARIES) - find_package(Boost REQUIRED COMPONENTS - "python${Python_VERSION_MAJOR}${Python_VERSION_MINOR}" - "numpy${Python_VERSION_MAJOR}${Python_VERSION_MINOR}") + find_package( + Boost REQUIRED COMPONENTS + "python${Python_VERSION_MAJOR}${Python_VERSION_MINOR}" + "numpy${Python_VERSION_MAJOR}${Python_VERSION_MINOR}") set(BOOST_PYTHON_LIB "${Boost_LIBRARIES}") unset(Boost_LIBRARIES) if(WIN32) @@ -154,13 +156,29 @@ function(deployPythonPackage) endfunction() function(deployPythonPackageDevelop) - # The input arguments are [PKG_NAME...] - foreach(PKG_NAME IN LISTS ARGN) - install(CODE "execute_process(COMMAND ${Python_EXECUTABLE} -m pip install ${PYTHON_INSTALL_FLAGS} -e . + # The input arguments are [PKG_NAME...], ALLOW_FAILURE + + # Extract the output arguments (see `buildPythonWheel`) + set(ARGS ${ARGN}) + list(LENGTH ARGS NUM_ARGS) + if(${NUM_ARGS} LESS 2) + message(FATAL_ERROR "Please specify at least one PKG_NAME and ALLOW_FAILURE.") + endif() + list(GET ARGS -1 ALLOW_FAILURE) + list(REMOVE_AT ARGS -1) + + # Loop over all packages sequentially + foreach(PKG_NAME IN LISTS ARGS) + install(CODE "cmake_policy(SET CMP0012 NEW) + execute_process(COMMAND ${Python_EXECUTABLE} -m pip install ${PYTHON_INSTALL_FLAGS} -e . WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/${PKG_NAME} RESULT_VARIABLE RETURN_CODE) - if(NOT RETURN_CODE EQUAL 0) - message(FATAL_ERROR \"Python installation of '${PKG_NAME}' failed.\") + if(RETURN_CODE AND NOT RETURN_CODE EQUAL 0) + if (${ALLOW_FAILURE}) + message(WARNING \"Python installation of '${PKG_NAME}' failed.\") + else() + message(FATAL_ERROR \"Python installation of '${PKG_NAME}' failed.\") + endif() endif()") endforeach() endfunction() diff --git a/build_tools/easy_install_deps_ubuntu.sh b/build_tools/easy_install_deps_ubuntu.sh index 95fc366f6..f3215a4fc 100755 --- a/build_tools/easy_install_deps_ubuntu.sh +++ b/build_tools/easy_install_deps_ubuntu.sh @@ -59,7 +59,7 @@ echo "-- Python writable site-packages: ${PYTHON_SITELIB}" apt update && \ apt install -y python3-pip && \ ${SUDO_CMD} python3 -m pip install setuptools wheel "pip>=20.3" && \ -${SUDO_CMD} python3 -m pip install "numpy>=1.21,<2.0" "numba>=0.54.0" +${SUDO_CMD} python3 -m pip install "numpy>=1.24,<2.0" "numba>=0.54.0" # Install standard linux utilities apt install -y gnupg curl wget build-essential cmake doxygen graphviz pandoc @@ -81,8 +81,8 @@ fi # Note that `apt-get` is used instead of `apt` because it supports wildcard in package names apt-mark unhold "robotpkg-py3*-eigenpy" "robotpkg-py3*-hpp-fcl" "robotpkg-py3*-pinocchio" apt-get install -y --allow-downgrades --allow-unauthenticated \ - robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=2.4.1 robotpkg-pinocchio=2.7.0 \ - robotpkg-py3*-eigenpy=3.3.0 robotpkg-py3*-hpp-fcl=2.4.1 robotpkg-py3*-pinocchio=2.7.0 + robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=2.4.4 robotpkg-pinocchio=2.7.0 \ + robotpkg-py3*-eigenpy=3.4.0 robotpkg-py3*-hpp-fcl=2.4.4 robotpkg-py3*-pinocchio=2.7.0 apt-mark hold "robotpkg-py3*-eigenpy" "robotpkg-py3*-hpp-fcl" "robotpkg-py3*-pinocchio" # Add openrobots libraries to python packages search path diff --git a/build_tools/patch_deps_unix/boost-python.patch b/build_tools/patch_deps_unix/boost-python.patch index 8b8231154..5bed1e8cb 100644 --- a/build_tools/patch_deps_unix/boost-python.patch +++ b/build_tools/patch_deps_unix/boost-python.patch @@ -9,3 +9,22 @@ diff --git a/src/object/enum.cpp b/src/object/enum.cpp | Py_TPFLAGS_BASETYPE, /* tp_flags */ 0, /* tp_doc */ 0, /* tp_traverse */ +diff --git a/src/numpy/dtype.cpp b/src/numpy/dtype.cpp +index 88a20a27..da30d192 100644 +--- a/src/numpy/dtype.cpp ++++ b/src/numpy/dtype.cpp +@@ -98,7 +98,13 @@ python::detail::new_reference dtype::convert(object const & arg, bool align) + return python::detail::new_reference(reinterpret_cast(obj)); + } + +-int dtype::get_itemsize() const { return reinterpret_cast(ptr())->elsize;} ++int dtype::get_itemsize() const { ++#if NPY_ABI_VERSION < 0x02000000 ++ return reinterpret_cast(ptr())->elsize; ++#else ++ return PyDataType_ELSIZE(reinterpret_cast(ptr())); ++#endif ++} + + bool equivalent(dtype const & a, dtype const & b) { + // On Windows x64, the behaviour described on diff --git a/build_tools/patch_deps_windows/boost-python.patch b/build_tools/patch_deps_windows/boost-python.patch index 8b8231154..5bed1e8cb 100644 --- a/build_tools/patch_deps_windows/boost-python.patch +++ b/build_tools/patch_deps_windows/boost-python.patch @@ -9,3 +9,22 @@ diff --git a/src/object/enum.cpp b/src/object/enum.cpp | Py_TPFLAGS_BASETYPE, /* tp_flags */ 0, /* tp_doc */ 0, /* tp_traverse */ +diff --git a/src/numpy/dtype.cpp b/src/numpy/dtype.cpp +index 88a20a27..da30d192 100644 +--- a/src/numpy/dtype.cpp ++++ b/src/numpy/dtype.cpp +@@ -98,7 +98,13 @@ python::detail::new_reference dtype::convert(object const & arg, bool align) + return python::detail::new_reference(reinterpret_cast(obj)); + } + +-int dtype::get_itemsize() const { return reinterpret_cast(ptr())->elsize;} ++int dtype::get_itemsize() const { ++#if NPY_ABI_VERSION < 0x02000000 ++ return reinterpret_cast(ptr())->elsize; ++#else ++ return PyDataType_ELSIZE(reinterpret_cast(ptr())); ++#endif ++} + + bool equivalent(dtype const & a, dtype const & b) { + // On Windows x64, the behaviour described on diff --git a/build_tools/patch_deps_windows/eigenpy.patch b/build_tools/patch_deps_windows/eigenpy.patch index ad6534f74..ae5648d7e 100644 --- a/build_tools/patch_deps_windows/eigenpy.patch +++ b/build_tools/patch_deps_windows/eigenpy.patch @@ -4,7 +4,7 @@ diff --git a/CMakeLists.txt b/CMakeLists.txt @@ -206,7 +206,7 @@ src/optional.cpp src/version.cpp) - + -add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) @@ -27,7 +27,7 @@ diff --git a/cmake/boost.cmake b/cmake/boost.cmake + endif() + set(Boost_USE_STATIC_LIBS ON) string(TOUPPER ${BOOST_PYTHON_NAME} UPPERCOMPONENT) - + list(APPEND LOGGING_WATCHED_VARIABLES Boost_${UPPERCOMPONENT}_FOUND @@ -192,7 +200,6 @@ string(REPLACE "_" "." Boost_SHORT_VERSION ${Boost_LIB_VERSION}) diff --git a/build_tools/patch_deps_windows/hppfcl.patch b/build_tools/patch_deps_windows/hppfcl.patch index 0a1508269..070637785 100644 --- a/build_tools/patch_deps_windows/hppfcl.patch +++ b/build_tools/patch_deps_windows/hppfcl.patch @@ -16,7 +16,7 @@ diff --git a/CMakeLists.txt b/CMakeLists.txt + ADD_PROJECT_DEPENDENCY(Boost REQUIRED system) - find_package(Boost REQUIRED COMPONENTS system) endif(BUILD_PYTHON_INTERFACE) - + if(Boost_VERSION_STRING VERSION_LESS 1.81) # Default C++ version should be C++11 CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE) @@ -51,7 +51,7 @@ diff --git a/CMakeLists.txt b/CMakeLists.txt @@ -283,7 +274,6 @@ include/hpp/fcl/timings.h ) - + -add_subdirectory(doc) add_subdirectory(src) if (BUILD_PYTHON_INTERFACE) @@ -70,7 +70,7 @@ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt @@ -179,6 +178,7 @@ ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES) ADD_HEADER_GROUP(PROJECT_HEADERS_FULL_PATH) - + +ADD_PROJECT_DEPENDENCY(assimp REQUIRED) MODERNIZE_TARGET_LINK_LIBRARIES(${LIBRARY_NAME} SCOPE PRIVATE TARGETS assimp::assimp @@ -101,7 +101,7 @@ diff --git a/cmake/boost.cmake b/cmake/boost.cmake + endif() + set(Boost_USE_STATIC_LIBS ON) string(TOUPPER ${BOOST_PYTHON_NAME} UPPERCOMPONENT) - + list(APPEND LOGGING_WATCHED_VARIABLES Boost_${UPPERCOMPONENT}_FOUND @@ -192,7 +200,6 @@ string(REPLACE "_" "." Boost_SHORT_VERSION ${Boost_LIB_VERSION}) diff --git a/build_tools/patch_deps_windows/pinocchio.patch b/build_tools/patch_deps_windows/pinocchio.patch index c5a5a5ed8..75205d0c3 100644 --- a/build_tools/patch_deps_windows/pinocchio.patch +++ b/build_tools/patch_deps_windows/pinocchio.patch @@ -6,7 +6,7 @@ diff --git a/CMakeLists.txt b/CMakeLists.txt SET_BOOST_DEFAULT_OPTIONS() EXPORT_BOOST_DEFAULT_OPTIONS() -ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS ${BOOST_REQUIRED_COMPONENTS}) - + IF(Boost_VERSION_STRING VERSION_LESS 1.81) IF(BUILD_WITH_URDF_SUPPORT AND ${urdfdom_VERSION} VERSION_GREATER "0.4.2") @@ -163,6 +162,9 @@ @@ -49,7 +49,7 @@ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -17,7 +17,7 @@ ${PROJECT_NAME}_CORE_PUBLIC_HEADERS) - + # Create target libpinocchio.so -ADD_LIBRARY(${PROJECT_NAME} SHARED +ADD_LIBRARY(${PROJECT_NAME} @@ -73,7 +73,7 @@ diff --git a/cmake/boost.cmake b/cmake/boost.cmake + endif() + set(Boost_USE_STATIC_LIBS ON) string(TOUPPER ${BOOST_PYTHON_NAME} UPPERCOMPONENT) - + list(APPEND LOGGING_WATCHED_VARIABLES Boost_${UPPERCOMPONENT}_FOUND @@ -192,7 +200,6 @@ string(REPLACE "_" "." Boost_SHORT_VERSION ${Boost_LIB_VERSION}) diff --git a/build_tools/stub_gen.py b/build_tools/stub_gen.py index ad5d5c2dc..c4df9a2cf 100644 --- a/build_tools/stub_gen.py +++ b/build_tools/stub_gen.py @@ -30,7 +30,7 @@ def _issubclass(cls, class_or_tuple, /): def extract_boost_python_signature(args: str) -> str: - find_optional_args = re.search('\[(.*?)\]$', args) + find_optional_args = re.search(r'\[(.*?)\]$', args) if find_optional_args: optional_args = find_optional_args.group(1) nominal_args = args.replace("[" + optional_args + "]", "") @@ -53,7 +53,7 @@ def extract_boost_python_signature(args: str) -> str: if nominal_args: for k, arg in enumerate(nominal_args): - type_name = re.search('\((.*?)\)', arg).group(1) + type_name = re.search(r'\((.*?)\)', arg).group(1) # `bp::object` can be basically anything, so switching to 'Any'. if type_name == "object": type_name = "typing.Any" @@ -70,7 +70,7 @@ def extract_boost_python_signature(args: str) -> str: if optional_args and True: for k, arg in enumerate(optional_args): main_arg, *optional_args = map(str.strip, arg.split('=', 1)) - type_name = re.search('\((.*?)\)', main_arg).group(1) + type_name = re.search(r'\((.*?)\)', main_arg).group(1) if type_name == "object": type_name = "typing.Any" diff --git a/core/examples/double_pendulum/double_pendulum.cc b/core/examples/double_pendulum/double_pendulum.cc index 818ba35ba..ea11d4c04 100644 --- a/core/examples/double_pendulum/double_pendulum.cc +++ b/core/examples/double_pendulum/double_pendulum.cc @@ -55,6 +55,7 @@ int main(int /* argc */, char * /* argv */[]) const auto dataPath = jiminySrcPath / "data/toys_models"; const auto urdfPath = dataPath / "double_pendulum/double_pendulum.urdf"; const auto outputDirPath = std::filesystem::temp_directory_path(); + std::cout << "Output directory: " << outputDirPath << std::endl; // ===================================================================== // ============ Instantiate and configure the simulation =============== diff --git a/core/examples/external_project/double_pendulum.cc b/core/examples/external_project/double_pendulum.cc index 981579859..fa40433db 100644 --- a/core/examples/external_project/double_pendulum.cc +++ b/core/examples/external_project/double_pendulum.cc @@ -12,6 +12,7 @@ #include "jiminy/core/utilities/helpers.h" #include "jiminy/core/io/file_device.h" #include "jiminy/core/hardware/abstract_sensor.h" +#include "jiminy/core/hardware/basic_sensors.h" #include "jiminy/core/hardware/basic_motors.h" #include "jiminy/core/control/controller_functor.h" #include "jiminy/core/engine/engine.h" @@ -37,7 +38,7 @@ void internalDynamics(double t, { } -bool callback(double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v) +bool callback() { return true; } @@ -52,6 +53,7 @@ int main(int argc, char * argv[]) const std::filesystem::path filePath(__FILE__); const auto urdfPath = filePath.parent_path() / "double_pendulum.urdf"; const auto outputDirPath = std::filesystem::temp_directory_path(); + std::cout << "Output directory: " << outputDirPath << std::endl; // ===================================================================== // ============ Instantiate and configure the simulation =============== @@ -61,7 +63,6 @@ int main(int argc, char * argv[]) Timer timer; // Instantiate and configuration the robot - std::vector motorJointNames{"SecondPendulumJoint"}; auto robot = std::make_shared(); GenericConfig modelOptions = robot->getModelOptions(); GenericConfig & jointsOptions = boost::get(modelOptions.at("joints")); @@ -69,16 +70,21 @@ int main(int argc, char * argv[]) boost::get(jointsOptions.at("velocityLimitFromUrdf")) = true; robot->setModelOptions(modelOptions); robot->initialize(urdfPath.string(), false, {}); - for (const std::string & jointName : motorJointNames) - { - auto motor = std::make_shared(jointName); - robot->attachMotor(motor); - motor->initialize(jointName); - } + + // Attach motor and encoder to the robot + auto motor = std::make_shared("motor"); + robot->attachMotor(motor); + motor->initialize("SecondPendulumJoint"); + auto sensor = std::make_shared("encoder"); + robot->attachSensor(sensor); + sensor->initialize("SecondPendulumJoint"); + + // Print encoder sensor index + std::cout << "Encoder sensor index: " << sensor->getIndex() << std::endl; // Instantiate the controller - robot->setController( - std::make_shared>(computeCommand, internalDynamics)); + auto controller = std::make_shared>(computeCommand, internalDynamics); + robot->setController(controller); // Instantiate the engine Engine engine{}; @@ -97,9 +103,14 @@ int main(int argc, char * argv[]) // Run simulation timer.tic(); - engine.simulate(tf, q0, v0, std::nullopt, callback); + engine.simulate(tf, q0, v0, std::nullopt, false, callback); std::cout << "Simulation: " << timer.toc() << "ms" << std::endl; + // Print final encoder data for debug + std::cout << "Final encoder data: " + << controller->sensorMeasurements_[EncoderSensor::type_].getAll().transpose() + << std::endl; + // Write the log file std::vector fieldnames; std::shared_ptr logDataPtr = engine.getLog(); diff --git a/core/include/jiminy/core/constraints/abstract_constraint.h b/core/include/jiminy/core/constraints/abstract_constraint.h index d1badbb5f..9f0608144 100644 --- a/core/include/jiminy/core/constraints/abstract_constraint.h +++ b/core/include/jiminy/core/constraints/abstract_constraint.h @@ -19,7 +19,7 @@ namespace jiminy friend Model; public: - DISABLE_COPY(AbstractConstraintBase) + JIMINY_DISABLE_COPY(AbstractConstraintBase) public: explicit AbstractConstraintBase() = default; @@ -99,7 +99,7 @@ namespace jiminy }; template - class JIMINY_DLLAPI AbstractConstraintTpl : public AbstractConstraintBase + class JIMINY_TEMPLATE_DLLAPI AbstractConstraintTpl : public AbstractConstraintBase { public: auto shared_from_this() { return shared_from(this); } @@ -108,7 +108,7 @@ namespace jiminy const std::string & getType() const { return type_; } public: - static const std::string type_; + static const std::string JIMINY_STATIC_MEMBER_DLLAPI type_; }; } diff --git a/core/include/jiminy/core/constraints/distance_constraint.h b/core/include/jiminy/core/constraints/distance_constraint.h index 6b6a99a37..e71ab99b4 100644 --- a/core/include/jiminy/core/constraints/distance_constraint.h +++ b/core/include/jiminy/core/constraints/distance_constraint.h @@ -12,10 +12,17 @@ namespace jiminy { class Model; + class DistanceConstraint; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) + template<> + const std::string JIMINY_DLLAPI AbstractConstraintTpl::type_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractConstraintTpl; + class JIMINY_DLLAPI DistanceConstraint : public AbstractConstraintTpl { public: - DISABLE_COPY(DistanceConstraint) + JIMINY_DISABLE_COPY(DistanceConstraint) auto shared_from_this() { return shared_from(this); } diff --git a/core/include/jiminy/core/constraints/frame_constraint.h b/core/include/jiminy/core/constraints/frame_constraint.h index bf03ef707..d58a6d41d 100644 --- a/core/include/jiminy/core/constraints/frame_constraint.h +++ b/core/include/jiminy/core/constraints/frame_constraint.h @@ -11,12 +11,19 @@ namespace jiminy { class Model; + class FrameConstraint; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) + template<> + const std::string JIMINY_DLLAPI AbstractConstraintTpl::type_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractConstraintTpl; + /// \brief This class implements the constraint for fixing a given frame wrt /// world. class JIMINY_DLLAPI FrameConstraint : public AbstractConstraintTpl { public: - DISABLE_COPY(FrameConstraint) + JIMINY_DISABLE_COPY(FrameConstraint) auto shared_from_this() { return shared_from(this); } diff --git a/core/include/jiminy/core/constraints/joint_constraint.h b/core/include/jiminy/core/constraints/joint_constraint.h index 065e0126b..6b2f6dba5 100644 --- a/core/include/jiminy/core/constraints/joint_constraint.h +++ b/core/include/jiminy/core/constraints/joint_constraint.h @@ -12,10 +12,17 @@ namespace jiminy { class Model; + class JointConstraint; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) + template<> + const std::string JIMINY_DLLAPI AbstractConstraintTpl::type_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractConstraintTpl; + class JIMINY_DLLAPI JointConstraint : public AbstractConstraintTpl { public: - DISABLE_COPY(JointConstraint) + JIMINY_DISABLE_COPY(JointConstraint) auto shared_from_this() { return shared_from(this); } diff --git a/core/include/jiminy/core/constraints/sphere_constraint.h b/core/include/jiminy/core/constraints/sphere_constraint.h index 76ef16b02..9011a5203 100644 --- a/core/include/jiminy/core/constraints/sphere_constraint.h +++ b/core/include/jiminy/core/constraints/sphere_constraint.h @@ -12,6 +12,13 @@ namespace jiminy { class Model; + class SphereConstraint; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) + template<> + const std::string JIMINY_DLLAPI AbstractConstraintTpl::type_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractConstraintTpl; + /// \brief Class constraining a sphere to roll without slipping on a flat plane. /// /// \details Given a frame to represent the sphere center, this class constrains it to move @@ -19,7 +26,7 @@ namespace jiminy class JIMINY_DLLAPI SphereConstraint : public AbstractConstraintTpl { public: - DISABLE_COPY(SphereConstraint) + JIMINY_DISABLE_COPY(SphereConstraint) auto shared_from_this() { return shared_from(this); } diff --git a/core/include/jiminy/core/constraints/wheel_constraint.h b/core/include/jiminy/core/constraints/wheel_constraint.h index e45fa7bb4..47b666e68 100644 --- a/core/include/jiminy/core/constraints/wheel_constraint.h +++ b/core/include/jiminy/core/constraints/wheel_constraint.h @@ -12,6 +12,13 @@ namespace jiminy { class Model; + class WheelConstraint; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) + template<> + const std::string JIMINY_DLLAPI AbstractConstraintTpl::type_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractConstraintTpl; + /// \brief Class constraining a wheel to roll without slipping on a flat plane. /// /// \details Given a frame to represent the wheel center, this class constrains it to move @@ -19,7 +26,7 @@ namespace jiminy class JIMINY_DLLAPI WheelConstraint : public AbstractConstraintTpl { public: - DISABLE_COPY(WheelConstraint) + JIMINY_DISABLE_COPY(WheelConstraint) auto shared_from_this() { return shared_from(this); } diff --git a/core/include/jiminy/core/control/abstract_controller.h b/core/include/jiminy/core/control/abstract_controller.h index df61d0ebc..e090514d8 100644 --- a/core/include/jiminy/core/control/abstract_controller.h +++ b/core/include/jiminy/core/control/abstract_controller.h @@ -11,7 +11,7 @@ namespace jiminy { /// \brief Namespace of the telemetry object. - inline constexpr std::string_view CONTROLLER_TELEMETRY_NAMESPACE{"HighLevelController"}; + inline constexpr std::string_view CONTROLLER_TELEMETRY_NAMESPACE{"controller"}; class TelemetrySender; class TelemetryData; @@ -46,7 +46,7 @@ namespace jiminy }; public: - DISABLE_COPY(AbstractController) + JIMINY_DISABLE_COPY(AbstractController) public: explicit AbstractController() noexcept; @@ -60,6 +60,15 @@ namespace jiminy /// \param[in] robot Robot virtual void initialize(std::weak_ptr robot); + /// \brief Register a constant (so-called invariant) to the telemetry. + /// + /// \details The user is responsible to convert it as a byte array (eg `std::string`), + /// either using `toString` for arithmetic types or `saveToBinary` complex types. + /// + /// \param[in] name Name of the constant. + /// \param[in] value Constant to add to the telemetry. + void registerConstant(const std::string_view & name, const std::string & value); + /// \brief Dynamically registered a scalar variable to the telemetry. It is the main entry /// point for a user to log custom variables. /// @@ -88,13 +97,6 @@ namespace jiminy const Eigen::Ref, 0, Eigen::Stride> & values); - /// \brief Register a constant float64 to the telemetry. - /// - /// \param[in] name Name of the variable. - /// \param[in] values Variable to add to the telemetry - template - void registerConstant(const std::string_view & name, const T & value); - /// \brief Remove all variables dynamically registered to the telemetry. /// /// \details Note that one must reset Jiminy Engine for this to take effect. @@ -126,9 +128,6 @@ namespace jiminy const Eigen::VectorXd & v, Eigen::VectorXd & uCustom) = 0; - /// \brief Dictionary with the parameters of the controller. - GenericConfig getOptions() const noexcept; - /// \brief Set the configuration options of the controller. /// /// \details Note that one must reset Jiminy Engine for this to take effect. @@ -136,6 +135,9 @@ namespace jiminy /// \param[in] controllerOptions Dictionary with the parameters of the controller. void setOptions(const GenericConfig & controllerOptions); + /// \brief Dictionary with the parameters of the controller. + const GenericConfig & getOptions() const noexcept; + /// \brief Configure the telemetry of the controller. /// /// \details This method connects the controller-specific telemetry sender to a given diff --git a/core/include/jiminy/core/control/abstract_controller.hxx b/core/include/jiminy/core/control/abstract_controller.hxx index bcee42589..baded74ef 100644 --- a/core/include/jiminy/core/control/abstract_controller.hxx +++ b/core/include/jiminy/core/control/abstract_controller.hxx @@ -7,40 +7,13 @@ namespace jiminy { - /// \brief Register a constant to the telemetry. - /// - /// \param[in] fieldnames Name of the variable. - /// \param[in] values Variable to add to the telemetry. - template - void AbstractController::registerConstant(const std::string_view & fieldname, const T & value) - { - // Delayed variable registration (Taken into account by 'configureTelemetry') - - if (isTelemetryConfigured_) - { - THROW_ERROR(bad_control_flow, - "Telemetry already initialized. Impossible to register new variables."); - } - - // Check in local cache before. - auto constantIt = std::find_if(constantRegistry_.begin(), - constantRegistry_.end(), - [&fieldname](const auto & element) - { return element.first == fieldname; }); - if (constantIt != constantRegistry_.end()) - { - THROW_ERROR(bad_control_flow, "Constant already registered."); - } - constantRegistry_.emplace_back(fieldname, toString(value)); - } - template void AbstractController::registerVariable(const std::string_view & name, const T & value) { if (isTelemetryConfigured_) { - THROW_ERROR(bad_control_flow, - "Telemetry already initialized. Impossible to register new variables."); + JIMINY_THROW(bad_control_flow, + "Telemetry already initialized. Impossible to register new variables."); } // Check in local cache before. @@ -50,7 +23,7 @@ namespace jiminy { return element.first == name; }); if (variableIt != variableRegistry_.end()) { - THROW_ERROR(bad_control_flow, "Variable already registered."); + JIMINY_THROW(bad_control_flow, "Variable '", name, "' already registered."); } variableRegistry_.emplace_back(name, &value); } diff --git a/core/include/jiminy/core/control/controller_functor.h b/core/include/jiminy/core/control/controller_functor.h index 55a445348..a5089acea 100644 --- a/core/include/jiminy/core/control/controller_functor.h +++ b/core/include/jiminy/core/control/controller_functor.h @@ -27,7 +27,7 @@ namespace jiminy class FunctionalController : public AbstractController { public: - DISABLE_COPY(FunctionalController) + JIMINY_DISABLE_COPY(FunctionalController) public: /// \remark A valid 'callable' is a function pointer, functor or lambda with signature: diff --git a/core/include/jiminy/core/control/controller_functor.hxx b/core/include/jiminy/core/control/controller_functor.hxx index 195fd2397..f6ff5fb81 100644 --- a/core/include/jiminy/core/control/controller_functor.hxx +++ b/core/include/jiminy/core/control/controller_functor.hxx @@ -31,7 +31,7 @@ namespace jiminy { if (!getIsInitialized()) { - THROW_ERROR(bad_control_flow, "Controller not initialized."); + JIMINY_THROW(bad_control_flow, "Controller not initialized."); } commandFun_(t, q, v, sensorMeasurements_, command); @@ -43,10 +43,10 @@ namespace jiminy { if (!getIsInitialized()) { - THROW_ERROR(bad_control_flow, "Controller not initialized."); + JIMINY_THROW(bad_control_flow, "Controller not initialized."); } // Sensor data are already up-to-date internalDynamicsFun_(t, q, v, sensorMeasurements_, uCustom); } -} \ No newline at end of file +} diff --git a/core/include/jiminy/core/engine/engine.h b/core/include/jiminy/core/engine/engine.h index cf9522fc4..aec19621a 100644 --- a/core/include/jiminy/core/engine/engine.h +++ b/core/include/jiminy/core/engine/engine.h @@ -12,16 +12,16 @@ namespace jiminy { - inline constexpr std::string_view ENGINE_TELEMETRY_NAMESPACE{"HighLevelController"}; + inline constexpr std::string_view ENGINE_TELEMETRY_NAMESPACE{""}; - enum class JIMINY_DLLAPI ContactModelType : uint8_t + enum class ContactModelType : uint8_t { UNSUPPORTED = 0, SPRING_DAMPER = 1, CONSTRAINT = 2 }; - enum class JIMINY_DLLAPI ConstraintSolverType : uint8_t + enum class ConstraintSolverType : uint8_t { UNSUPPORTED = 0, PGS = 1 // Projected Gauss-Seidel @@ -159,7 +159,7 @@ namespace jiminy struct JIMINY_DLLAPI RobotData { public: - DISABLE_COPY(RobotData) + JIMINY_DISABLE_COPY(RobotData) /* Must move all definitions in source files to avoid compilation failure due to incomplete destructor for objects managed by `unique_ptr` member variable with MSVC compiler. @@ -497,7 +497,7 @@ namespace jiminy }; public: - DISABLE_COPY(Engine) + JIMINY_DISABLE_COPY(Engine) public: explicit Engine() noexcept; @@ -661,10 +661,22 @@ namespace jiminy const ImpulseForceVector & getImpulseForces(const std::string & robotName) const; const ProfileForceVector & getProfileForces(const std::string & robotName) const; - GenericConfig getOptions() const noexcept; void setOptions(const GenericConfig & engineOptions); + const GenericConfig & getOptions() const noexcept; + /// \brief Set the options of the engine and all the robots. + /// + /// \param[in] simulationOptions Dictionary gathering all the options. See + /// `getSimulationOptions` for details about the hierarchy. + void setSimulationOptions(const GenericConfig & simulationOptions); + /// \brief Get the options of the engine and all the robots. + /// + /// \details The key 'engine' maps to the engine options, whereas `robot.name` maps to the + /// individual options of each robot for multi-robot simulations, 'robot' for + /// single-robot simulations. + GenericConfig getSimulationOptions() const noexcept; + bool getIsTelemetryConfigured() const; - std::shared_ptr & getRobot(const std::string & robotName); + std::shared_ptr getRobot(const std::string & robotName); std::ptrdiff_t getRobotIndex(const std::string & robotName) const; const RobotState & getRobotState(const std::string & robotName) const; const StepperState & getStepperState() const; @@ -829,7 +841,8 @@ namespace jiminy protected: bool isTelemetryConfigured_{false}; bool isSimulationRunning_{false}; - GenericConfig engineOptionsGeneric_{}; + mutable bool areSimulationOptionsRefreshed_{false}; + mutable GenericConfig simulationOptionsGeneric_{}; PCG32 generator_; private: diff --git a/core/include/jiminy/core/fwd.h b/core/include/jiminy/core/fwd.h index c92c866d3..3cd45f272 100644 --- a/core/include/jiminy/core/fwd.h +++ b/core/include/jiminy/core/fwd.h @@ -194,10 +194,10 @@ namespace jiminy const Eigen::Vector2d & /* xy */, double & /* height */, Eigen::Vector3d & /* normal */)>; // Flexible joints - struct FlexibleJointData + struct FlexibilityJointConfig { // FIXME: Replace by default spaceship operator `<=>` when moving to C++20. - inline bool operator==(const FlexibleJointData & other) const noexcept + inline bool operator==(const FlexibilityJointConfig & other) const noexcept { return (this->frameName == other.frameName && this->stiffness == other.stiffness && this->damping == other.damping && this->inertia == other.inertia); @@ -209,8 +209,9 @@ namespace jiminy Eigen::Vector3d inertia{}; }; - using FlexibilityConfig = std::vector; + using FlexibilityConfig = std::vector; + // Generic configuration holder using GenericConfig = std::unordered_map) { static const Eigen::IOFormat k_heavy_fmt( Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - return var.format(k_heavy_fmt); + sstr << var.format(k_heavy_fmt); } else { - return var; + sstr << var; } }; - ((sstr << format(std::forward(args))), ...); + (format(std::forward(args)), ...); return sstr.str(); } } diff --git a/core/include/jiminy/core/hardware/abstract_motor.h b/core/include/jiminy/core/hardware/abstract_motor.h index 18af87a3a..4af2fda12 100644 --- a/core/include/jiminy/core/hardware/abstract_motor.h +++ b/core/include/jiminy/core/hardware/abstract_motor.h @@ -48,6 +48,8 @@ namespace jiminy config["commandLimit"] = 0.0; config["enableArmature"] = false; config["armature"] = 0.0; + config["enableBacklash"] = false; + config["backlash"] = 0.0; return config; }; @@ -61,6 +63,8 @@ namespace jiminy const double commandLimit; const bool enableArmature; const double armature; + const bool enableBacklash; + const double backlash; AbstractMotorOptions(const GenericConfig & options) : mechanicalReduction(boost::get(options.at("mechanicalReduction"))), @@ -68,13 +72,15 @@ namespace jiminy commandLimitFromUrdf(boost::get(options.at("commandLimitFromUrdf"))), commandLimit(boost::get(options.at("commandLimit"))), enableArmature(boost::get(options.at("enableArmature"))), - armature(boost::get(options.at("armature"))) + armature(boost::get(options.at("armature"))), + enableBacklash(boost::get(options.at("enableBacklash"))), + backlash(boost::get(options.at("backlash"))) { } }; public: - DISABLE_COPY(AbstractMotorBase) + JIMINY_DISABLE_COPY(AbstractMotorBase) public: /// \param[in] name Name of the motor. @@ -96,7 +102,7 @@ namespace jiminy virtual void resetAll(); /// \brief Configuration options of the motor. - GenericConfig getOptions() const noexcept; + const GenericConfig & getOptions() const noexcept; /// \brief Actual effort of the motor at the current time. double get() const; @@ -144,6 +150,9 @@ namespace jiminy /// \brief Rotor inertia of the motor. double getArmature() const; + /// \brief Backlash of the transmission. + double getBacklash() const; + /// \brief Request the motor to update its actual effort based of the input data. /// /// \details It assumes that the internal state of the robot is consistent with the input @@ -190,7 +199,8 @@ namespace jiminy /// /// \details This method must be called before initializing the sensor. void attach(std::weak_ptr robot, - std::function notifyRobot, + std::function notifyRobot, MotorSharedStorage * sharedStorage); /// \brief Detach the sensor from the robot. @@ -216,14 +226,17 @@ namespace jiminy Eigen::Index jointVelocityIndex_{0}; double commandLimit_{0.0}; double armature_{0.0}; + double backlash_{0.0}; private: /// \brief Name of the motor. std::string name_; /// \brief Flag to determine whether the motor is attached to a robot. bool isAttached_{false}; - /// \brief Notify the robot that the configuration of the sensors have changed. - std::function notifyRobot_{}; + /// \brief Whether the robot must be notified. + bool mustNotifyRobot_{false}; + /// \brief Notify the robot that the configuration of the motor have changed. + std::function notifyRobot_{}; /// \brief Shared data between every motors associated with the robot. MotorSharedStorage * sharedStorage_{nullptr}; }; diff --git a/core/include/jiminy/core/hardware/abstract_sensor.h b/core/include/jiminy/core/hardware/abstract_sensor.h index 51486e617..14bbf7bbd 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.h +++ b/core/include/jiminy/core/hardware/abstract_sensor.h @@ -103,7 +103,7 @@ namespace jiminy }; public: - DISABLE_COPY(AbstractSensorBase) + JIMINY_DISABLE_COPY(AbstractSensorBase) public: /// \param[in] name Name of the sensor @@ -165,7 +165,7 @@ namespace jiminy virtual void setOptionsAll(const GenericConfig & sensorOptions) = 0; /// \brief Configuration options of the sensor. - GenericConfig getOptions() const noexcept; + const GenericConfig & getOptions() const noexcept; template void set(const Eigen::MatrixBase & value); @@ -314,10 +314,10 @@ namespace jiminy }; template - class AbstractSensorTpl : public AbstractSensorBase + class JIMINY_TEMPLATE_DLLAPI AbstractSensorTpl : public AbstractSensorBase { public: - DISABLE_COPY(AbstractSensorTpl) + JIMINY_DISABLE_COPY(AbstractSensorTpl) public: using AbstractSensorBase::AbstractSensorBase; @@ -358,9 +358,8 @@ namespace jiminy public: /* Be careful, the static variables must be const since the 'static' keyword binds all the sensors together, even if they are associated to complete separated robots. */ - static const std::string type_; - static const std::vector fieldnames_; - static const bool areFieldnamesGrouped_; + static const std::string JIMINY_STATIC_MEMBER_DLLAPI type_; + static const std::vector JIMINY_STATIC_MEMBER_DLLAPI fieldnames_; protected: std::size_t sensorIndex_{0}; diff --git a/core/include/jiminy/core/hardware/abstract_sensor.hxx b/core/include/jiminy/core/hardware/abstract_sensor.hxx index f3d4337fb..ab5d7479f 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.hxx +++ b/core/include/jiminy/core/hardware/abstract_sensor.hxx @@ -16,15 +16,15 @@ namespace jiminy { if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Sensor not attached to any robot."); + JIMINY_THROW(bad_control_flow, "Sensor not attached to any robot."); } auto robot = robot_.lock(); if (!robot || robot->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot is locked, probably because a simulation is running. " - "Please stop it before setting sensor value manually."); + JIMINY_THROW(bad_control_flow, + "Robot is locked, probably because a simulation is running. " + "Please stop it before setting sensor value manually."); } get() = value; @@ -49,7 +49,7 @@ namespace jiminy // Make sure the sensor is not already attached if (isAttached_) { - THROW_ERROR( + JIMINY_THROW( bad_control_flow, "Sensor already attached to a robot. Please 'detach' method before attaching it."); } @@ -57,7 +57,7 @@ namespace jiminy // Make sure the robot still exists if (robot.expired()) { - THROW_ERROR(bad_control_flow, "Robot pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Robot pointer expired or unset."); } // Copy references to the robot and shared data @@ -98,7 +98,7 @@ namespace jiminy if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Sensor not attached to any robot."); + JIMINY_THROW(bad_control_flow, "Sensor not attached to any robot."); } // Remove associated col in the shared data buffers @@ -148,33 +148,48 @@ namespace jiminy template void AbstractSensorTpl::resetAll(uint32_t seed) { - // Make sure all the sensors are attached to a robot + // Make sure the sensor is attached to a robot + if (!isAttached_) + { + JIMINY_THROW(bad_control_flow, "Sensor not attached to any robot."); + } + + // Make sure all the sensors are attached to a robot and initialized for (AbstractSensorBase * sensor : sharedStorage_->sensors_) { if (!sensor->isAttached_) { - THROW_ERROR(bad_control_flow, - "Sensor '", - sensor->name_, - "' of type '", - type_, - "' not attached to any robot."); + JIMINY_THROW(bad_control_flow, + "Sensor '", + sensor->name_, + "' of type '", + type_, + "' not attached to any robot."); + } + if (!sensor->isInitialized_) + { + JIMINY_THROW(bad_control_flow, + "Sensor '", + sensor->name_, + "' of type '", + type_, + "' not initialized."); } } // Make sure the robot still exists if (robot_.expired()) { - THROW_ERROR(bad_control_flow, "Robot has been deleted. Impossible to reset sensors."); + JIMINY_THROW(bad_control_flow, "Robot has been deleted. Impossible to reset sensors."); } // Make sure that no simulation is already running auto robot = robot_.lock(); if (robot && robot->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before resetting sensors."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before resetting sensors."); } // Clear the shared data buffers @@ -223,7 +238,7 @@ namespace jiminy { if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Sensor not attached to any robot."); + JIMINY_THROW(bad_control_flow, "Sensor not attached to any robot."); } for (AbstractSensorBase * sensor : sharedStorage_->sensors_) @@ -259,14 +274,7 @@ namespace jiminy template std::string AbstractSensorTpl::getTelemetryName() const { - if (areFieldnamesGrouped_) - { - return addCircumfix(name_, getType(), {}, TELEMETRY_FIELDNAME_DELIMITER); - } - else - { - return name_; - } + return addCircumfix(name_, getType(), {}, TELEMETRY_FIELDNAME_DELIMITER); } template @@ -371,7 +379,7 @@ namespace jiminy { if (idxLeft < 0) { - THROW_ERROR(std::runtime_error, "No data old enough is available."); + JIMINY_THROW(std::runtime_error, "No data old enough is available."); } else if (baseSensorOptions_->delayInterpolationOrder == 0) { @@ -389,8 +397,8 @@ namespace jiminy } else { - THROW_ERROR(not_implemented_error, - "`delayInterpolationOrder` must be either 0 or 1."); + JIMINY_THROW(not_implemented_error, + "`delayInterpolationOrder` must be either 0 or 1."); } } else @@ -443,7 +451,7 @@ namespace jiminy { if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Sensor not attached to any robot."); + JIMINY_THROW(bad_control_flow, "Sensor not attached to any robot."); } /* Make sure at least the requested delay plus the maximum time step is available to handle diff --git a/core/include/jiminy/core/hardware/basic_sensors.h b/core/include/jiminy/core/hardware/basic_sensors.h index a338b5964..a3a216b34 100644 --- a/core/include/jiminy/core/hardware/basic_sensors.h +++ b/core/include/jiminy/core/hardware/basic_sensors.h @@ -10,6 +10,15 @@ namespace jiminy { class Robot; + class ImuSensor; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) + template<> + const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; + template<> + const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; + class JIMINY_DLLAPI ImuSensor final : public AbstractSensorTpl { public: @@ -41,12 +50,14 @@ namespace jiminy Eigen::Matrix3d sensorRotationBiasInv_{Eigen::Matrix3d::Identity()}; }; + class ContactSensor; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) template<> - const std::string AbstractSensorTpl::type_; + const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> - const std::vector AbstractSensorTpl::fieldnames_; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_; + const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; class JIMINY_DLLAPI ContactSensor final : public AbstractSensorTpl { @@ -77,12 +88,14 @@ namespace jiminy std::size_t contactIndex_{0}; }; + class ForceSensor; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) template<> - const std::string AbstractSensorTpl::type_; - template<> - const std::vector AbstractSensorTpl::fieldnames_; + const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_; + const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; class JIMINY_DLLAPI ForceSensor final : public AbstractSensorTpl { @@ -116,12 +129,14 @@ namespace jiminy pinocchio::Force f_{}; }; + class EncoderSensor; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) template<> - const std::string AbstractSensorTpl::type_; - template<> - const std::vector AbstractSensorTpl::fieldnames_; + const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_; + const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; class JIMINY_DLLAPI EncoderSensor final : public AbstractSensorTpl { @@ -153,12 +168,14 @@ namespace jiminy JointModelType jointType_{JointModelType::UNSUPPORTED}; }; + class EffortSensor; +#if defined EXPORT_SYMBOLS || (!defined _WIN32 && !defined __CYGWIN__) template<> - const std::string AbstractSensorTpl::type_; + const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> - const std::vector AbstractSensorTpl::fieldnames_; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_; + const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; +#endif + template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; class JIMINY_DLLAPI EffortSensor final : public AbstractSensorTpl { @@ -187,13 +204,6 @@ namespace jiminy std::string motorName_{}; std::size_t motorIndex_{0}; }; - - template<> - const std::string AbstractSensorTpl::type_; - template<> - const std::vector AbstractSensorTpl::fieldnames_; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_; } #endif // end of JIMINY_BASIC_SENSORS_H \ No newline at end of file diff --git a/core/include/jiminy/core/macros.h b/core/include/jiminy/core/macros.h index 8f053c953..2edc78a55 100644 --- a/core/include/jiminy/core/macros.h +++ b/core/include/jiminy/core/macros.h @@ -12,7 +12,7 @@ // ************************************* Generic utilities ************************************* // -#define DISABLE_COPY(className) \ +#define JIMINY_DISABLE_COPY(className) \ className(const className & other) = delete; \ className & operator=(const className & other) = delete; @@ -22,17 +22,35 @@ // On Microsoft Windows, use dllimport and dllexport to tag symbols # define JIMINY_DLLIMPORT __declspec(dllimport) # define JIMINY_DLLEXPORT __declspec(dllexport) +# define JIMINY_TEMPLATE_DLLIMPORT +# define JIMINY_TEMPLATE_DLLEXPORT +# define JIMINY_STATIC_MEMBER_DLLIMPORT +# define JIMINY_STATIC_MEMBER_DLLEXPORT JIMINY_DLLEXPORT +# define JIMINY_TEMPLATE_INSTANTIATION_DLLIMPORT JIMINY_DLLIMPORT +# define JIMINY_TEMPLATE_INSTANTIATION_DLLEXPORT JIMINY_DLLEXPORT #else -// On Linux, for GCC >= 4, tag symbols using GCC extension +// On Linux, tag symbols using de-facto standard visibility attribute extension # define JIMINY_DLLIMPORT __attribute__((visibility("default"))) # define JIMINY_DLLEXPORT __attribute__((visibility("default"))) +# define JIMINY_TEMPLATE_DLLIMPORT JIMINY_DLLIMPORT +# define JIMINY_TEMPLATE_DLLEXPORT JIMINY_DLLEXPORT +# define JIMINY_STATIC_MEMBER_DLLIMPORT +# define JIMINY_STATIC_MEMBER_DLLEXPORT +# define JIMINY_TEMPLATE_INSTANTIATION_DLLIMPORT +# define JIMINY_TEMPLATE_INSTANTIATION_DLLEXPORT #endif // Define DLLAPI to import or export depending on whether one is building or using the library #ifdef EXPORT_SYMBOLS # define JIMINY_DLLAPI JIMINY_DLLEXPORT +# define JIMINY_TEMPLATE_DLLAPI JIMINY_TEMPLATE_DLLEXPORT +# define JIMINY_STATIC_MEMBER_DLLAPI JIMINY_STATIC_MEMBER_DLLEXPORT +# define JIMINY_TEMPLATE_INSTANTIATION_DLLAPI JIMINY_TEMPLATE_INSTANTIATION_DLLEXPORT #else # define JIMINY_DLLAPI JIMINY_DLLIMPORT +# define JIMINY_TEMPLATE_DLLAPI JIMINY_TEMPLATE_DLLIMPORT +# define JIMINY_STATIC_MEMBER_DLLAPI JIMINY_STATIC_MEMBER_DLLIMPORT +# define JIMINY_TEMPLATE_INSTANTIATION_DLLAPI JIMINY_TEMPLATE_INSTANTIATION_DLLIMPORT #endif // ******************************* Error and warnings utilities ******************************** // @@ -63,7 +81,7 @@ namespace jiminy::internal /* ANSI escape codes is used here as a cross-platform way to color text. For reference, see: https://solarianprogrammer.com/2019/04/08/c-programming-ansi-escape-codes-windows-macos-linux-terminals/ */ -#define THROW_ERROR(exception, ...) \ +#define JIMINY_THROW(exception, ...) \ throw exception( \ toString(jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION), \ "(" FILE_LINE "):\n", \ diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h index 80f0cb1b8..de5efce07 100644 --- a/core/include/jiminy/core/robot/model.h +++ b/core/include/jiminy/core/robot/model.h @@ -16,6 +16,7 @@ namespace jiminy inline constexpr std::string_view JOINT_PREFIX_BASE{"current"}; inline constexpr std::string_view FREE_FLYER_NAME{"Freeflyer"}; inline constexpr std::string_view FLEXIBLE_JOINT_SUFFIX{"Flexibility"}; + inline constexpr std::string_view BACKLASH_JOINT_SUFFIX{"Backlash"}; class AbstractConstraintBase; class FrameConstraint; @@ -25,7 +26,7 @@ namespace jiminy using ConstraintMap = static_map_t>; - enum class JIMINY_DLLAPI ConstraintNodeType : uint8_t + enum class ConstraintNodeType : uint8_t { BOUNDS_JOINTS = 0, CONTACT_FRAMES = 1, @@ -135,7 +136,6 @@ namespace jiminy virtual GenericConfig getDefaultJointOptions() { GenericConfig config; - config["enablePositionLimit"] = true; config["positionLimitFromUrdf"] = true; config["positionLimitMin"] = Eigen::VectorXd{}; config["positionLimitMax"] = Eigen::VectorXd{}; @@ -154,7 +154,7 @@ namespace jiminy config["massBodiesBiasStd"] = 0.0; config["centerOfMassPositionBodiesBiasStd"] = 0.0; config["relativePositionBodiesBiasStd"] = 0.0; - config["enableFlexibleModel"] = true; + config["enableFlexibility"] = true; config["flexibilityConfig"] = FlexibilityConfig{}; return config; @@ -182,10 +182,8 @@ namespace jiminy struct JointOptions { - const bool enablePositionLimit; const bool positionLimitFromUrdf; - /// \brief Min position limit of all the rigid joints, ie without freeflyer and - /// flexibility joints if any. + /// \brief Min position limit of all the mechanical joints of the theoretical model. const Eigen::VectorXd positionLimitMin; const Eigen::VectorXd positionLimitMax; const bool enableVelocityLimit; @@ -193,7 +191,6 @@ namespace jiminy const Eigen::VectorXd velocityLimit; JointOptions(const GenericConfig & options) : - enablePositionLimit{boost::get(options.at("enablePositionLimit"))}, positionLimitFromUrdf{boost::get(options.at("positionLimitFromUrdf"))}, positionLimitMin{boost::get(options.at("positionLimitMin"))}, positionLimitMax{boost::get(options.at("positionLimitMax"))}, @@ -210,7 +207,7 @@ namespace jiminy const double massBodiesBiasStd; const double centerOfMassPositionBodiesBiasStd; const double relativePositionBodiesBiasStd; - const bool enableFlexibleModel; + const bool enableFlexibility; const FlexibilityConfig flexibilityConfig; DynamicsOptions(const GenericConfig & options) : @@ -220,7 +217,7 @@ namespace jiminy boost::get(options.at("centerOfMassPositionBodiesBiasStd"))}, relativePositionBodiesBiasStd{ boost::get(options.at("relativePositionBodiesBiasStd"))}, - enableFlexibleModel{boost::get(options.at("enableFlexibleModel"))}, + enableFlexibility{boost::get(options.at("enableFlexibility"))}, flexibilityConfig{boost::get(options.at("flexibilityConfig"))} { } @@ -251,7 +248,7 @@ namespace jiminy }; public: - DISABLE_COPY(Model) + JIMINY_DISABLE_COPY(Model) public: explicit Model() noexcept; @@ -274,7 +271,8 @@ namespace jiminy void addFrame(const std::string & frameName, const std::string & parentBodyName, const pinocchio::SE3 & framePlacement); - void removeFrame(const std::string & frameName); + void removeFrames(const std::vector & frameNames); + void addCollisionBodies(const std::vector & bodyNames, bool ignoreMeshes = false); void removeCollisionBodies(std::vector frameNames = {}); // Copy on purpose @@ -320,9 +318,8 @@ namespace jiminy /// \param[in] v Joint velocity. void computeConstraints(const Eigen::VectorXd & q, const Eigen::VectorXd & v); - // Copy on purpose - void setOptions(GenericConfig modelOptions); - GenericConfig getOptions() const noexcept; + void setOptions(const GenericConfig & modelOptions); + const GenericConfig & getOptions() const noexcept; /// \remark This method does not have to be called manually before running a simulation. /// The Engine is taking care of it. @@ -345,12 +342,14 @@ namespace jiminy const std::vector> & getCollisionPairIndices() const; const std::vector & getContactFrameIndices() const; - const std::vector & getRigidJointNames() const; - const std::vector & getRigidJointIndices() const; - const std::vector & getRigidJointPositionIndices() const; - const std::vector & getRigidJointVelocityIndices() const; - const std::vector & getFlexibleJointNames() const; - const std::vector & getFlexibleJointIndices() const; + const std::vector & getMechanicalJointNames() const; + const std::vector & getMechanicalJointIndices() const; + const std::vector & getMechanicalJointPositionIndices() const; + const std::vector & getMechanicalJointVelocityIndices() const; + const std::vector & getFlexibilityJointNames() const; + const std::vector & getFlexibilityJointIndices() const; + const std::vector & getBacklashJointNames() const; + const std::vector & getBacklashJointIndices() const; const Eigen::VectorXd & getPositionLimitMin() const; const Eigen::VectorXd & getPositionLimitMax() const; @@ -361,24 +360,28 @@ namespace jiminy const std::vector & getLogAccelerationFieldnames() const; const std::vector & getLogForceExternalFieldnames() const; - void getFlexiblePositionFromRigid(const Eigen::VectorXd & qRigid, - Eigen::VectorXd & qFlex) const; - void getRigidPositionFromFlexible(const Eigen::VectorXd & qFlex, - Eigen::VectorXd & qRigid) const; - void getFlexibleVelocityFromRigid(const Eigen::VectorXd & vRigid, - Eigen::VectorXd & vFlex) const; - void getRigidVelocityFromFlexible(const Eigen::VectorXd & vFlex, - Eigen::VectorXd & vRigid) const; + void getExtendedPositionFromTheoretical(const Eigen::VectorXd & qTheoretical, + Eigen::VectorXd & qExtended) const; + void getExtendedVelocityFromTheoretical(const Eigen::VectorXd & vTheoretical, + Eigen::VectorXd & vExtended) const; + void getTheoreticalPositionFromExtended(const Eigen::VectorXd & qExtended, + Eigen::VectorXd & qTheoretical) const; + void getTheoreticalVelocityFromExtended(const Eigen::VectorXd & vExtended, + Eigen::VectorXd & vTheoretical) const; protected: - void generateModelFlexible(); - void generateModelBiased(const uniform_random_bit_generator_ref & g); + void generateModelExtended(const uniform_random_bit_generator_ref & g); + + virtual void initializeExtendedModel(); + void addFlexibilityJointsToExtendedModel(); + void addBiasedToExtendedModel(const uniform_random_bit_generator_ref & g); void addFrame(const std::string & frameName, const std::string & parentBodyName, const pinocchio::SE3 & framePlacement, const pinocchio::FrameType & frameType); - void removeFrames(const std::vector & frameNames); + void removeFrames(const std::vector & frameNames, + const std::vector & filter); void addConstraint(const std::string & constraintName, const std::shared_ptr & constraint, @@ -395,13 +398,13 @@ namespace jiminy virtual void refreshProxies(); public: - pinocchio::Model pinocchioModelOrig_{}; + pinocchio::Model pinocchioModelTh_{}; pinocchio::Model pinocchioModel_{}; - pinocchio::GeometryModel collisionModelOrig_{}; + pinocchio::GeometryModel collisionModelTh_{}; pinocchio::GeometryModel collisionModel_{}; - pinocchio::GeometryModel visualModelOrig_{}; + pinocchio::GeometryModel visualModelTh_{}; pinocchio::GeometryModel visualModel_{}; - mutable pinocchio::Data pinocchioDataOrig_{}; + mutable pinocchio::Data pinocchioDataTh_{}; mutable pinocchio::Data pinocchioData_{}; mutable pinocchio::GeometryData collisionData_{}; mutable pinocchio::GeometryData visualData_{}; @@ -427,22 +430,25 @@ namespace jiminy std::vector> collisionPairIndices_{}; /// \brief Indices of the contact frames in the frame list of the robot. std::vector contactFrameIndices_{}; - /// \brief Name of the actual joints of the robot, not taking into account the freeflyer. - std::vector rigidJointNames_{}; - /// \brief Index of the actual joints in the pinocchio robot. - std::vector rigidJointIndices_{}; - /// \brief All the indices of the actual joints in the configuration vector of the robot - /// (ie including all the degrees of freedom). - std::vector rigidJointPositionIndices_{}; - /// \brief All the indices of the actual joints in the velocity vector of the robot (ie - /// including all the degrees of freedom). - std::vector rigidJointVelocityIndices_{}; - /// \brief Name of the flexibility joints of the robot regardless of whether the - /// flexibilities are enabled. - std::vector flexibleJointNames_{}; - /// \brief Index of the flexibility joints in the pinocchio robot regardless of whether the - /// flexibilities are enabled. - std::vector flexibleJointIndices_{}; + /// \brief Name of the mechanical joints of the robot, ie all joints of the theoretical + /// excluding freeflyer if any. + std::vector mechanicalJointNames_{}; + /// \brief Index of the mechanical joints in the pinocchio robot. + std::vector mechanicalJointIndices_{}; + /// \brief All the indices of the mechanical joints in the configuration vector of the + /// robot, ie including all their respective degrees of freedom. + std::vector mechanicalJointPositionIndices_{}; + /// \brief All the indices of the mechanical joints in the velocity vector of the robot, + /// ie including all their respective degrees of freedom. + std::vector mechanicalJointVelocityIndices_{}; + /// \brief Name of the flexibility joints of the robot if enabled. + std::vector flexibilityJointNames_{}; + /// \brief Index of the flexibility joints in the pinocchio robot if enabled. + std::vector flexibilityJointIndices_{}; + /// \brief Name of the backlash joints of the robot if enabled. + std::vector backlashJointNames_{}; + /// \brief Index of the backlash joints in the pinocchio robot if enabled. + std::vector backlashJointIndices_{}; /// \brief Store constraints. ConstraintTree constraints_{}; @@ -466,9 +472,8 @@ namespace jiminy std::vector logForceExternalFieldnames_{}; private: - pinocchio::Model pncModelFlexibleOrig_{}; - /// \brief Vector of joints acceleration corresponding to a copy of data.a - temporary - /// buffer for computing constraints. + /// \brief Vector of joints acceleration corresponding to a copy of data.a. + // Used for computing constraints as a temporary buffer. MotionVector jointSpatialAccelerations_{}; Eigen::Index nq_{0}; diff --git a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h index 2b535a4d8..4148a8e78 100644 --- a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h +++ b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h @@ -509,7 +509,7 @@ namespace jiminy::pinocchio_overload It is always the case in theory but not sure in practice because of rounding errors. */ if ((data.Dinv.array() < 0.0).any()) { - THROW_ERROR(std::runtime_error, "Inertia matrix not positive definite."); + JIMINY_THROW(std::runtime_error, "Inertia matrix not positive definite."); } /* Compute sDUiJt := sqrt(D)^-1 * U^-1 * J.T diff --git a/core/include/jiminy/core/robot/robot.h b/core/include/jiminy/core/robot/robot.h index 74583fda5..b4a3f0572 100644 --- a/core/include/jiminy/core/robot/robot.h +++ b/core/include/jiminy/core/robot/robot.h @@ -20,13 +20,26 @@ namespace jiminy class JIMINY_DLLAPI Robot : public Model { + public: + virtual GenericConfig getDefaultRobotOptions() + { + GenericConfig config; + config["model"] = getDefaultModelOptions(); + config["motors"] = GenericConfig{}; + config["sensors"] = GenericConfig{}; + config["controller"] = GenericConfig{}; + config["telemetry"] = GenericConfig{}; + + return config; + }; + public: using MotorVector = std::vector>; using SensorVector = std::vector>; using SensorTree = std::unordered_map; public: - DISABLE_COPY(Robot) + JIMINY_DISABLE_COPY(Robot) public: /// \param[in] name Name of the Robot. @@ -52,7 +65,7 @@ namespace jiminy std::weak_ptr getMotor(const std::string & motorName) const; const MotorVector & getMotors() const; void detachMotor(const std::string & motorName); - void detachMotors(std::vector motorsNames = {}); + void detachMotors(std::vector motorNames = {}); void attachSensor(std::shared_ptr sensor); std::shared_ptr getSensor(const std::string & sensorType, const std::string & sensorName); @@ -89,20 +102,9 @@ namespace jiminy const std::string & sensorType, const std::string & sensorName) const; void setOptions(const GenericConfig & robotOptions); - GenericConfig getOptions() const noexcept; + const GenericConfig & getOptions() const noexcept; void setModelOptions(const GenericConfig & modelOptions); - GenericConfig getModelOptions() const; - void setMotorsOptions(const GenericConfig & motorsOptions); - GenericConfig getMotorsOptions() const; - void setSensorsOptions(const GenericConfig & sensorsOptions); - GenericConfig getSensorsOptions() const; - void setControllerOptions(const GenericConfig & controllerOptions); - GenericConfig getControllerOptions() const; - void setTelemetryOptions(const GenericConfig & telemetryOptions); - GenericConfig getTelemetryOptions() const; - - void dumpOptions(const std::string & filepath) const; - void loadOptions(const std::string & filepath); + const GenericConfig & getModelOptions() const noexcept; /// \remark This method does not have to be called manually before running a simulation. /// The Engine is taking care of it. @@ -132,7 +134,12 @@ namespace jiminy protected: void refreshMotorProxies(); void refreshSensorProxies(); - virtual void refreshProxies() override; + void refreshProxies() override; + + void initializeExtendedModel() override; + + private: + void detachMotor(const std::string & motorName, bool triggerReset); protected: bool isTelemetryConfigured_{false}; @@ -141,7 +148,10 @@ namespace jiminy MotorVector motors_{}; /// \brief Sensors attached to the robot. SensorTree sensors_{}; - std::unordered_map telemetryOptions_{}; + /// \brief Whether the robot options are guaranteed to be up-to-date. + mutable bool areRobotOptionsRefreshed_{false}; + /// \brief Dictionary with the parameters of the robot. + mutable GenericConfig robotOptionsGeneric_{}; /// \brief Name of the motors. std::vector motorNames_{}; /// \brief Name of the sensors. diff --git a/core/include/jiminy/core/solver/constraint_solvers.h b/core/include/jiminy/core/solver/constraint_solvers.h index c93bf6545..472cbd9b0 100644 --- a/core/include/jiminy/core/solver/constraint_solvers.h +++ b/core/include/jiminy/core/solver/constraint_solvers.h @@ -48,7 +48,7 @@ namespace jiminy class JIMINY_DLLAPI PGSSolver : public AbstractConstraintSolver { public: - DISABLE_COPY(PGSSolver) + JIMINY_DISABLE_COPY(PGSSolver) public: explicit PGSSolver(const pinocchio::Model * model, diff --git a/core/include/jiminy/core/stepper/abstract_stepper.h b/core/include/jiminy/core/stepper/abstract_stepper.h index dea23d42c..7a4884e95 100644 --- a/core/include/jiminy/core/stepper/abstract_stepper.h +++ b/core/include/jiminy/core/stepper/abstract_stepper.h @@ -1,13 +1,31 @@ #ifndef JIMINY_ABSTRACT_STEPPER_H #define JIMINY_ABSTRACT_STEPPER_H +#include #include +#include #include "jiminy/core/fwd.h" #include "jiminy/core/stepper/lie_group.h" namespace jiminy { + namespace stepper + { + enum class ReturnCode : uint8_t + { + IS_SUCCESS = 0, + IS_FAILURE = 1, + IS_ERROR = 2 + }; + + struct JIMINY_DLLAPI StatusInfo + { + ReturnCode returnCode; + std::exception_ptr exception; + }; + } + using systemDynamics = std::function & /*qSplit*/, const std::vector & /*vSplit*/, @@ -15,7 +33,7 @@ namespace jiminy /// \brief Generic interface for steppers used to integrate the dynamic equations of motion. /// - /// \details The generalized position of a mechanical system evolves not in a vector space, but + /// \details The generalized position of a dynamical system evolves not in a vector space, but /// in a Lie group: this is visible for instance in the fact that quaternions have 4 /// components, but only 3 are degrees of freedom, as quaternion must remain unitary /// to represent a rotation. As such, the velocity vector v is not the term-by-term @@ -47,11 +65,11 @@ namespace jiminy /// unmodified. /// /// \return Whether integration was successful. If not, (q, v, a) are not updated. - bool tryStep(std::vector & q, - std::vector & v, - std::vector & a, - double & t, - double & dt); + stepper::StatusInfo tryStep(std::vector & q, + std::vector & v, + std::vector & a, + double & t, + double & dt); protected: /// \brief Internal tryStep method wrapping the arguments as State and diff --git a/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h b/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h index bd0e97ae3..46519fc65 100644 --- a/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h +++ b/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h @@ -61,8 +61,12 @@ namespace jiminy double tolAbs) noexcept; protected: + /// \brief Determine if step has succeeded or failed, and adjust dt. /// + /// \details Scale timestep based on normalized error wrt prescribed absolute and relative + /// tolerances. + /// /// \param[in] intialState Starting state used to compute alternative estimates of the /// solution. /// \param[in] solution Current solution computed by the main Runge-Kutta step. @@ -82,9 +86,6 @@ namespace jiminy /// \returns Normalized error, >1 indicates step failure. double computeError(const State & initialState, const State & solution, double dt); - /// \brief Scale timestep based on normalized error value. - bool adjustStepImpl(double error, double & dt); - private: /// \brief Relative tolerance. double tolRel_; diff --git a/core/include/jiminy/core/telemetry/telemetry_data.h b/core/include/jiminy/core/telemetry/telemetry_data.h index caeffaa85..b63e97f78 100644 --- a/core/include/jiminy/core/telemetry/telemetry_data.h +++ b/core/include/jiminy/core/telemetry/telemetry_data.h @@ -10,7 +10,7 @@ namespace jiminy class JIMINY_DLLAPI TelemetryData { public: - DISABLE_COPY(TelemetryData) + JIMINY_DISABLE_COPY(TelemetryData) public: explicit TelemetryData() = default; @@ -28,10 +28,12 @@ namespace jiminy template T * registerVariable(const std::string & name); - /// \brief Register a constant for the telemetry. + /// \brief Register a constant (so-called invariant) to the telemetry. /// - /// \param[in] name Name of the invariant. - /// \param[in] value Value of the invariant. + /// \details The user is responsible to convert it as a byte array (eg `std::string`). + /// + /// \param[in] name Name of the constant. + /// \param[in] value Value of the constant. void registerConstant(const std::string & name, const std::string & value); /// \brief Format the telemetry header with the current recorded informations. diff --git a/core/include/jiminy/core/telemetry/telemetry_data.hxx b/core/include/jiminy/core/telemetry/telemetry_data.hxx index 8d45055a8..bd886491b 100644 --- a/core/include/jiminy/core/telemetry/telemetry_data.hxx +++ b/core/include/jiminy/core/telemetry/telemetry_data.hxx @@ -25,8 +25,10 @@ namespace jiminy // Check if registration is possible if (!isRegisteringAvailable_) { - THROW_ERROR(std::invalid_argument, - "Entry not found and registration is not available."); + JIMINY_THROW(std::invalid_argument, + "Entry '", + name, + "' not found and registration is not available."); } // Create new variable in registry diff --git a/core/include/jiminy/core/telemetry/telemetry_recorder.h b/core/include/jiminy/core/telemetry/telemetry_recorder.h index 27f87db9d..aadd42599 100644 --- a/core/include/jiminy/core/telemetry/telemetry_recorder.h +++ b/core/include/jiminy/core/telemetry/telemetry_recorder.h @@ -14,7 +14,7 @@ namespace jiminy class JIMINY_DLLAPI TelemetryRecorder { public: - DISABLE_COPY(TelemetryRecorder) + JIMINY_DISABLE_COPY(TelemetryRecorder) public: explicit TelemetryRecorder() = default; @@ -71,7 +71,7 @@ namespace jiminy int64_t floatSectionSize_{-1}; /// \brief Precision to use when logging the time. - double timeUnitInv_{NAN}; + double timeUnitInv_{qNAN}; }; } diff --git a/core/include/jiminy/core/telemetry/telemetry_sender.h b/core/include/jiminy/core/telemetry/telemetry_sender.h index c886630d0..1650dbeb9 100644 --- a/core/include/jiminy/core/telemetry/telemetry_sender.h +++ b/core/include/jiminy/core/telemetry/telemetry_sender.h @@ -17,7 +17,7 @@ namespace jiminy class JIMINY_DLLAPI TelemetrySender { public: - DISABLE_COPY(TelemetrySender) + JIMINY_DISABLE_COPY(TelemetrySender) public: template @@ -42,7 +42,7 @@ namespace jiminy /// \brief Register a new variable to the telemetry. /// /// \details A variable must be registered to be taken into account by the telemetry. The - /// user is responsible for managing its lifetime and updating it in-place. The + /// user is responsible for managing its lifetime and updating it in-place. The /// telemetry sender will fetch its value when calling 'updateValues'. /// /// \param[in] name Name of the field to record in the telemetry. @@ -55,10 +55,12 @@ namespace jiminy void registerVariable(const std::vector & fieldnames, const Eigen::MatrixBase & values); - /// \brief Add an invariant header entry in the log file. + /// \brief Register a constant (so-called invariant) to the telemetry. /// - /// \param[in] name Name of the invariant. - /// \param[in] value Value of the invariant. + /// \details The user is responsible to convert it as a byte array (eg `std::string`). + /// + /// \param[in] name Name of the constant. + /// \param[in] value Value of the constant. void registerConstant(const std::string & name, const std::string & value); /// \brief Update all registered variables in the telemetry buffer. diff --git a/core/include/jiminy/core/utilities/helpers.h b/core/include/jiminy/core/utilities/helpers.h index 9e2788317..54ba2e0ad 100644 --- a/core/include/jiminy/core/utilities/helpers.h +++ b/core/include/jiminy/core/utilities/helpers.h @@ -21,7 +21,7 @@ namespace jiminy friend LockGuardLocal; public: - DISABLE_COPY(MutexLocal) + JIMINY_DISABLE_COPY(MutexLocal) public: explicit MutexLocal() = default; @@ -38,7 +38,7 @@ namespace jiminy class JIMINY_DLLAPI LockGuardLocal { public: - DISABLE_COPY(LockGuardLocal) + JIMINY_DISABLE_COPY(LockGuardLocal) public: explicit LockGuardLocal(MutexLocal & mutexLocal) noexcept; @@ -95,23 +95,40 @@ namespace jiminy std::string JIMINY_DLLAPI getUserDirectory(); + // ********************************* GenericConfig helpers ********************************* // + + /// \brief Partially update destination configuration with values from source configuration + /// recursively. + /// + /// \details The destination must be pre-allocated. No keys are allowed to be added and value + /// types are not allowed to change. Moreover, not all destination keys have to be + /// specified in source. + /// + /// \param[out] dst Destination configuration. + /// \param[in] src Source configuration. + /// \param[in] strict If set to 'true', then an exception will be thrown if a source key is + /// missing from destination, otherwise any missing key will be silently + /// ignored. Value type mismatch will raise an exception no matter what. + void deepUpdate(GenericConfig & dst, const GenericConfig & src, bool strict = false); + // ********************************** Telemetry utilities ********************************** // - bool endsWith(const std::string & str, const std::string & substr); + bool JIMINY_DLLAPI endsWith(const std::string & str, const std::string & substr); - std::string addCircumfix(std::string fieldname, // Make a copy - const std::string_view & prefix = {}, - const std::string_view & suffix = {}, - const std::string_view & delimiter = {}); - std::vector addCircumfix(const std::vector & fieldnames, - const std::string_view & prefix = {}, - const std::string_view & suffix = {}, - const std::string_view & delimiter = {}); + std::string JIMINY_DLLAPI addCircumfix(std::string fieldname, // Make a copy + const std::string_view & prefix, + const std::string_view & suffix, + const std::string_view & delimiter); + std::vector JIMINY_DLLAPI addCircumfix( + const std::vector & fieldnames, + const std::string_view & prefix, + const std::string_view & suffix, + const std::string_view & delimiter); - std::string removeSuffix(std::string fieldname, // Make a copy - const std::string & suffix); - std::vector removeSuffix(const std::vector & fieldnames, - const std::string & suffix); + std::string JIMINY_DLLAPI removeSuffix(std::string fieldname, // Make a copy + const std::string & suffix); + std::vector JIMINY_DLLAPI removeSuffix( + const std::vector & fieldnames, const std::string & suffix); /// \brief Value of a single logged variable (by copy). /// @@ -148,25 +165,6 @@ namespace jiminy std::tuple> isGcdIncluded(InputIt first, InputIt last, const UnaryFunction & func, const Args &... values); - // ********************************** Std::vector helpers ********************************** // - - template - std::enable_if_t, bool> checkDuplicates(const T & vect); - - template - std::enable_if_t && is_vector_v, bool> checkIntersection(const T1 & vec1, - const T2 & vec2); - - template - std::enable_if_t && is_vector_v, bool> checkInclusion(const T1 & vec1, - const T2 & vec2); - - template - std::enable_if_t && is_vector_v, void> eraseVector(const T1 & vec1, - const T2 & vec2); - - // ************************************* Miscellaneous ************************************* // - /// \brief Swap two disjoint row-blocks of data in a matrix. /// /// \details Let b1, b2 be two row-blocks of arbitrary sizes of a matrix B s.t. @@ -185,6 +183,24 @@ namespace jiminy Eigen::Index firstBlockSize, Eigen::Index secondBlockStart, Eigen::Index secondBlockSize); + + // ********************************** std::vector helpers ********************************** // + + template + std::enable_if_t, bool> checkDuplicates(const T & vect); + + template + std::enable_if_t && is_vector_v, bool> checkIntersection(const T1 & vec1, + const T2 & vec2); + + template + std::enable_if_t && is_vector_v, bool> checkInclusion(const T1 & vec1, + const T2 & vec2); + + template + std::enable_if_t && is_vector_v, void> eraseVector(const T1 & vec1, + const T2 & vec2); + } #include "jiminy/core/utilities/helpers.hxx" diff --git a/core/include/jiminy/core/utilities/helpers.hxx b/core/include/jiminy/core/utilities/helpers.hxx index 25f44663a..25ac8031d 100644 --- a/core/include/jiminy/core/utilities/helpers.hxx +++ b/core/include/jiminy/core/utilities/helpers.hxx @@ -109,11 +109,8 @@ namespace jiminy bool isIncluded = ( [&valueMin](double value) { - if (value < EPS) - { - return true; - } - return std::fmod(value, valueMin) < EPS; + // No need to be more accurate than the minimum stepper update period + return std::fmod(value, valueMin) < STEPPER_MIN_TIMESTEP; }(values) && ...); return {isIncluded, valueMin}; } diff --git a/core/include/jiminy/core/utilities/json.hxx b/core/include/jiminy/core/utilities/json.hxx index 400c7398e..591333b02 100644 --- a/core/include/jiminy/core/utilities/json.hxx +++ b/core/include/jiminy/core/utilities/json.hxx @@ -49,7 +49,7 @@ namespace jiminy } template<> - Json::Value convertToJson(const FlexibleJointData & value); + Json::Value convertToJson(const FlexibilityJointConfig & value); template<> Json::Value convertToJson(const HeightmapFunction & value); @@ -76,8 +76,8 @@ namespace jiminy } template<> - constexpr const char * - getJsonVectorType(const std::vector & /* value */) + constexpr const char * getJsonVectorType( + const std::vector & /* value */) { return "list(flexibility)"; } @@ -139,7 +139,7 @@ namespace jiminy Eigen::MatrixXd convertFromJson(const Json::Value & value); template<> - FlexibleJointData convertFromJson(const Json::Value & value); + FlexibilityJointConfig convertFromJson(const Json::Value & value); template<> HeightmapFunction convertFromJson(const Json::Value & value); diff --git a/core/include/jiminy/core/utilities/pinocchio.h b/core/include/jiminy/core/utilities/pinocchio.h index 272886480..4e414e67a 100644 --- a/core/include/jiminy/core/utilities/pinocchio.h +++ b/core/include/jiminy/core/utilities/pinocchio.h @@ -68,16 +68,19 @@ namespace jiminy const Eigen::VectorXd & q, double tolAbs = Eigen::NumTraits::dummy_precision()); - void swapJoints(pinocchio::Model & model, + void swapJointIndices(pinocchio::Model & model, pinocchio::JointIndex jointIndex1, pinocchio::JointIndex jointIndex2); - void insertFlexibilityBeforeJointInModel(pinocchio::Model & model, - const std::string & childJointName, - const std::string & newJointName); + void addFlexibilityJointBeforeMechanicalJoint(pinocchio::Model & model, + const std::string & childJointName, + const std::string & newJointName); - void insertFlexibilityAtFixedFrameInModel(pinocchio::Model & model, - const std::string & frameName); + void addFlexibilityJointAtFixedFrame(pinocchio::Model & model, const std::string & frameName); + + void addBacklashJointAfterMechanicalJoint(pinocchio::Model & model, + const std::string & parentJointName, + const std::string & newJointName); Eigen::MatrixXd JIMINY_DLLAPI interpolatePositions(const pinocchio::Model & model, const Eigen::VectorXd & timesIn, diff --git a/core/include/jiminy/core/utilities/random.h b/core/include/jiminy/core/utilities/random.h index 11894590f..d2f2642a6 100644 --- a/core/include/jiminy/core/utilities/random.h +++ b/core/include/jiminy/core/utilities/random.h @@ -320,7 +320,7 @@ namespace jiminy class JIMINY_DLLAPI PeriodicGaussianProcess { public: - DISABLE_COPY(PeriodicGaussianProcess) + JIMINY_DISABLE_COPY(PeriodicGaussianProcess) public: explicit PeriodicGaussianProcess(double wavelength, double period) noexcept; @@ -364,7 +364,7 @@ namespace jiminy class JIMINY_DLLAPI PeriodicFourierProcess { public: - DISABLE_COPY(PeriodicFourierProcess) + JIMINY_DISABLE_COPY(PeriodicFourierProcess) public: explicit PeriodicFourierProcess(double wavelength, double period) noexcept; @@ -487,7 +487,7 @@ namespace jiminy class JIMINY_DLLAPI AbstractPerlinProcess { public: - DISABLE_COPY(AbstractPerlinProcess) + JIMINY_DISABLE_COPY(AbstractPerlinProcess) using OctaveScalePair = std::pair, const double>; diff --git a/core/src/constraints/abstract_constraint.cc b/core/src/constraints/abstract_constraint.cc index 6cbcccd80..fc8941232 100644 --- a/core/src/constraints/abstract_constraint.cc +++ b/core/src/constraints/abstract_constraint.cc @@ -19,13 +19,13 @@ namespace jiminy // Make sure the constraint is not already attached if (isAttached_) { - THROW_ERROR(bad_control_flow, "Constraint already attached to a model."); + JIMINY_THROW(bad_control_flow, "Constraint already attached to a model."); } // Make sure the model still exists if (model.expired()) { - THROW_ERROR(bad_control_flow, "Model pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Model pointer expired or unset."); } // Consider the constraint is attached at this point @@ -62,7 +62,7 @@ namespace jiminy { if (kp < 0.0) { - THROW_ERROR(std::invalid_argument, "Position gain must be positive."); + JIMINY_THROW(std::invalid_argument, "Position gain must be positive."); } kp_ = kp; } @@ -76,7 +76,7 @@ namespace jiminy { if (kd < 0.0) { - THROW_ERROR(std::invalid_argument, "Velocity gain must be positive."); + JIMINY_THROW(std::invalid_argument, "Velocity gain must be positive."); } kd_ = kd; } @@ -90,7 +90,7 @@ namespace jiminy { if (freq < 0.0) { - THROW_ERROR(std::invalid_argument, "Natural frequency must be positive."); + JIMINY_THROW(std::invalid_argument, "Natural frequency must be positive."); } // Critically damped position/velocity gains diff --git a/core/src/constraints/distance_constraint.cc b/core/src/constraints/distance_constraint.cc index 83f839677..ebcfa1f20 100644 --- a/core/src/constraints/distance_constraint.cc +++ b/core/src/constraints/distance_constraint.cc @@ -33,7 +33,7 @@ namespace jiminy { if (distanceRef < 0.0) { - THROW_ERROR(std::invalid_argument, "Reference distance must be positive."); + JIMINY_THROW(std::invalid_argument, "Reference distance must be positive."); } distanceRef_ = distanceRef; } @@ -50,7 +50,7 @@ namespace jiminy auto model = model_.lock(); if (!model) { - THROW_ERROR(bad_control_flow, "Model pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Model pointer expired or unset."); } // Get frames indices @@ -82,7 +82,7 @@ namespace jiminy { if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Constraint not attached to a model."); + JIMINY_THROW(bad_control_flow, "Constraint not attached to a model."); } // Assuming model still exists. diff --git a/core/src/constraints/frame_constraint.cc b/core/src/constraints/frame_constraint.cc index 7ffcfe099..5d99531c4 100644 --- a/core/src/constraints/frame_constraint.cc +++ b/core/src/constraints/frame_constraint.cc @@ -78,7 +78,7 @@ namespace jiminy auto model = model_.lock(); if (!model) { - THROW_ERROR(bad_control_flow, "Model pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Model pointer expired or unset."); } // Get frame index @@ -105,7 +105,7 @@ namespace jiminy { if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Constraint not attached to a model."); + JIMINY_THROW(bad_control_flow, "Constraint not attached to a model."); } // Assuming the model still exists. diff --git a/core/src/constraints/joint_constraint.cc b/core/src/constraints/joint_constraint.cc index cdeffe604..7e30b12d5 100644 --- a/core/src/constraints/joint_constraint.cc +++ b/core/src/constraints/joint_constraint.cc @@ -59,7 +59,7 @@ namespace jiminy auto model = model_.lock(); if (!model) { - THROW_ERROR(bad_control_flow, "Model pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Model pointer expired or unset."); } // Get joint index diff --git a/core/src/constraints/sphere_constraint.cc b/core/src/constraints/sphere_constraint.cc index 4b0134a50..72b4c8a9c 100644 --- a/core/src/constraints/sphere_constraint.cc +++ b/core/src/constraints/sphere_constraint.cc @@ -47,7 +47,7 @@ namespace jiminy auto model = model_.lock(); if (!model) { - THROW_ERROR(bad_control_flow, "Model pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Model pointer expired or unset."); } // Get frame index @@ -70,7 +70,7 @@ namespace jiminy { if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Constraint not attached to a model."); + JIMINY_THROW(bad_control_flow, "Constraint not attached to a model."); } // Assuming the model still exists diff --git a/core/src/constraints/wheel_constraint.cc b/core/src/constraints/wheel_constraint.cc index c39d592f3..32e809a07 100644 --- a/core/src/constraints/wheel_constraint.cc +++ b/core/src/constraints/wheel_constraint.cc @@ -49,7 +49,7 @@ namespace jiminy auto model = model_.lock(); if (!model) { - THROW_ERROR(bad_control_flow, "Model pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Model pointer expired or unset."); } // Get frame index @@ -72,7 +72,7 @@ namespace jiminy { if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Constraint not attached to a model."); + JIMINY_THROW(bad_control_flow, "Constraint not attached to a model."); } // Assuming the model still exists diff --git a/core/src/control/abstract_controller.cc b/core/src/control/abstract_controller.cc index 363c6be23..3f0b4e3a9 100644 --- a/core/src/control/abstract_controller.cc +++ b/core/src/control/abstract_controller.cc @@ -11,8 +11,9 @@ namespace jiminy AbstractController::AbstractController() noexcept : telemetrySender_{std::make_unique()} { - // Clarify that the base implementation is called - AbstractController::setOptions(getDefaultControllerOptions()); + // The base implementation is called + controllerOptionsGeneric_ = AbstractController::getDefaultControllerOptions(); + AbstractController::setOptions(getOptions()); } AbstractController::~AbstractController() = default; @@ -27,21 +28,21 @@ namespace jiminy auto robotOld = robot_.lock(); if (robotOld && robotOld->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before re-initializing its controller."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before re-initializing its controller."); } // Make sure the robot is valid auto robot = robotIn.lock(); if (!robot) { - THROW_ERROR(bad_control_flow, "Robot pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Robot pointer expired or unset."); } if (!robot->getIsInitialized()) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } // Make sure that the controller is not already bound to another robot @@ -52,9 +53,9 @@ namespace jiminy auto controllerOld = robotOld->getController().lock(); if (controllerOld && controllerOld.get() == this) { - THROW_ERROR(bad_control_flow, - "Controller already bound to another robot. Please unbind it " - "first before re-initializing it."); + JIMINY_THROW(bad_control_flow, + "Controller already bound to another robot. Please unbind it " + "first before re-initializing it."); } } } @@ -84,38 +85,39 @@ namespace jiminy isInitialized_ = false; robot_.reset(); sensorMeasurements_.clear(); - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Something is wrong, probably because of 'commandFun'.\nRaised from exception: ", e.what()); } if (command.size() != robot->nmotors()) { - THROW_ERROR(std::invalid_argument, - "'computeCommand' returns command with wrong size."); + JIMINY_THROW(std::invalid_argument, + "'computeCommand' returns command with wrong size."); } internalDynamics(t, q, v, uCustom); if (uCustom.size() != robot->nv()) { - THROW_ERROR(std::invalid_argument, - "'internalDynamics' returns command with wrong size."); + JIMINY_THROW(std::invalid_argument, + "'internalDynamics' returns command with wrong size."); } } void AbstractController::reset(bool resetDynamicTelemetry) { + // Make sure that the controller is initialized if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "The controller is not initialized."); + JIMINY_THROW(bad_control_flow, "Controller not initialized."); } // Make sure that no simulation is already running auto robot = robot_.lock(); if (robot && robot->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before re-initializing its controller."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before re-initializing its controller."); } // Reset the telemetry buffer of dynamically registered quantities @@ -127,7 +129,7 @@ namespace jiminy // Make sure the robot still exists if (!robot) { - THROW_ERROR(bad_control_flow, "Robot pointer expired or unset."); + JIMINY_THROW(bad_control_flow, "Robot pointer expired or unset."); } /* Refresh the sensor data proxy. @@ -143,15 +145,15 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Controller not initialized."); + JIMINY_THROW(bad_control_flow, "Controller not initialized."); } if (!isTelemetryConfigured_ && baseControllerOptions_->telemetryEnable) { if (!telemetryData) { - THROW_ERROR(bad_control_flow, - "Telemetry not initialized. Impossible to log controller data."); + JIMINY_THROW(bad_control_flow, + "Telemetry not initialized. Impossible to log controller data."); } std::string telemetryName{CONTROLLER_TELEMETRY_NAMESPACE}; @@ -189,8 +191,8 @@ namespace jiminy { if (isTelemetryConfigured) { - THROW_ERROR(bad_control_flow, - "Telemetry already initialized. Impossible to register new variables."); + JIMINY_THROW(bad_control_flow, + "Telemetry already initialized. Impossible to register new variables."); } std::vector::const_iterator fieldIt = fieldnames.begin(); @@ -203,12 +205,37 @@ namespace jiminy { return element.first == *fieldIt; }); if (variableIt != registeredVariables.end()) { - THROW_ERROR(lookup_error, "Variable '", *fieldIt, "' already registered."); + JIMINY_THROW(lookup_error, "Variable '", *fieldIt, "' already registered."); } registeredVariables.emplace_back(*fieldIt, &values[i]); } } + void AbstractController::registerConstant(const std::string_view & name, + const std::string & value) + { + // Delayed variable registration (Taken into account by 'configureTelemetry') + + if (isTelemetryConfigured_) + { + JIMINY_THROW(bad_control_flow, + "Telemetry already initialized. Impossible to register new variables."); + } + + // Check in local cache before. + auto constantIt = std::find_if(constantRegistry_.begin(), + constantRegistry_.end(), + [&name](const auto & element) + { return element.first == name; }); + if (constantIt != constantRegistry_.end()) + { + JIMINY_THROW(bad_control_flow, "Constant '", name, "' already registered."); + } + + // Register serialized constant + constantRegistry_.emplace_back(name, value); + } + void AbstractController::registerVariable( const std::vector & fieldnames, const Eigen::Ref, 0, Eigen::Stride> & @@ -241,7 +268,7 @@ namespace jiminy } } - GenericConfig AbstractController::getOptions() const noexcept + const GenericConfig & AbstractController::getOptions() const noexcept { return controllerOptionsGeneric_; } @@ -252,9 +279,9 @@ namespace jiminy auto robot = robot_.lock(); if (robot && robot->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before re-initializing its controller."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before re-initializing its controller."); } // Set controller options diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc index 67438c07c..336f435d1 100644 --- a/core/src/engine/engine.cc +++ b/core/src/engine/engine.cc @@ -109,7 +109,7 @@ namespace jiminy { if (!robot.getIsInitialized()) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } Eigen::Index nv = robot.nv(); @@ -157,7 +157,8 @@ namespace jiminy telemetryRecorder_{std::make_unique()} { // Initialize the configuration options to the default. - setOptions(getDefaultEngineOptions()); + simulationOptionsGeneric_["engine"] = getDefaultEngineOptions(); + setOptions(getOptions()); } // ************************************ Engine *********************************** // @@ -170,18 +171,18 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before adding new robot."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before adding new robot."); } if (!robotIn) { - THROW_ERROR(std::invalid_argument, "No robot specified."); + JIMINY_THROW(std::invalid_argument, "No robot specified."); } if (!robotIn->getIsInitialized()) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } /* All the robots must have a unique name. The latter will be used as prefix of telemetry @@ -189,11 +190,14 @@ namespace jiminy engine is allowed to have no name. In such a case, no prefix will be added to telemetry variables for this specific robot. This does not prevent adding other robots with qualified names later on. This branching adds complexity internally, but simplifies - single-robot simulation for the end-user, which is by far the most common use-case. */ + single-robot simulation for the end-user, which is by far the most common use-case. + Similarly, the name 'robot' is reserved for the first robot only. */ const std::string & robotName = robotIn->getName(); - if (!robots_.empty() && robotName == "") + if (!robots_.empty() && (robotName == "" || robotName == "robot")) { - THROW_ERROR(std::invalid_argument, "All robots but the first one must have a name."); + JIMINY_THROW(std::invalid_argument, + "All robots, except the first, must have a non-empty name other than " + "'robot'."); } // Check if a robot with the same name already exists @@ -203,10 +207,10 @@ namespace jiminy { return (robot->getName() == robotName); }); if (robotIt != robots_.end()) { - THROW_ERROR(std::invalid_argument, - "Robot with name '", - robotName, - "' already added to the engine."); + JIMINY_THROW(std::invalid_argument, + "Robot with name '", + robotName, + "' already added to the engine."); } robots_.push_back(std::move(robotIn)); @@ -218,8 +222,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing a robot."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before removing a robot."); } /* Remove every coupling forces involving the robot. @@ -245,6 +249,14 @@ namespace jiminy // Remove the robot from the list robots_.erase(robots_.begin() + robotIndex); robotDataVec_.erase(robotDataVec_.begin() + robotIndex); + + // Remove robot from generic options + std::string robotOptionsKey = robotName; + if (robotOptionsKey.empty()) + { + robotOptionsKey = "robot"; + } + simulationOptionsGeneric_.erase(robotOptionsKey); } void Engine::registerCouplingForce(const std::string & robotName1, @@ -256,8 +268,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before adding coupling forces."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before adding coupling forces."); } // Get robot and frame indices @@ -271,8 +283,8 @@ namespace jiminy // Make sure it is not coupling the exact same frame if (robotIndex1 == robotIndex2 && frameIndex1 == frameIndex2) { - THROW_ERROR(std::invalid_argument, - "A coupling force must involve two different frames."); + JIMINY_THROW(std::invalid_argument, + "A coupling force must involve two different frames."); } couplingForces_.emplace_back(robotName1, @@ -297,8 +309,8 @@ namespace jiminy // Make sure that the input arguments are valid if ((stiffness.array() < 0.0).any() || (damping.array() < 0.0).any()) { - THROW_ERROR(std::invalid_argument, - "Stiffness and damping parameters must be positive."); + JIMINY_THROW(std::invalid_argument, + "Stiffness and damping parameters must be positive."); } // Get robot and frame indices @@ -347,9 +359,9 @@ namespace jiminy rotLog12 = pinocchio::log3(rot12, angle); if (angle > 0.95 * M_PI) { - THROW_ERROR(std::runtime_error, - "Relative angle between reference frames of viscoelastic " - "coupling must be smaller than 0.95 * pi."); + JIMINY_THROW(std::runtime_error, + "Relative angle between reference frames of viscoelastic " + "coupling must be smaller than 0.95 * pi."); } pinocchio::Jlog3(angle, rotLog12, rotJLog12); fAng = stiffness.tail<3>().array() * rotLog12.array(); @@ -411,8 +423,8 @@ namespace jiminy // Make sure that the input arguments are valid if (stiffness < 0.0 || damping < 0.0) { - THROW_ERROR(std::invalid_argument, - "The stiffness and damping parameters must be positive."); + JIMINY_THROW(std::invalid_argument, + "The stiffness and damping parameters must be positive."); } // Get robot and frame indices @@ -487,8 +499,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); } // Make sure that the robots exist @@ -510,8 +522,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); } // Make sure that the robot exists @@ -532,8 +544,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); } couplingForces_.clear(); @@ -555,7 +567,7 @@ namespace jiminy { if (robots_.empty()) { - THROW_ERROR(bad_control_flow, "No robot added to the engine."); + JIMINY_THROW(bad_control_flow, "No robot added to the engine."); } if (!isTelemetryConfigured_) @@ -568,39 +580,42 @@ namespace jiminy auto energyIt = energy_.begin(); for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt, ++energyIt) { + // Define proxy for convenience + const std::string & robotName = (*robotIt)->getName(); + // Generate the log fieldnames robotDataIt->logPositionFieldnames = addCircumfix((*robotIt)->getLogPositionFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logVelocityFieldnames = addCircumfix((*robotIt)->getLogVelocityFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logAccelerationFieldnames = addCircumfix((*robotIt)->getLogAccelerationFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logForceExternalFieldnames = addCircumfix((*robotIt)->getLogForceExternalFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logCommandFieldnames = addCircumfix((*robotIt)->getLogCommandFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logMotorEffortFieldnames = addCircumfix((*robotIt)->getLogMotorEffortFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); - robotDataIt->logEnergyFieldname = addCircumfix( - "energy", (*robotIt)->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logEnergyFieldname = + addCircumfix("energy", robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); // Register variables to the telemetry senders if (engineOptions_->telemetry.enableConfiguration) @@ -890,6 +905,37 @@ namespace jiminy } } + template + static std::string serialize(T && value) + { + if constexpr (std::is_same_v>) + { + return value; + } + else if constexpr (is_vector_v) + { + std::ostringstream sstr; + const std::size_t size = value.size(); + for (std::size_t i = 0; i < size; ++i) + { + serialize(value[i]); + if (i < size) + { + sstr << ";"; + } + } + return sstr.str(); + } + else if constexpr (std::is_arithmetic_v>) + { + return toString(value); + } + else + { + return ::jiminy::saveToBinary(value); + } + } + void Engine::start(const std::map & qInit, const std::map & vInit, const std::optional> & aInit) @@ -897,20 +943,20 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Please stop it before starting a new one."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Please stop it before starting a new one."); } if (robots_.empty()) { - THROW_ERROR(bad_control_flow, - "No robot to simulate. Please add one before starting a simulation."); + JIMINY_THROW(bad_control_flow, + "No robot to simulate. Please add one before starting a simulation."); } const std::size_t nRobots = robots_.size(); if (qInit.size() != nRobots || vInit.size() != nRobots) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Mismatching between number of robots and initial configurations or velocities."); } @@ -928,21 +974,21 @@ namespace jiminy auto vInitIt = vInit.find(robotName); if (qInitIt == qInit.end() || vInitIt == vInit.end()) { - THROW_ERROR(std::invalid_argument, - "Robot '", - robotName, - "'does not have an initial configuration or velocity."); + JIMINY_THROW(std::invalid_argument, + "Robot '", + robotName, + "' does not have an initial configuration or velocity."); } const Eigen::VectorXd & q = qInitIt->second; const Eigen::VectorXd & v = vInitIt->second; if (q.rows() != robot->nq() || v.rows() != robot->nv()) { - THROW_ERROR(std::invalid_argument, - "The dimension of the initial configuration or velocity is " - "inconsistent with model size for robot '", - robotName, - "'."); + JIMINY_THROW(std::invalid_argument, + "The dimension of the initial configuration or velocity is " + "inconsistent with model size for robot '", + robotName, + "'."); } // Make sure the initial state is normalized @@ -950,34 +996,33 @@ namespace jiminy isPositionValid(robot->pinocchioModel_, q, std::numeric_limits::epsilon()); if (!isValid) { - THROW_ERROR(std::invalid_argument, - "Initial configuration not consistent with model for robot '", - robotName, - "'."); + JIMINY_THROW(std::invalid_argument, + "Initial configuration not consistent with model for robot '", + robotName, + "'."); } /* Check that the initial configuration is not out-of-bounds if appropriate. Note that EPS allows to be very slightly out-of-bounds, which may occurs because of rounding errors. */ - if ((robot->modelOptions_->joints.enablePositionLimit && - (contactModel_ == ContactModelType::CONSTRAINT) && + if (((contactModel_ == ContactModelType::CONSTRAINT) && ((EPS < q.array() - robot->getPositionLimitMax().array()).any() || (EPS < robot->getPositionLimitMin().array() - q.array()).any()))) { - THROW_ERROR(std::invalid_argument, - "Initial configuration out-of-bounds for robot '", - robotName, - "'."); + JIMINY_THROW(std::invalid_argument, + "Initial configuration out-of-bounds for robot '", + robotName, + "'."); } // Check that the initial velocity is not out-of-bounds if ((robot->modelOptions_->joints.enableVelocityLimit && (robot->getVelocityLimit().array() < v.array().abs()).any())) { - THROW_ERROR(std::invalid_argument, - "Initial velocity out-of-bounds for robot '", - robotName, - "'."); + JIMINY_THROW(std::invalid_argument, + "Initial velocity out-of-bounds for robot '", + robotName, + "'."); } /* Make sure the configuration is normalized (as double), since normalization is @@ -997,9 +1042,9 @@ namespace jiminy // Check the dimension of the initial acceleration associated with every robot if (aInit->size() != nRobots) { - THROW_ERROR(std::invalid_argument, - "If specified, the number of initial accelerations must match the " - "number of robots."); + JIMINY_THROW(std::invalid_argument, + "If specified, the number of initial accelerations must match the " + "number of robots."); } for (const auto & robot : robots_) @@ -1007,16 +1052,16 @@ namespace jiminy auto aInitIt = aInit->find(robot->getName()); if (aInitIt == aInit->end()) { - THROW_ERROR(std::invalid_argument, - "Robot '", - robot->getName(), - "'does not have an initial acceleration."); + JIMINY_THROW(std::invalid_argument, + "Robot '", + robot->getName(), + "'does not have an initial acceleration."); } const Eigen::VectorXd & a = aInitIt->second; if (a.rows() != robot->nv()) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Dimension of initial acceleration inconsistent with model for robot '", robot->getName(), @@ -1044,8 +1089,8 @@ namespace jiminy { if (!sensor->getIsInitialized()) { - THROW_ERROR(bad_control_flow, - "At least a sensor of a robot is not initialized."); + JIMINY_THROW(bad_control_flow, + "At least a sensor of a robot is not initialized."); } } } @@ -1054,8 +1099,8 @@ namespace jiminy { if (!motor->getIsInitialized()) { - THROW_ERROR(bad_control_flow, - "At least a motor of a robot is not initialized."); + JIMINY_THROW(bad_control_flow, + "At least a motor of a robot is not initialized."); } } } @@ -1075,7 +1120,7 @@ namespace jiminy for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt) { // Propagate the user-defined gravity at robot level - (*robotIt)->pinocchioModelOrig_.gravity = engineOptions_->world.gravity; + (*robotIt)->pinocchioModelTh_.gravity = engineOptions_->world.gravity; (*robotIt)->pinocchioModel_.gravity = engineOptions_->world.gravity; /* Reinitialize the robot state buffers, since the robot kinematic may have changed. @@ -1218,7 +1263,6 @@ namespace jiminy const ConstraintTree & constraints = (*robotIt)->getConstraints(); constraints.foreach( [&contactModel = contactModel_, - &enablePositionLimit = (*robotIt)->modelOptions_->joints.enablePositionLimit, &freq = engineOptions_->contacts.stabilizationFreq]( const std::shared_ptr & constraint, ConstraintNodeType node) @@ -1235,15 +1279,11 @@ namespace jiminy switch (node) { case ConstraintNodeType::BOUNDS_JOINTS: - if (!enablePositionLimit) - { - return; - } - { - auto & jointConstraint = - static_cast(*constraint.get()); - jointConstraint.setRotationDir(false); - } + { + auto & jointConstraint = + static_cast(*constraint.get()); + jointConstraint.setRotationDir(false); + } [[fallthrough]]; case ConstraintNodeType::CONTACT_FRAMES: case ConstraintNodeType::COLLISION_BODIES: @@ -1286,7 +1326,7 @@ namespace jiminy if (forceMax > 1e5) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "The initial force exceeds 1e5 for at least one contact point, which is " "forbidden for the sake of numerical stability. Please update the initial " @@ -1377,9 +1417,9 @@ namespace jiminy // Make sure there is no nan at this point if ((a.array() != a.array()).any()) { - THROW_ERROR(std::runtime_error, - "Impossible to compute the acceleration. Probably a " - "subtree has zero inertia along an articulated axis."); + JIMINY_THROW(std::runtime_error, + "Impossible to compute the acceleration. Probably a " + "subtree has zero inertia along an articulated axis."); } // Compute all external terms including joints accelerations and forces @@ -1442,66 +1482,56 @@ namespace jiminy // Log robots data for (const auto & robot : robots_) { + // Define helper to make it easy to register a robot member as a constant + auto registerRobotMember = [&](const std::string & name, auto && member) + { + // Prepend robot name to variable name + const std::string key = + addCircumfix(name, robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + + // Dump serialized constant + if constexpr (std::is_member_function_pointer_v>) + { + telemetrySender_->registerConstant(key, serialize((robot.get()->*member)())); + } + else + { + telemetrySender_->registerConstant(key, serialize(robot.get()->*member)); + } + }; + // Backup URDF file - const std::string telemetryUrdfFile = - addCircumfix("urdf_file", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); - const std::string & urdfFileString = robot->getUrdfAsString(); - telemetrySender_->registerConstant(telemetryUrdfFile, urdfFileString); + registerRobotMember("urdf_file", &Robot::getUrdfAsString); // Backup 'has_freeflyer' option - const std::string telemetrHasFreeflyer = - addCircumfix("has_freeflyer", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); - telemetrySender_->registerConstant(telemetrHasFreeflyer, - toString(robot->getHasFreeflyer())); + registerRobotMember("has_freeflyer", &Robot::getHasFreeflyer); // Backup mesh package lookup directories - const std::string telemetryMeshPackageDirs = addCircumfix( - "mesh_package_dirs", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); - std::string meshPackageDirsString; - std::stringstream meshPackageDirsStream; - const std::vector & meshPackageDirs = robot->getMeshPackageDirs(); - copy(meshPackageDirs.begin(), - meshPackageDirs.end(), - std::ostream_iterator(meshPackageDirsStream, ";")); - if (meshPackageDirsStream.peek() != - decltype(meshPackageDirsStream)::traits_type::eof()) - { - meshPackageDirsString = meshPackageDirsStream.str(); - meshPackageDirsString.pop_back(); - } - telemetrySender_->registerConstant(telemetryMeshPackageDirs, meshPackageDirsString); + registerRobotMember("mesh_package_dirs", &Robot::getMeshPackageDirs); - // Backup the true and theoretical Pinocchio::Model - std::string key = addCircumfix( - "pinocchio_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); - std::string value = saveToBinary(robot->pinocchioModel_); - telemetrySender_->registerConstant(key, value); + // Backup the theoretical model and current extended model + registerRobotMember("pinocchio_model_th", &Robot::pinocchioModelTh_); + registerRobotMember("pinocchio_model", &Robot::pinocchioModel_); /* Backup the Pinocchio GeometryModel for collisions and visuals. It may fail because of missing serialization methods for convex, or because it - cannot fit into memory (return code). Persistent mode is automatically enforced - if no URDF is associated with the robot.*/ - if (engineOptions_->telemetry.isPersistent || urdfFileString.empty()) + cannot fit into memory. + Persistent mode is automatically enabled if no URDF is associated with the robot. */ + const bool hasUrdfFile = !robot->getUrdfAsString().empty(); + if (engineOptions_->telemetry.isPersistent || !hasUrdfFile) { try { - key = addCircumfix( - "collision_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); - value = saveToBinary(robot->collisionModel_); - telemetrySender_->registerConstant(key, value); - - key = addCircumfix( - "visual_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); - value = saveToBinary(robot->visualModel_); - telemetrySender_->registerConstant(key, value); + registerRobotMember("collision_model", &Robot::collisionModel_); + registerRobotMember("visual_model", &Robot::visualModel_); } catch (const std::exception & e) { std::string msg{"Failed to log the collision and/or visual model."}; - if (urdfFileString.empty()) + if (!hasUrdfFile) { - msg += " It will be impossible to replay log files because no URDF " - "file is available as fallback."; + msg += "\nIt will be impossible to replay log files because no URDF file " + "is available as fallback."; } msg += "\nRaised from exception: "; PRINT_WARNING(msg, e.what()); @@ -1510,23 +1540,19 @@ namespace jiminy } // Log all options - GenericConfig allOptions; - for (const auto & robot : robots_) - { - const std::string telemetryRobotOptions = - addCircumfix("robot", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); - allOptions[telemetryRobotOptions] = robot->getOptions(); - } - allOptions["engine"] = engineOptionsGeneric_; - Json::Value allOptionsJson = convertToJson(allOptions); + Json::Value simulationOptionsJson = convertToJson(getSimulationOptions()); Json::StreamWriterBuilder jsonWriter; jsonWriter["indentation"] = ""; - const std::string allOptionsString = Json::writeString(jsonWriter, allOptionsJson); - telemetrySender_->registerConstant("options", allOptionsString); + const std::string simulationOptionsString = + Json::writeString(jsonWriter, simulationOptionsJson); + telemetrySender_->registerConstant("options", simulationOptionsString); // Write the header: this locks the registration of new variables telemetryRecorder_->initialize(telemetryData_.get(), getTelemetryTimeUnit()); + // Make sure tha the simulation options are not considered refreshed anymore + areSimulationOptionsRefreshed_ = false; + // At this point, consider that the simulation is running isSimulationRunning_ = true; } @@ -1545,10 +1571,10 @@ namespace jiminy // Process initial configuration std::map qInitMap; - if (isStateTheoretical && robot->modelOptions_->dynamics.enableFlexibleModel) + if (isStateTheoretical) { Eigen::VectorXd q0; - robot->getFlexiblePositionFromRigid(qInit, q0); + robot->getExtendedPositionFromTheoretical(qInit, q0); qInitMap.emplace(robotName, std::move(q0)); } else @@ -1558,10 +1584,10 @@ namespace jiminy // Process initial velocity std::map vInitMap; - if (isStateTheoretical && robot->modelOptions_->dynamics.enableFlexibleModel) + if (isStateTheoretical) { Eigen::VectorXd v0; - robot->getFlexibleVelocityFromRigid(vInit, v0); + robot->getExtendedVelocityFromTheoretical(vInit, v0); vInitMap.emplace(robotName, std::move(v0)); } else @@ -1574,10 +1600,10 @@ namespace jiminy if (aInit) { aInitMap.emplace(); - if (isStateTheoretical && robot->modelOptions_->dynamics.enableFlexibleModel) + if (isStateTheoretical) { Eigen::VectorXd a0; - robot->getFlexibleVelocityFromRigid(*aInit, a0); + robot->getExtendedVelocityFromTheoretical(*aInit, a0); aInitMap->emplace(robotName, std::move(a0)); } else @@ -1597,9 +1623,9 @@ namespace jiminy // Make sure that a single robot has been added to the engine if (robots_.size() != 1) { - THROW_ERROR(bad_control_flow, - "Multi-robot simulation requires specifying the initial state of each " - "robot individually."); + JIMINY_THROW(bad_control_flow, + "Multi-robot simulation requires specifying the initial state of each " + "robot individually."); } // Pre-process initial state @@ -1619,14 +1645,14 @@ namespace jiminy // Make sure that no simulation is already running if (robots_.empty()) { - THROW_ERROR(bad_control_flow, - "No robot to simulate. Please add one before starting simulation."); + JIMINY_THROW(bad_control_flow, + "No robot to simulate. Please add one before starting simulation."); } // Make sure that the user-specified simulation duration is long enough if (tEnd < 5e-3) { - THROW_ERROR(std::invalid_argument, "Simulation duration cannot be shorter than 5ms."); + JIMINY_THROW(std::invalid_argument, "Simulation duration cannot be shorter than 5ms."); } // Reset the engine and all the robots @@ -1638,11 +1664,11 @@ namespace jiminy // Now that telemetry has been initialized, check simulation duration if (tEnd > telemetryRecorder_->getLogDurationMax()) { - THROW_ERROR(std::runtime_error, - "Time overflow: with the current precision the maximum value that can be " - "logged is ", - telemetryRecorder_->getLogDurationMax(), - "s. Decrease logger precision to simulate for longer than that."); + JIMINY_THROW(std::runtime_error, + "Time overflow: with the current precision the maximum value that can be " + "logged is ", + telemetryRecorder_->getLogDurationMax(), + "s. Decrease logger precision to simulate for longer than that."); } // Integration loop based on boost::numeric::odeint::detail::integrate_times @@ -1707,9 +1733,9 @@ namespace jiminy // Make sure that a single robot has been added to the engine if (robots_.size() != 1) { - THROW_ERROR(bad_control_flow, - "Multi-robot simulation requires specifying the initial state of each " - "robot individually."); + JIMINY_THROW(bad_control_flow, + "Multi-robot simulation requires specifying the initial state of each " + "robot individually."); } // Pre-process initial state @@ -1725,8 +1751,8 @@ namespace jiminy // Check if the simulation has started if (!isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "No simulation running. Please start one before using step method."); + JIMINY_THROW(bad_control_flow, + "No simulation running. Please start one before using step method."); } // Clear log data buffer @@ -1740,15 +1766,15 @@ namespace jiminy { if (qIt->hasNaN() || vIt->hasNaN() || aIt->hasNaN()) { - THROW_ERROR(std::runtime_error, - "Low-level ode solver failed. Consider increasing stepper accuracy."); + JIMINY_THROW(std::runtime_error, + "Low-level ode solver failed. Consider increasing stepper accuracy."); } } // Check if the desired step size is suitable if (stepSize > EPS && stepSize < SIMULATION_MIN_TIMESTEP) { - THROW_ERROR(std::invalid_argument, "Step size out of bounds."); + JIMINY_THROW(std::invalid_argument, "Step size out of bounds."); } /* Set end time: The default step size is equal to the controller update period if @@ -1779,11 +1805,11 @@ namespace jiminy integration. */ if (stepperState_.t + stepSize > telemetryRecorder_->getLogDurationMax()) { - THROW_ERROR(std::runtime_error, - "Time overflow: with the current precision the maximum value that can be " - "logged is ", - telemetryRecorder_->getLogDurationMax(), - "s. Decrease logger precision to simulate for longer than that."); + JIMINY_THROW(std::runtime_error, + "Time overflow: with the current precision the maximum value that can be " + "logged is ", + telemetryRecorder_->getLogDurationMax(), + "s. Decrease logger precision to simulate for longer than that."); } /* Avoid compounding of error using Kahan algorithm. It consists in keeping track of the @@ -1804,7 +1830,7 @@ namespace jiminy // Monitor iteration failure uint32_t successiveIterFailed = 0; std::vector successiveSolveFailedAll(robots_.size(), 0U); - bool isNan = false; + stepper::StatusInfo status{stepper::ReturnCode::IS_SUCCESS, {}}; /* Flag monitoring if the current time step depends of a breakpoint or the integration tolerance. It will be used by the restoration mechanism, if dt gets very small to reach @@ -2108,17 +2134,8 @@ namespace jiminy dtLargest = dt; // Try doing one integration step - bool isStepSuccessful = - stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest); - - /* Check if the integrator failed miserably even if successfully. - It would happen the timestep is fixed and too large, causing the integrator - to fail miserably returning nan. */ - isNan = std::isnan(dtLargest); - if (isNan) - { - break; - } + status = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest); + bool isStepSuccessful = status.returnCode == stepper::ReturnCode::IS_SUCCESS; // Update buffer if really successful if (isStepSuccessful) @@ -2175,6 +2192,17 @@ namespace jiminy } else { + /* Check if the integrator raised an exception. This typically happens + when the timestep is fixed and too large, causing the integrator to + fail miserably returning nan. In such a case, adjust the timestep + manually as a recovery mechanism based on a simple heuristic. + Note that it has no effect for fixed-timestep integrator since + `dtLargest` should be INF already. */ + if (status.returnCode == stepper::ReturnCode::IS_ERROR) + { + dtLargest *= 0.1; + } + // Increment the failed iteration counters ++successiveIterFailed; ++stepperState_.iterFailed; @@ -2207,9 +2235,6 @@ namespace jiminy bool isStepSuccessful = false; while (!isStepSuccessful) { - // Set the timestep to be tried by the stepper - dtLargest = dt; - // Break the loop in case of too many successive failed inner iteration if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax) { @@ -2236,15 +2261,12 @@ namespace jiminy } } - // Try to do a step - isStepSuccessful = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest); + // Set the timestep to be tried by the stepper + dtLargest = dt; - // Check if the integrator failed miserably even if successfully - isNan = std::isnan(dtLargest); - if (isNan) - { - break; - } + // Try to do a step + status = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest); + isStepSuccessful = status.returnCode == stepper::ReturnCode::IS_SUCCESS; if (isStepSuccessful) { @@ -2285,6 +2307,12 @@ namespace jiminy } else { + // Adjust timestep manually if necessary + if (status.returnCode == stepper::ReturnCode::IS_ERROR) + { + dtLargest *= 0.1; + } + // Increment the failed iteration counter ++successiveIterFailed; ++stepperState_.iterFailed; @@ -2305,22 +2333,33 @@ namespace jiminy } // Exception handling - if (isNan) - { - THROW_ERROR(std::runtime_error, - "Something is wrong with the physics. Aborting integration."); - } if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax) { - THROW_ERROR(std::runtime_error, - "Too many successive iteration failures. Probably something " - "is going wrong with the physics. Aborting integration."); + if (status.exception) + { + try + { + std::rethrow_exception(status.exception); + } + catch (const std::exception & e) + { + // TODO: Support `std::throw_with_nested` in JIMINY_THROW instead + JIMINY_THROW( + std::runtime_error, + "Something is wrong with the physics. Try using an adaptive stepper. " + "Aborting integration.\nRaised from exception: ", + e.what()); + } + } + JIMINY_THROW(std::runtime_error, + "Too many successive iteration failures. Probably something is wrong " + "with the physics. Aborting integration."); } for (uint32_t successiveSolveFailed : successiveSolveFailedAll) { if (successiveSolveFailed > engineOptions_->stepper.successiveIterFailedMax) { - THROW_ERROR( + JIMINY_THROW( std::runtime_error, "Too many successive constraint solving failures. Try increasing the " "regularization factor. Aborting integration."); @@ -2328,14 +2367,15 @@ namespace jiminy } if (dt < STEPPER_MIN_TIMESTEP) { - THROW_ERROR(std::runtime_error, - "The internal time step is getting too small. Impossible to " - "integrate physics further in time. Aborting integration."); + JIMINY_THROW(std::runtime_error, + "The internal time step is getting too small. Impossible to " + "integrate physics further in time. Aborting integration."); } if (EPS < engineOptions_->stepper.timeout && engineOptions_->stepper.timeout < timer_.toc()) { - THROW_ERROR(std::runtime_error, "Step computation timeout. Aborting integration."); + JIMINY_THROW(std::runtime_error, + "Step computation timeout. Aborting integration."); } // Update sensors data if necessary, namely if time-continuous or breakpoint @@ -2410,28 +2450,28 @@ namespace jiminy { if (isSimulationRunning_) { - THROW_ERROR( + JIMINY_THROW( bad_control_flow, "Simulation already running. Please stop it before registering new forces."); } if (dt < STEPPER_MIN_TIMESTEP) { - THROW_ERROR(std::invalid_argument, - "Force duration cannot be smaller than ", - STEPPER_MIN_TIMESTEP, - "s."); + JIMINY_THROW(std::invalid_argument, + "Force duration cannot be smaller than ", + STEPPER_MIN_TIMESTEP, + "s."); } if (t < 0.0) { - THROW_ERROR(std::invalid_argument, "Force application time must be positive."); + JIMINY_THROW(std::invalid_argument, "Force application time must be positive."); } if (frameName == "universe") { - THROW_ERROR(std::invalid_argument, - "Impossible to apply external forces to the universe itself!"); + JIMINY_THROW(std::invalid_argument, + "Impossible to apply external forces to the universe itself!"); } // TODO: Make sure that the forces do NOT overlap while taking into account dt. @@ -2479,15 +2519,15 @@ namespace jiminy { if (isSimulationRunning_) { - THROW_ERROR( + JIMINY_THROW( bad_control_flow, "Simulation already running. Please stop it before registering new forces."); } if (frameName == "universe") { - THROW_ERROR(std::invalid_argument, - "Impossible to apply external forces to the universe itself!"); + JIMINY_THROW(std::invalid_argument, + "Impossible to apply external forces to the universe itself!"); } // Get robot and frame indices @@ -2498,7 +2538,7 @@ namespace jiminy // Make sure the update period is valid if (EPS < updatePeriod && updatePeriod < SIMULATION_MIN_TIMESTEP) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Cannot register external force profile with update period smaller than ", SIMULATION_MIN_TIMESTEP, @@ -2509,10 +2549,10 @@ namespace jiminy isGcdIncluded(robotDataVec_, stepperUpdatePeriod_, updatePeriod); if (!isIncluded) { - THROW_ERROR(std::invalid_argument, - "In discrete mode, the update period of force profiles and the " - "stepper update period (min of controller and sensor update " - "periods) must be multiple of each other."); + JIMINY_THROW(std::invalid_argument, + "In discrete mode, the update period of force profiles and the " + "stepper update period (min of controller and sensor update " + "periods) must be multiple of each other."); } // Set breakpoint period during the integration loop @@ -2528,8 +2568,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); } std::ptrdiff_t robotIndex = getRobotIndex(robotName); @@ -2542,8 +2582,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "simulation already running. Stop it before removing coupling forces."); + JIMINY_THROW(bad_control_flow, + "simulation already running. Stop it before removing coupling forces."); } for (auto & robotData : robotDataVec_) @@ -2557,8 +2597,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); } @@ -2580,8 +2620,8 @@ namespace jiminy // Make sure that no simulation is running if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Stop it before removing coupling forces."); + JIMINY_THROW(bad_control_flow, + "Simulation already running. Stop it before removing coupling forces."); } for (auto & robotData : robotDataVec_) @@ -2604,30 +2644,27 @@ namespace jiminy return robotData.profileForces; } - GenericConfig Engine::getOptions() const noexcept - { - return engineOptionsGeneric_; - } - void Engine::setOptions(const GenericConfig & engineOptions) { if (isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "Simulation already running. Please stop it before updating the options."); + JIMINY_THROW( + bad_control_flow, + "Simulation already running. Please stop it before updating the options."); } // Make sure the dtMax is not out of range - GenericConfig stepperOptions = boost::get(engineOptions.at("stepper")); + const GenericConfig & stepperOptions = + boost::get(engineOptions.at("stepper")); const double dtMax = boost::get(stepperOptions.at("dtMax")); if (SIMULATION_MAX_TIMESTEP + EPS < dtMax || dtMax < SIMULATION_MIN_TIMESTEP) { - THROW_ERROR(std::invalid_argument, - "'dtMax' option must bge in range [", - SIMULATION_MIN_TIMESTEP, - ", ", - SIMULATION_MAX_TIMESTEP, - "]."); + JIMINY_THROW(std::invalid_argument, + "'dtMax' option must bge in range [", + SIMULATION_MIN_TIMESTEP, + ", ", + SIMULATION_MAX_TIMESTEP, + "]."); } // Make sure successiveIterFailedMax is strictly positive @@ -2635,15 +2672,15 @@ namespace jiminy boost::get(stepperOptions.at("successiveIterFailedMax")); if (successiveIterFailedMax < 1) { - THROW_ERROR(std::invalid_argument, - "'successiveIterFailedMax' must be strictly positive."); + JIMINY_THROW(std::invalid_argument, + "'successiveIterFailedMax' must be strictly positive."); } // Make sure the selected ode solver is available and instantiate it const std::string & odeSolver = boost::get(stepperOptions.at("odeSolver")); if (STEPPERS.find(odeSolver) == STEPPERS.end()) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Requested ODE solver '", odeSolver, "' not available."); } @@ -2657,7 +2694,7 @@ namespace jiminy if ((EPS < sensorsUpdatePeriod && sensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP) || (EPS < controllerUpdatePeriod && controllerUpdatePeriod < SIMULATION_MIN_TIMESTEP)) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Cannot simulate a discrete robot with update period smaller than ", SIMULATION_MIN_TIMESTEP, @@ -2665,9 +2702,9 @@ namespace jiminy } else if (!isIncluded) { - THROW_ERROR(std::invalid_argument, - "In discrete mode, the controller and sensor update " - "periods must be multiple of each other."); + JIMINY_THROW(std::invalid_argument, + "In discrete mode, the controller and sensor update " + "periods must be multiple of each other."); } // Make sure the contacts options are fine @@ -2678,50 +2715,52 @@ namespace jiminy const auto constraintSolverIt = CONSTRAINT_SOLVERS_MAP.find(constraintSolverType); if (constraintSolverIt == CONSTRAINT_SOLVERS_MAP.end()) { - THROW_ERROR(std::invalid_argument, - "Requested constraint solver '", - constraintSolverType, - "' not available."); + JIMINY_THROW(std::invalid_argument, + "Requested constraint solver '", + constraintSolverType, + "' not available."); } double regularization = boost::get(constraintsOptions.at("regularization")); if (regularization < 0.0) { - THROW_ERROR(std::invalid_argument, - "Constraint option 'regularization' must be positive."); + JIMINY_THROW(std::invalid_argument, + "Constraint option 'regularization' must be positive."); } // Make sure the contacts options are fine - GenericConfig contactOptions = boost::get(engineOptions.at("contacts")); + const GenericConfig & contactOptions = + boost::get(engineOptions.at("contacts")); const std::string & contactModel = boost::get(contactOptions.at("model")); const auto contactModelIt = CONTACT_MODELS_MAP.find(contactModel); if (contactModelIt == CONTACT_MODELS_MAP.end()) { - THROW_ERROR(std::invalid_argument, "Requested contact model not available."); + JIMINY_THROW(std::invalid_argument, "Requested contact model not available."); } double contactsTransitionEps = boost::get(contactOptions.at("transitionEps")); if (contactsTransitionEps < 0.0) { - THROW_ERROR(std::invalid_argument, "Contact option 'transitionEps' must be positive."); + JIMINY_THROW(std::invalid_argument, + "Contact option 'transitionEps' must be positive."); } double transitionVelocity = boost::get(contactOptions.at("transitionVelocity")); if (transitionVelocity < EPS) { - THROW_ERROR(std::invalid_argument, - "Contact option 'transitionVelocity' must be strictly positive."); + JIMINY_THROW(std::invalid_argument, + "Contact option 'transitionVelocity' must be strictly positive."); } double stabilizationFreq = boost::get(contactOptions.at("stabilizationFreq")); if (stabilizationFreq < 0.0) { - THROW_ERROR(std::invalid_argument, - "Contact option 'stabilizationFreq' must be positive."); + JIMINY_THROW(std::invalid_argument, + "Contact option 'stabilizationFreq' must be positive."); } // Make sure the user-defined gravity force has the right dimension - GenericConfig worldOptions = boost::get(engineOptions.at("world")); + const GenericConfig & worldOptions = boost::get(engineOptions.at("world")); Eigen::VectorXd gravity = boost::get(worldOptions.at("gravity")); if (gravity.size() != 6) { - THROW_ERROR(std::invalid_argument, "The size of the gravity force vector must be 6."); + JIMINY_THROW(std::invalid_argument, "The size of the gravity force vector must be 6."); } /* Reset random number generators if setOptions is called for the first time, or if the @@ -2734,11 +2773,12 @@ namespace jiminy generator_.seed(std::seed_seq(seedSeq.data(), seedSeq.data() + seedSeq.size())); } - // Update the internal options - engineOptionsGeneric_ = engineOptions; + // Update class-specific "strongly typed" accessor for fast and convenient access + engineOptions_ = std::make_unique(engineOptions); - // Create a fast struct accessor - engineOptions_ = std::make_unique(engineOptionsGeneric_); + // Update inherited polymorphic accessor + deepUpdate(boost::get(simulationOptionsGeneric_.at("engine")), + engineOptions); // Backup contact model as enum for fast check contactModel_ = contactModelIt->second; @@ -2747,6 +2787,67 @@ namespace jiminy stepperUpdatePeriod_ = updatePeriodMin; } + const GenericConfig & Engine::getOptions() const noexcept + { + return boost::get(simulationOptionsGeneric_.at("engine")); + } + + void Engine::setSimulationOptions(const GenericConfig & simulationOptions) + { + // Set engine options + GenericConfig::const_iterator engineOptionsIt = simulationOptions.find("engine"); + if (engineOptionsIt == simulationOptions.end()) + { + JIMINY_THROW(std::invalid_argument, "Engine options key 'engine' is missing."); + } + setOptions(boost::get(engineOptionsIt->second)); + + // Set options for each robot sequentially + for (auto & robot : robots_) + { + std::string robotOptionsKey = robot->getName(); + if (robotOptionsKey.empty()) + { + robotOptionsKey = "robot"; + } + GenericConfig::const_iterator robotOptionsIt = simulationOptions.find(robotOptionsKey); + if (robotOptionsIt == simulationOptions.end()) + { + JIMINY_THROW(std::invalid_argument, + "Robot options key '", + robotOptionsKey, + "' is missing."); + } + robot->setOptions(boost::get(robotOptionsIt->second)); + } + } + + GenericConfig Engine::getSimulationOptions() const noexcept + { + /* Return options without refreshing all options if and only if the same simulation is + still running since the last time they were considered valid. */ + if (areSimulationOptionsRefreshed_ && isSimulationRunning_) + { + return simulationOptionsGeneric_; + } + + // Refresh robot options + for (const auto & robot : robots_) + { + std::string robotOptionsKey = robot->getName(); + if (robotOptionsKey.empty()) + { + robotOptionsKey = "robot"; + } + simulationOptionsGeneric_[robotOptionsKey] = robot->getOptions(); + } + + // Options are now considered "valid" + areSimulationOptionsRefreshed_ = true; + + return simulationOptionsGeneric_; + } + std::ptrdiff_t Engine::getRobotIndex(const std::string & robotName) const { auto robotIt = std::find_if(robots_.begin(), @@ -2755,16 +2856,16 @@ namespace jiminy { return (robot->getName() == robotName); }); if (robotIt == robots_.end()) { - THROW_ERROR(std::invalid_argument, - "No robot with name '", - robotName, - "' has been added to the engine."); + JIMINY_THROW(std::invalid_argument, + "No robot with name '", + robotName, + "' has been added to the engine."); } return std::distance(robots_.begin(), robotIt); } - std::shared_ptr & Engine::getRobot(const std::string & robotName) + std::shared_ptr Engine::getRobot(const std::string & robotName) { std::ptrdiff_t robotIndex = getRobotIndex(robotName); return robots_[robotIndex]; @@ -3412,38 +3513,39 @@ namespace jiminy const pinocchio::Data & data = robot->pinocchioData_; const ConstraintTree & constraints = robot->getConstraints(); - // Enforce the position limit (rigid joints only) - if (robot->modelOptions_->joints.enablePositionLimit) + /* Enforce position limits for all joints having bounds constraints, ie mechanical and + backlash joints. */ + const Eigen::VectorXd & positionLimitMin = robot->getPositionLimitMin(); + const Eigen::VectorXd & positionLimitMax = robot->getPositionLimitMax(); + for (auto & constraintItem : constraints.boundJoints) { - const Eigen::VectorXd & positionLimitMin = robot->getPositionLimitMin(); - const Eigen::VectorXd & positionLimitMax = robot->getPositionLimitMax(); - const std::vector & rigidJointIndices = - robot->getRigidJointIndices(); - for (std::size_t i = 0; i < rigidJointIndices.size(); ++i) - { - auto & constraint = constraints.boundJoints[i].second; - computePositionLimitsForcesAlgo::run( - model.joints[rigidJointIndices[i]], - typename computePositionLimitsForcesAlgo::ArgsType(data, - q, - v, - positionLimitMin, - positionLimitMax, - engineOptions_, - contactModel_, - constraint, - uInternal)); - } - } - - // Enforce the velocity limit (rigid joints only) + auto & constraint = constraintItem.second; + const auto jointConstraint = std::static_pointer_cast(constraint); + const pinocchio::JointIndex jointIndex = jointConstraint->getJointIndex(); + computePositionLimitsForcesAlgo::run( + model.joints[jointIndex], + typename computePositionLimitsForcesAlgo::ArgsType(data, + q, + v, + positionLimitMin, + positionLimitMax, + engineOptions_, + contactModel_, + constraint, + uInternal)); + } + + // Enforce velocity limits for all joints having bounds constraints if requested if (robot->modelOptions_->joints.enableVelocityLimit) { const Eigen::VectorXd & velocityLimitMax = robot->getVelocityLimit(); - for (pinocchio::JointIndex rigidJointIndex : robot->getRigidJointIndices()) + for (auto & constraintItem : constraints.boundJoints) { + auto & constraint = constraintItem.second; + const auto jointConstraint = std::static_pointer_cast(constraint); + const pinocchio::JointIndex jointIndex = jointConstraint->getJointIndex(); computeVelocityLimitsForcesAlgo::run( - model.joints[rigidJointIndex], + model.joints[jointIndex], typename computeVelocityLimitsForcesAlgo::ArgsType( data, v, velocityLimitMax, engineOptions_, contactModel_, uInternal)); } @@ -3454,7 +3556,7 @@ namespace jiminy Eigen::Matrix3d rotJlog3; const Robot::DynamicsOptions & modelDynOptions = robot->modelOptions_->dynamics; const std::vector & flexibilityJointIndices = - robot->getFlexibleJointIndices(); + robot->getFlexibilityJointIndices(); for (std::size_t i = 0; i < flexibilityJointIndices.size(); ++i) { const pinocchio::JointIndex jointIndex = flexibilityJointIndices[i]; @@ -3467,8 +3569,8 @@ namespace jiminy const Eigen::Vector3d angleAxis = pinocchio::quaternion::log3(quat, angle); if (angle > 0.95 * M_PI) // Angle is always positive { - THROW_ERROR(std::runtime_error, - "Flexible joint angle must be smaller than 0.95 * pi."); + JIMINY_THROW(std::runtime_error, + "Flexible joint angle must be smaller than 0.95 * pi."); } pinocchio::Jlog3(angle, angleAxis, rotJlog3); uInternal.segment<3>(velocityIndex) -= @@ -3682,8 +3784,8 @@ namespace jiminy // Make sure that a simulation is running if (!isSimulationRunning_) { - THROW_ERROR(bad_control_flow, - "No simulation running. Please start one before calling this method."); + JIMINY_THROW(bad_control_flow, + "No simulation running. Please start one before calling this method."); } // Make sure memory has been allocated for the output acceleration @@ -3984,9 +4086,9 @@ namespace jiminy } catch (const H5::FileIException &) { - THROW_ERROR(std::runtime_error, - "Impossible to open the log file. Make sure it exists and " - "you have reading permissions."); + JIMINY_THROW(std::runtime_error, + "Impossible to open the log file. Make sure it exists and " + "you have reading permissions."); } // Extract all constants. There is no ordering among them, unlike variables. @@ -4106,10 +4208,10 @@ namespace jiminy { return readLogHdf5(filename); } - THROW_ERROR(std::invalid_argument, - "Format '", - format, - "' not recognized. It must be either 'binary' or 'hdf5'."); + JIMINY_THROW(std::invalid_argument, + "Format '", + format, + "' not recognized. It must be either 'binary' or 'hdf5'."); } static void writeLogHdf5(const std::string & filename, @@ -4126,9 +4228,9 @@ namespace jiminy } catch (const H5::FileIException & open_file) { - THROW_ERROR(std::runtime_error, - "Impossible to create the log file. Make sure the root folder " - "exists and you have writing permissions."); + JIMINY_THROW(std::runtime_error, + "Impossible to create the log file. Make sure the root folder " + "exists and you have writing permissions."); } // Add "VERSION" attribute @@ -4257,8 +4359,9 @@ namespace jiminy // Make sure there is something to write if (!isTelemetryConfigured_) { - THROW_ERROR(bad_control_flow, - "Telemetry not configured. Please start a simulation before writing log."); + JIMINY_THROW( + bad_control_flow, + "Telemetry not configured. Please start a simulation before writing log."); } // Pick the appropriate format @@ -4276,10 +4379,10 @@ namespace jiminy } else { - THROW_ERROR(std::invalid_argument, - "Format '", - format, - "' not recognized. It must be either 'binary' or 'hdf5'."); + JIMINY_THROW(std::invalid_argument, + "Format '", + format, + "' not recognized. It must be either 'binary' or 'hdf5'."); } } } diff --git a/core/src/hardware/abstract_motor.cc b/core/src/hardware/abstract_motor.cc index 959cd6656..13fbc2c2c 100644 --- a/core/src/hardware/abstract_motor.cc +++ b/core/src/hardware/abstract_motor.cc @@ -9,8 +9,9 @@ namespace jiminy AbstractMotorBase::AbstractMotorBase(const std::string & name) noexcept : name_{name} { - // Initialize the options - setOptions(getDefaultMotorOptions()); + // Initialize options + motorOptionsGeneric_ = getDefaultMotorOptions(); + setOptions(getOptions()); } AbstractMotorBase::~AbstractMotorBase() @@ -22,14 +23,15 @@ namespace jiminy } } - void AbstractMotorBase::attach(std::weak_ptr robot, - std::function notifyRobot, - MotorSharedStorage * sharedStorage) + void AbstractMotorBase::attach( + std::weak_ptr robot, + std::function notifyRobot, + MotorSharedStorage * sharedStorage) { // Make sure the motor is not already attached if (isAttached_) { - THROW_ERROR( + JIMINY_THROW( std::logic_error, "Motor already attached to a robot. Please 'detach' method before attaching it."); } @@ -37,7 +39,7 @@ namespace jiminy // Make sure the robot still exists if (robot.expired()) { - THROW_ERROR(std::runtime_error, "Robot pointer expired or unset."); + JIMINY_THROW(std::runtime_error, "Robot pointer expired or unset."); } // Copy references to the robot and shared data @@ -66,7 +68,7 @@ namespace jiminy if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Motor not attached to any robot."); + JIMINY_THROW(bad_control_flow, "Motor not attached to any robot."); } // Remove associated col in the global data buffer @@ -106,22 +108,37 @@ namespace jiminy // Make sure the motor is attached to a robot if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Motor not attached to any robot."); + JIMINY_THROW(bad_control_flow, "Motor not attached to any robot."); + } + + // Make sure all the motors are attached to a robot and initialized + for (AbstractMotorBase * motor : sharedStorage_->motors_) + { + if (!motor->isAttached_) + { + JIMINY_THROW( + bad_control_flow, "Motor '", motor->name_, "' not attached to any robot."); + } + if (!motor->isInitialized_) + { + JIMINY_THROW(bad_control_flow, "Motor '", motor->name_, "' not initialized."); + } } // Make sure the robot still exists if (robot_.expired()) { - THROW_ERROR(std::runtime_error, "Robot has been deleted. Impossible to reset motors."); + JIMINY_THROW(std::runtime_error, + "Robot has been deleted. Impossible to reset motors."); } // Make sure that no simulation is already running auto robot = robot_.lock(); if (robot && robot->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before resetting motors."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before resetting motors."); } // Clear the shared data buffer @@ -141,53 +158,64 @@ namespace jiminy auto robot = robot_.lock(); if (robot && robot->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before setting motor options."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before setting motor options."); } // Check if the internal buffers must be updated - bool internalBuffersMustBeUpdated = false; if (isInitialized_) { // Check if armature has changed const bool enableArmature = boost::get(motorOptions.at("enableArmature")); - internalBuffersMustBeUpdated |= (baseMotorOptions_->enableArmature != enableArmature); + mustNotifyRobot_ |= (baseMotorOptions_->enableArmature != enableArmature); if (enableArmature) { const double armature = boost::get(motorOptions.at("armature")); - internalBuffersMustBeUpdated |= // + mustNotifyRobot_ |= // std::abs(armature - baseMotorOptions_->armature) > EPS; } + // Check if backlash has changed + const bool enableBacklash = boost::get(motorOptions.at("enableBacklash")); + mustNotifyRobot_ |= (baseMotorOptions_->enableBacklash != enableBacklash); + if (enableBacklash) + { + const double backlash = boost::get(motorOptions.at("backlash")); + mustNotifyRobot_ |= // + std::abs(backlash - baseMotorOptions_->backlash) > EPS; + } + // Check if command limit has changed const bool commandLimitFromUrdf = boost::get(motorOptions.at("commandLimitFromUrdf")); - internalBuffersMustBeUpdated |= + mustNotifyRobot_ |= (baseMotorOptions_->commandLimitFromUrdf != commandLimitFromUrdf); if (!commandLimitFromUrdf) { const double commandLimit = boost::get(motorOptions.at("commandLimit")); - internalBuffersMustBeUpdated |= + mustNotifyRobot_ |= std::abs(commandLimit - baseMotorOptions_->commandLimit) > EPS; } } - // Update the motor's options - motorOptionsGeneric_ = motorOptions; - baseMotorOptions_ = std::make_unique(motorOptionsGeneric_); + // Update class-specific "strongly typed" accessor for fast and convenient access + baseMotorOptions_ = std::make_unique(motorOptions); + + // Update inherited polymorphic accessor + deepUpdate(motorOptionsGeneric_, motorOptions); // Refresh the proxies if the robot is initialized if available if (robot) { - if (internalBuffersMustBeUpdated && robot->getIsInitialized() && isAttached_) + if (mustNotifyRobot_ && robot->getIsInitialized() && isAttached_) { refreshProxies(); } } } - GenericConfig AbstractMotorBase::getOptions() const noexcept + const GenericConfig & AbstractMotorBase::getOptions() const noexcept { return motorOptionsGeneric_; } @@ -196,27 +224,27 @@ namespace jiminy { if (!isAttached_) { - THROW_ERROR(bad_control_flow, - "Motor not attached to any robot. Impossible to refresh motor proxies."); + JIMINY_THROW(bad_control_flow, + "Motor not attached to any robot. Impossible to refresh motor proxies."); } auto robot = robot_.lock(); if (!robot) { - THROW_ERROR(std::runtime_error, - "Robot has been deleted. Impossible to refresh motor proxies."); + JIMINY_THROW(std::runtime_error, + "Robot has been deleted. Impossible to refresh motor proxies."); } if (!isInitialized_) { - THROW_ERROR(bad_control_flow, - "Motor not initialized. Impossible to refresh motor proxies."); + JIMINY_THROW(bad_control_flow, + "Motor not initialized. Impossible to refresh motor proxies."); } if (!robot->getIsInitialized()) { - THROW_ERROR(bad_control_flow, - "Robot not initialized. Impossible to refresh motor proxies."); + JIMINY_THROW(bad_control_flow, + "Robot not initialized. Impossible to refresh motor proxies."); } jointIndex_ = ::jiminy::getJointIndex(robot->pinocchioModel_, jointName_); @@ -226,8 +254,8 @@ namespace jiminy if (jointType_ != JointModelType::LINEAR && jointType_ != JointModelType::ROTARY && jointType_ != JointModelType::ROTARY_UNBOUNDED) { - THROW_ERROR(std::logic_error, - "A motor can only be associated with a 1-dof linear or rotary joint."); + JIMINY_THROW(std::logic_error, + "A motor can only be associated with a 1-dof linear or rotary joint."); } jointPositionIndex_ = getJointPositionFirstIndex(robot->pinocchioModel_, jointName_); @@ -236,9 +264,9 @@ namespace jiminy // Get the motor effort limits from the URDF or the user options. if (baseMotorOptions_->commandLimitFromUrdf) { - const Eigen::Index jointVelocityOrigIndex = - getJointVelocityFirstIndex(robot->pinocchioModelOrig_, jointName_); - commandLimit_ = robot->pinocchioModelOrig_.effortLimit[jointVelocityOrigIndex] / + const Eigen::Index mechanicalJointVelocityIndex = + getJointVelocityFirstIndex(robot->pinocchioModelTh_, jointName_); + commandLimit_ = robot->pinocchioModelTh_.effortLimit[mechanicalJointVelocityIndex] / baseMotorOptions_->mechanicalReduction; } else @@ -256,10 +284,22 @@ namespace jiminy armature_ = 0.0; } + // Get the transmission backlash + if (baseMotorOptions_->enableBacklash) + { + backlash_ = baseMotorOptions_->backlash; + } + else + { + backlash_ = 0.0; + } + // Propagate the user-defined motor inertia at Pinocchio model level if (notifyRobot_) { - notifyRobot_(*this); + const bool mustNotifyRobot = mustNotifyRobot_; + mustNotifyRobot_ = false; + notifyRobot_(*this, mustNotifyRobot); } } @@ -293,7 +333,7 @@ namespace jiminy // Make sure the motor is attached to a robot if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Motor not attached to any robot."); + JIMINY_THROW(bad_control_flow, "Motor not attached to any robot."); } for (AbstractMotorBase * motor : sharedStorage_->motors_) @@ -352,6 +392,11 @@ namespace jiminy return armature_; } + double AbstractMotorBase::getBacklash() const + { + return backlash_; + } + void AbstractMotorBase::computeEffortAll(double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, @@ -361,7 +406,7 @@ namespace jiminy // Make sure the motor is attached to a robot if (!isAttached_) { - THROW_ERROR(bad_control_flow, "Motor not attached to any robot."); + JIMINY_THROW(bad_control_flow, "Motor not attached to any robot."); } // Compute the actual effort of every motor diff --git a/core/src/hardware/abstract_sensor.cc b/core/src/hardware/abstract_sensor.cc index 4281fd146..29a02f8b6 100644 --- a/core/src/hardware/abstract_sensor.cc +++ b/core/src/hardware/abstract_sensor.cc @@ -11,8 +11,11 @@ namespace jiminy name_{name}, telemetrySender_{std::make_unique()} { - // Initialize the options - setOptions(getDefaultSensorOptions()); + /* Initialize options. + Note that the base implementation is called even if derived. This is a limitation of the + C++ language specification which is not going to disappear anytime soon. */ + sensorOptionsGeneric_ = getDefaultSensorOptions(); + setOptions(getOptions()); } AbstractSensorBase::~AbstractSensorBase() = default; @@ -27,18 +30,18 @@ namespace jiminy if (!isInitialized_) { - THROW_ERROR(bad_control_flow, - "Sensor '", - name_, - "' of type '", - getType(), - "' is not initialized."); + JIMINY_THROW(bad_control_flow, + "Sensor '", + name_, + "' of type '", + getType(), + "' is not initialized."); } if (!telemetryData) { - THROW_ERROR(bad_control_flow, - "Telemetry not initialized. Impossible to log sensor data."); + JIMINY_THROW(bad_control_flow, + "Telemetry not initialized. Impossible to log sensor data."); } std::string name = getTelemetryName(); @@ -81,17 +84,19 @@ namespace jiminy auto robot = robot_.lock(); if (robot && robot->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before setting sensor options."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before setting sensor options."); } - // Set sensor options - sensorOptionsGeneric_ = sensorOptions; - baseSensorOptions_ = std::make_unique(sensorOptionsGeneric_); + // Update class-specific "strongly typed" accessor for fast and convenient access + baseSensorOptions_ = std::make_unique(sensorOptions); + + // Update inherited polymorphic accessor + deepUpdate(sensorOptionsGeneric_, sensorOptions); } - GenericConfig AbstractSensorBase::getOptions() const noexcept + const GenericConfig & AbstractSensorBase::getOptions() const noexcept { return sensorOptionsGeneric_; } @@ -110,4 +115,4 @@ namespace jiminy { return name_; } -} \ No newline at end of file +} diff --git a/core/src/hardware/basic_motors.cc b/core/src/hardware/basic_motors.cc index 4092051aa..448d9685d 100644 --- a/core/src/hardware/basic_motors.cc +++ b/core/src/hardware/basic_motors.cc @@ -12,7 +12,8 @@ namespace jiminy /* AbstractMotorBase constructor calls the base implementations of the virtual methods since the derived class is not available at this point. Thus it must be called explicitly in the constructor. */ - setOptions(getDefaultMotorOptions()); + motorOptionsGeneric_ = getDefaultMotorOptions(); + setOptions(getOptions()); } void SimpleMotor::initialize(const std::string & jointName) @@ -22,9 +23,9 @@ namespace jiminy auto robot = robot_.lock(); if (robot && robot->getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before refreshing motor proxies."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before refreshing motor proxies."); } // Update joint name @@ -46,32 +47,34 @@ namespace jiminy void SimpleMotor::setOptions(const GenericConfig & motorOptions) { - AbstractMotorBase::setOptions(motorOptions); - // Check if the friction parameters make sense // Make sure the user-defined position limit has the right dimension if (boost::get(motorOptions.at("frictionViscousPositive")) > 0.0) { - THROW_ERROR(std::invalid_argument, "'frictionViscousPositive' must be negative."); + JIMINY_THROW(std::invalid_argument, "'frictionViscousPositive' must be negative."); } if (boost::get(motorOptions.at("frictionViscousNegative")) > 0.0) { - THROW_ERROR(std::invalid_argument, "'frictionViscousNegative' must be negative."); + JIMINY_THROW(std::invalid_argument, "'frictionViscousNegative' must be negative."); } if (boost::get(motorOptions.at("frictionDryPositive")) > 0.0) { - THROW_ERROR(std::invalid_argument, "'frictionDryPositive' must be negative."); + JIMINY_THROW(std::invalid_argument, "'frictionDryPositive' must be negative."); } if (boost::get(motorOptions.at("frictionDryNegative")) > 0.0) { - THROW_ERROR(std::invalid_argument, "'frictionDryNegative' must be negative."); + JIMINY_THROW(std::invalid_argument, "'frictionDryNegative' must be negative."); } if (boost::get(motorOptions.at("frictionDrySlope")) < 0.0) { - THROW_ERROR(std::invalid_argument, "'frictionDrySlope' must be positive."); + JIMINY_THROW(std::invalid_argument, "'frictionDrySlope' must be positive."); } + // Update class-specific "strongly typed" accessor for fast and convenient access motorOptions_ = std::make_unique(motorOptions); + + // Update base "strongly typed" option accessor and inherited polymorphic accessor + AbstractMotorBase::setOptions(motorOptions); } void SimpleMotor::computeEffort(double /* t */, @@ -82,8 +85,8 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, - "Motor not initialized. Impossible to compute actual motor effort."); + JIMINY_THROW(bad_control_flow, + "Motor not initialized. Impossible to compute actual motor effort."); } /* Compute the motor effort, taking into account the limit, if any. diff --git a/core/src/hardware/basic_sensors.cc b/core/src/hardware/basic_sensors.cc index 31ba1e4c2..8bbcc0d7a 100644 --- a/core/src/hardware/basic_sensors.cc +++ b/core/src/hardware/basic_sensors.cc @@ -14,48 +14,48 @@ #include "jiminy/core/hardware/basic_sensors.h" -#define GET_ROBOT_AND_CHECK_SENSOR_INTEGRITY() \ - if (!isAttached_) \ - { \ - THROW_ERROR(bad_control_flow, \ - "Sensor not attached to any robot. Impossible to refresh sensor proxies."); \ - } \ - \ - auto robot = robot_.lock(); \ - if (!robot) \ - { \ - THROW_ERROR(bad_control_flow, \ - "Robot has been deleted. Impossible to refresh sensor proxies."); \ - } \ - \ - if (!robot->getIsInitialized()) \ +#define GET_ROBOT_AND_CHECK_SENSOR_INTEGRITY() \ + if (!isAttached_) \ + { \ + JIMINY_THROW(bad_control_flow, \ + "Sensor not attached to any robot. Impossible to refresh sensor proxies."); \ + } \ + \ + auto robot = robot_.lock(); \ + if (!robot) \ + { \ + JIMINY_THROW(bad_control_flow, \ + "Robot has been deleted. Impossible to refresh sensor proxies."); \ + } \ + \ + if (!robot->getIsInitialized()) \ + { \ + JIMINY_THROW(bad_control_flow, \ + "Robot not initialized. Impossible to refresh sensor proxies."); \ + } \ + \ + if (!isInitialized_) \ + { \ + JIMINY_THROW(bad_control_flow, \ + "Sensor not initialized. Impossible to refresh sensor proxies."); \ + } + + +#define GET_ROBOT_IF_INITIALIZED() \ + if (!isInitialized_) \ { \ - THROW_ERROR(bad_control_flow, \ - "Robot not initialized. Impossible to refresh sensor proxies."); \ + JIMINY_THROW(bad_control_flow, "Sensor not initialized. Impossible to update sensor."); \ } \ \ - if (!isInitialized_) \ - { \ - THROW_ERROR(bad_control_flow, \ - "Sensor not initialized. Impossible to refresh sensor proxies."); \ - } - - -#define GET_ROBOT_IF_INITIALIZED() \ - if (!isInitialized_) \ - { \ - THROW_ERROR(bad_control_flow, "Sensor not initialized. Impossible to update sensor."); \ - } \ - \ auto robot = robot_.lock(); -#define CHECK_SIMULATION_NOT_RUNNING() \ - auto robot = robot_.lock(); \ - if (robot && robot->getIsLocked()) \ - { \ - THROW_ERROR(bad_control_flow, \ - "Robot already locked, probably because a simulation is running. " \ - "Please stop it before refreshing sensor proxies."); \ +#define CHECK_SIMULATION_NOT_RUNNING() \ + auto robot = robot_.lock(); \ + if (robot && robot->getIsLocked()) \ + { \ + JIMINY_THROW(bad_control_flow, \ + "Robot already locked, probably because a simulation is running. " \ + "Please stop it before refreshing sensor proxies."); \ } namespace jiminy @@ -66,9 +66,7 @@ namespace jiminy const std::string AbstractSensorTpl::type_{"ImuSensor"}; template<> const std::vector AbstractSensorTpl::fieldnames_{ - "Gyrox", "Gyroy", "Gyroz", "Accelx", "Accely", "Accelz"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{false}; + "GyroX", "GyroY", "GyroZ", "AccelX", "AccelY", "AccelZ"}; void ImuSensor::initialize(const std::string & frameName) { @@ -101,19 +99,19 @@ namespace jiminy boost::get(sensorOptions.at("noiseStd")); if (bias.size() && bias.size() != 9) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Wrong bias vector. It must contain 9 values:\n" " - the first three are the angle-axis representation of a rotation bias applied " "to all sensor signal.\n" " - the next six are respectively gyroscope and accelerometer additive bias."); } - if (noiseStd.size() && noiseStd.size() != 6) + if (noiseStd.size() && static_cast(noiseStd.size()) != getSize()) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Wrong noise std vector. It must contain 6 values corresponding respectively to " - "gyroscope and accelerometer additive bias."); + "gyroscope and accelerometer additive noise."); } // Set options now that sanity check were made @@ -200,8 +198,6 @@ namespace jiminy const std::string AbstractSensorTpl::type_{"ContactSensor"}; template<> const std::vector AbstractSensorTpl::fieldnames_{"FX", "FY", "FZ"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{false}; void ContactSensor::initialize(const std::string & frameName) { @@ -232,13 +228,13 @@ namespace jiminy const Eigen::VectorXd & bias = boost::get(sensorOptions.at("bias")); const Eigen::VectorXd & noiseStd = boost::get(sensorOptions.at("noiseStd")); - if (bias.size() && bias.size() != 3) + if (bias.size() && static_cast(bias.size()) != getSize()) { - THROW_ERROR(std::invalid_argument, "Wrong bias vector size."); + JIMINY_THROW(std::invalid_argument, "Wrong bias vector size."); } - if (noiseStd.size() && noiseStd.size() != 3) + if (noiseStd.size() && static_cast(noiseStd.size()) != getSize()) { - THROW_ERROR(std::invalid_argument, "Wrong noise std vector size."); + JIMINY_THROW(std::invalid_argument, "Wrong noise std vector size."); } // Set options now that sanity check were made @@ -254,9 +250,9 @@ namespace jiminy std::find(contactFrameNames.begin(), contactFrameNames.end(), frameName_); if (contactFrameNameIt == contactFrameNames.end()) { - THROW_ERROR(std::logic_error, - "Sensor frame not associated with any contact point of the robot. " - "Impossible to refresh sensor proxies."); + JIMINY_THROW(std::logic_error, + "Sensor frame not associated with any contact point of the robot. " + "Impossible to refresh sensor proxies."); } frameIndex_ = ::jiminy::getFrameIndex(robot->pinocchioModel_, frameName_); @@ -297,8 +293,6 @@ namespace jiminy template<> const std::vector AbstractSensorTpl::fieldnames_{ "FX", "FY", "FZ", "MX", "MY", "MZ"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{false}; void ForceSensor::initialize(const std::string & frameName) { @@ -329,13 +323,13 @@ namespace jiminy const Eigen::VectorXd & bias = boost::get(sensorOptions.at("bias")); const Eigen::VectorXd & noiseStd = boost::get(sensorOptions.at("noiseStd")); - if (bias.size() && bias.size() != 6) + if (bias.size() && static_cast(bias.size()) != getSize()) { - THROW_ERROR(std::invalid_argument, "Wrong bias vector size."); + JIMINY_THROW(std::invalid_argument, "Wrong bias vector size."); } - if (noiseStd.size() && noiseStd.size() != 6) + if (noiseStd.size() && static_cast(noiseStd.size()) != getSize()) { - THROW_ERROR(std::invalid_argument, "Wrong noise std vector size."); + JIMINY_THROW(std::invalid_argument, "Wrong noise std vector size."); } // Set options now that sanity check were made @@ -412,8 +406,6 @@ namespace jiminy const std::string AbstractSensorTpl::type_{"EncoderSensor"}; template<> const std::vector AbstractSensorTpl::fieldnames_{"Q", "V"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{true}; void EncoderSensor::initialize(const std::string & jointName) { @@ -444,13 +436,13 @@ namespace jiminy const Eigen::VectorXd & bias = boost::get(sensorOptions.at("bias")); const Eigen::VectorXd & noiseStd = boost::get(sensorOptions.at("noiseStd")); - if (bias.size() && bias.size() != 1) + if (bias.size() && static_cast(bias.size()) != getSize()) { - THROW_ERROR(std::invalid_argument, "Wrong bias vector size."); + JIMINY_THROW(std::invalid_argument, "Wrong bias vector size."); } - if (noiseStd.size() && noiseStd.size() != 1) + if (noiseStd.size() && static_cast(noiseStd.size()) != getSize()) { - THROW_ERROR(std::invalid_argument, "Wrong noise std vector size."); + JIMINY_THROW(std::invalid_argument, "Wrong noise std vector size."); } // Set options now that sanity check were made @@ -463,7 +455,7 @@ namespace jiminy if (!robot->pinocchioModel_.existJointName(jointName_)) { - THROW_ERROR(std::runtime_error, "Sensor attached to a joint that does not exist."); + JIMINY_THROW(std::runtime_error, "Sensor attached to a joint that does not exist."); } jointIndex_ = ::jiminy::getJointIndex(robot->pinocchioModel_, jointName_); @@ -473,7 +465,7 @@ namespace jiminy if (jointType_ != JointModelType::LINEAR && jointType_ != JointModelType::ROTARY && jointType_ != JointModelType::ROTARY_UNBOUNDED) { - THROW_ERROR( + JIMINY_THROW( std::runtime_error, "Encoder sensors can only be associated with a 1-dof linear or rotary joint."); } @@ -525,8 +517,6 @@ namespace jiminy const std::string AbstractSensorTpl::type_{"EffortSensor"}; template<> const std::vector AbstractSensorTpl::fieldnames_{"U"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{true}; void EffortSensor::initialize(const std::string & motorName) { @@ -557,13 +547,13 @@ namespace jiminy const Eigen::VectorXd & bias = boost::get(sensorOptions.at("bias")); const Eigen::VectorXd & noiseStd = boost::get(sensorOptions.at("noiseStd")); - if (bias.size() && bias.size() != 1) + if (bias.size() && static_cast(bias.size()) != getSize()) { - THROW_ERROR(std::invalid_argument, "Wrong bias vector size."); + JIMINY_THROW(std::invalid_argument, "Wrong bias vector size."); } - if (noiseStd.size() && noiseStd.size() != 1) + if (noiseStd.size() && static_cast(noiseStd.size()) != getSize()) { - THROW_ERROR(std::invalid_argument, "Wrong noise std vector size."); + JIMINY_THROW(std::invalid_argument, "Wrong noise std vector size."); } // Set options now that sanity check were made @@ -597,8 +587,8 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, - "Sensor not initialized. Impossible to set sensor data."); + JIMINY_THROW(bad_control_flow, + "Sensor not initialized. Impossible to set sensor data."); } data()[0] = uMotor[motorIndex_]; diff --git a/core/src/io/abstract_io_device.cc b/core/src/io/abstract_io_device.cc index 08c793d29..957ce0d4c 100644 --- a/core/src/io/abstract_io_device.cc +++ b/core/src/io/abstract_io_device.cc @@ -38,13 +38,13 @@ namespace jiminy { if (isOpen()) { - THROW_ERROR(bad_control_flow, "Device already open."); + JIMINY_THROW(bad_control_flow, "Device already open."); } if ((modes & supportedModes_) != modes) { - THROW_ERROR(std::invalid_argument, - "At least one of the selected modes is not supported by the device."); + JIMINY_THROW(std::invalid_argument, + "At least one of the selected modes is not supported by the device."); } doOpen(modes); @@ -55,7 +55,7 @@ namespace jiminy { if (!isOpen()) { - THROW_ERROR(bad_control_flow, "Device not open."); + JIMINY_THROW(bad_control_flow, "Device not open."); } doClose(); @@ -99,12 +99,12 @@ namespace jiminy void AbstractIODevice::resize(std::size_t /* size */) { - THROW_ERROR(not_implemented_error, "Method not available."); + JIMINY_THROW(not_implemented_error, "Method not available."); } void AbstractIODevice::seek(std::ptrdiff_t /* pos */) { - THROW_ERROR(not_implemented_error, "Method not available."); + JIMINY_THROW(not_implemented_error, "Method not available."); } std::ptrdiff_t AbstractIODevice::pos() @@ -127,7 +127,7 @@ namespace jiminy std::ptrdiff_t writtenBytes = writeData(bufferPos + (dataSize - toWrite), toWrite); if (writtenBytes <= 0) { - THROW_ERROR(std::ios_base::failure, "No data was written. Device probably full."); + JIMINY_THROW(std::ios_base::failure, "No data was written. Device probably full."); } toWrite -= writtenBytes; } @@ -143,7 +143,7 @@ namespace jiminy std::ptrdiff_t readBytes = readData(bufferPos + (dataSize - toRead), toRead); if (readBytes <= 0) { - THROW_ERROR(std::ios_base::failure, "No data was read. Device probably empty."); + JIMINY_THROW(std::ios_base::failure, "No data was read. Device probably empty."); } toRead -= readBytes; } diff --git a/core/src/io/file_device.cc b/core/src/io/file_device.cc index 4e182e183..d3ea55869 100644 --- a/core/src/io/file_device.cc +++ b/core/src/io/file_device.cc @@ -110,8 +110,8 @@ namespace jiminy const int32_t rc = ::open(filename_.c_str(), openFlags, S_IRUSR | S_IWUSR); if (rc < 0) { - THROW_ERROR(std::ios_base::failure, - "Impossible to open the file using the desired mode."); + JIMINY_THROW(std::ios_base::failure, + "Impossible to open the file using the desired mode."); } fileDescriptor_ = rc; } @@ -121,7 +121,7 @@ namespace jiminy const int32_t rc = ::close(fileDescriptor_); if (rc < 0) { - THROW_ERROR(std::ios_base::failure, "Impossible to close the file."); + JIMINY_THROW(std::ios_base::failure, "Impossible to close the file."); } fileDescriptor_ = -1; } @@ -131,10 +131,10 @@ namespace jiminy const ssize_t rc = ::lseek(fileDescriptor_, pos, SEEK_SET); if (rc < 0) { - THROW_ERROR(std::ios_base::failure, - "File not open, or requested position '", - pos, - "' is out of scope."); + JIMINY_THROW(std::ios_base::failure, + "File not open, or requested position '", + pos, + "' is out of scope."); } } @@ -143,8 +143,8 @@ namespace jiminy const ssize_t pos_cur = ::lseek(fileDescriptor_, 0, SEEK_CUR); if (pos_cur < 0) { - THROW_ERROR(std::ios_base::failure, - "File not open, or position would be negative or beyond the end."); + JIMINY_THROW(std::ios_base::failure, + "File not open, or position would be negative or beyond the end."); } return pos_cur; } @@ -155,7 +155,7 @@ namespace jiminy int32_t rc = ::fstat(fileDescriptor_, &st); if (rc < 0) { - THROW_ERROR(std::ios_base::failure, "Impossible to access the file."); + JIMINY_THROW(std::ios_base::failure, "Impossible to access the file."); } return st.st_size; } @@ -174,8 +174,8 @@ namespace jiminy const ssize_t readBytes = ::read(fileDescriptor_, data, dataSize); if (readBytes < 0) { - THROW_ERROR(std::ios_base::failure, - "File not open, or data buffer is outside accessible address space."); + JIMINY_THROW(std::ios_base::failure, + "File not open, or data buffer is outside accessible address space."); } return static_cast(readBytes); } @@ -185,8 +185,8 @@ namespace jiminy const ssize_t writtenBytes = ::write(fileDescriptor_, data, dataSize); if (writtenBytes < 0) { - THROW_ERROR(std::ios_base::failure, - "File not open, or data buffer is outside accessible address space."); + JIMINY_THROW(std::ios_base::failure, + "File not open, or data buffer is outside accessible address space."); } return writtenBytes; } @@ -201,7 +201,7 @@ namespace jiminy const int rc = ::ftruncate(fileDescriptor_, size); if (rc < 0) { - THROW_ERROR(std::ios_base::failure, "File not open."); + JIMINY_THROW(std::ios_base::failure, "File not open."); } } -} \ No newline at end of file +} diff --git a/core/src/io/json_loader.cc b/core/src/io/json_loader.cc index 6a3d9b81d..56161d6ad 100644 --- a/core/src/io/json_loader.cc +++ b/core/src/io/json_loader.cc @@ -26,7 +26,7 @@ namespace jiminy (payload_.data()), payload_.data() + payload_.size(), rootJson_.get(), &errs); if (!isParsingOk) { - THROW_ERROR(std::ios_base::failure, "Impossible to parse JSON content."); + JIMINY_THROW(std::ios_base::failure, "Impossible to parse JSON content."); } device_->close(); diff --git a/core/src/io/memory_device.cc b/core/src/io/memory_device.cc index eba9c5cee..fb9312eda 100644 --- a/core/src/io/memory_device.cc +++ b/core/src/io/memory_device.cc @@ -46,7 +46,7 @@ namespace jiminy { if ((pos < 0) || static_cast(pos) > size()) { - THROW_ERROR(std::invalid_argument, "Requested position '", pos, "' out of bound."); + JIMINY_THROW(std::invalid_argument, "Requested position '", pos, "' out of bound."); } currentPos_ = pos; } diff --git a/core/src/robot/model.cc b/core/src/robot/model.cc index 2b37d33e2..2638537e9 100644 --- a/core/src/robot/model.cc +++ b/core/src/robot/model.cc @@ -188,16 +188,42 @@ namespace jiminy Model::Model() noexcept { - setOptions(getDefaultModelOptions()); + // Initialize options + modelOptionsGeneric_ = getDefaultModelOptions(); + setOptions(getOptions()); + } + + void initializePinocchioData(const pinocchio::Model & model, pinocchio::Data & data) + { + // Re-allocate Pinocchio Data from scratch + data = pinocchio::Data(model); + + /* Initialize Pinocchio data internal state. + This includes "basic" attributes such as the mass of each body. */ + const Eigen::VectorXd qNeutral = pinocchio::neutral(model); + pinocchio::forwardKinematics(model, data, qNeutral, Eigen::VectorXd::Zero(model.nv)); + pinocchio::updateFramePlacements(model, data); + pinocchio::centerOfMass(model, data, qNeutral); } void Model::initialize(const pinocchio::Model & pinocchioModel, const std::optional & collisionModel, const std::optional & visualModel) { + // Make sure that the pinocchio model is valid if (pinocchioModel.nq == 0) { - THROW_ERROR(std::invalid_argument, "Pinocchio model must not be empty."); + JIMINY_THROW(std::invalid_argument, "Pinocchio model must not be empty."); + } + + // Make sure that the model is actually managed by a shared ptr + try + { + (void)shared_from_this(); + } + catch (std::bad_weak_ptr & e) + { + JIMINY_THROW(bad_control_flow, "Model must be managed by a std::shared_ptr."); } // Clear existing constraints @@ -212,12 +238,12 @@ namespace jiminy meshPackageDirs_.clear(); // Set the models - pinocchioModelOrig_ = pinocchioModel; - collisionModelOrig_ = collisionModel.value_or(pinocchio::GeometryModel()); - visualModelOrig_ = visualModel.value_or(pinocchio::GeometryModel()); + pinocchioModelTh_ = pinocchioModel; + collisionModelTh_ = collisionModel.value_or(pinocchio::GeometryModel()); + visualModelTh_ = visualModel.value_or(pinocchio::GeometryModel()); // Add ground geometry object to collision model is not already available - if (!collisionModelOrig_.existGeometryName("ground")) + if (!collisionModelTh_.existGeometryName("ground")) { // Instantiate ground FCL box geometry, wrapped as a pinocchio collision geometry. // Note that half-space cannot be used for Shape-Shape collision because it has no @@ -234,50 +260,36 @@ namespace jiminy pinocchio::GeometryObject groundPlane("ground", 0, 0, groudBox, groundPose); // Add the ground plane pinocchio to the robot model - collisionModelOrig_.addGeometryObject(groundPlane, pinocchioModelOrig_); + collisionModelTh_.addGeometryObject(groundPlane, pinocchioModelTh_); } - /* Re-allocate rigid data from scratch for the original rigid model. - Note that the original rigid model is not used anywhere for simulation but is - provided nonetheless to make life easier for end-users willing to perform - computations on it rather than the actual simulation model. */ - pinocchioDataOrig_ = pinocchio::Data(pinocchioModelOrig_); + /* Re-allocate data from scratch for the theoretical model. + Note that the theoretical model is not used anywhere for simulation but is exposed + nonetheless to make life easier for end-users willing to perform computations on it + rather than the actual simulation model, which is supposed to be unknown. */ + initializePinocchioData(pinocchioModelTh_, pinocchioDataTh_); - /* Initialize Pinocchio data internal state. - This includes "basic" attributes such as the mass of each body. */ - const Eigen::VectorXd qNeutralOrig = pinocchio::neutral(pinocchioModelOrig_); - pinocchio::forwardKinematics(pinocchioModelOrig_, - pinocchioDataOrig_, - qNeutralOrig, - Eigen::VectorXd::Zero(pinocchioModelOrig_.nv)); - pinocchio::updateFramePlacements(pinocchioModelOrig_, pinocchioDataOrig_); - pinocchio::centerOfMass(pinocchioModelOrig_, pinocchioDataOrig_, qNeutralOrig); - - /* Get the list of joint names of the rigid model and remove the 'universe' and - 'root_joint' if any, since they are not actual joints. */ - rigidJointNames_ = pinocchioModelOrig_.names; - rigidJointNames_.erase(rigidJointNames_.begin()); // remove 'universe' + /* Get the list of joint names of the theoretical model, the 'universe' and + 'root_joint' excluded if any, since they are not mechanical joints. */ + mechanicalJointNames_ = pinocchioModelTh_.names; + mechanicalJointNames_.erase(mechanicalJointNames_.begin()); // remove 'universe' if (hasFreeflyer_) { - rigidJointNames_.erase(rigidJointNames_.begin()); // remove 'root_joint' + mechanicalJointNames_.erase(mechanicalJointNames_.begin()); // remove 'root_joint' } - // Create the flexible model - generateModelFlexible(); - // Assuming the model is fully initialized at this point isInitialized_ = true; try { - /* Add biases to the dynamics properties of the model. - Note that is also refresh all proxies automatically. */ - generateModelBiased(std::random_device{}); + // Create the "extended" model + generateModelExtended(std::random_device{}); /* Add joint constraints. - It will be used later to enforce bounds limits eventually. */ + It will be used later to enforce bounds limits if requested. */ ConstraintMap jointConstraintsMap; - jointConstraintsMap.reserve(rigidJointNames_.size()); - for (const std::string & jointName : rigidJointNames_) + jointConstraintsMap.reserve(mechanicalJointNames_.size()); + for (const std::string & jointName : mechanicalJointNames_) { jointConstraintsMap.emplace_back(jointName, std::make_shared(jointName)); @@ -327,15 +339,17 @@ namespace jiminy constraints, at least for baumgarte stabilization. Indeed, the current frame position must be stored. */ - if (isInitialized_) + // Make sure that the model is initialized + 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(g); + JIMINY_THROW(bad_control_flow, "Model not initialized."); } + + /* Re-generate the unbiased extended model and update bias added to the dynamics + properties of the model. + Note that re-generating the unbiased extended model is necessary since the + theoretical model may have been manually modified by the user. */ + generateModelExtended(g); } void Model::addFrame(const std::string & frameName, @@ -348,118 +362,117 @@ namespace jiminy parent joint must be computed. */ // Check that no frame with the same name already exists - if (pinocchioModelOrig_.existFrame(frameName)) + if (pinocchioModelTh_.existFrame(frameName)) { - THROW_ERROR(std::invalid_argument, "Frame with same name already exists."); + JIMINY_THROW(std::invalid_argument, "Frame with same name already exists."); } - // Add frame to original rigid model + // Add frame to theoretical model { const pinocchio::FrameIndex parentFrameIndex = - getFrameIndex(pinocchioModelOrig_, parentBodyName); + getFrameIndex(pinocchioModelTh_, parentBodyName); pinocchio::JointIndex parentJointIndex = - pinocchioModelOrig_.frames[parentFrameIndex].parent; + pinocchioModelTh_.frames[parentFrameIndex].parent; const pinocchio::SE3 & parentFramePlacement = - pinocchioModelOrig_.frames[parentFrameIndex].placement; + pinocchioModelTh_.frames[parentFrameIndex].placement; const pinocchio::SE3 jointFramePlacement = parentFramePlacement.act(framePlacement); const pinocchio::Frame frame( frameName, parentJointIndex, parentFrameIndex, jointFramePlacement, frameType); - pinocchioModelOrig_.addFrame(frame); + pinocchioModelTh_.addFrame(frame); + // TODO: Do NOT re-allocate from scratch but update existing data for efficiency - pinocchioDataOrig_ = pinocchio::Data(pinocchioModelOrig_); + initializePinocchioData(pinocchioModelTh_, pinocchioDataTh_); } - // Add frame to original flexible model + /* Add frame to extended model. + Note that, appending a frame to the model does not invalid proxies, and therefore it is + unecessary to call 'reset'. */ { const pinocchio::FrameIndex parentFrameIndex = - getFrameIndex(pncModelFlexibleOrig_, parentBodyName); + getFrameIndex(pinocchioModel_, parentBodyName); pinocchio::JointIndex parentJointIndex = - pncModelFlexibleOrig_.frames[parentFrameIndex].parent; + pinocchioModel_.frames[parentFrameIndex].parent; const pinocchio::SE3 & parentFramePlacement = - pncModelFlexibleOrig_.frames[parentFrameIndex].placement; + pinocchioModel_.frames[parentFrameIndex].placement; const pinocchio::SE3 jointFramePlacement = parentFramePlacement.act(framePlacement); - const pinocchio::Frame frame( - frameName, parentJointIndex, parentFrameIndex, jointFramePlacement, frameType); - pncModelFlexibleOrig_.addFrame(frame); - } - - /* Backup the current rotor inertias and effort limits to restore them. - Note that it is only necessary because 'reset' is not called for efficiency. It is - reasonable to assume that no other fields have been overriden by derived classes - such as Robot. */ - Eigen::VectorXd rotorInertia = pinocchioModel_.rotorInertia; - Eigen::VectorXd effortLimit = pinocchioModel_.effortLimit; + const pinocchio::Frame frame(frameName, + parentJointIndex, + parentFrameIndex, + jointFramePlacement, + pinocchio::FrameType::OP_FRAME); + pinocchioModel_.addFrame(frame); - /* One must re-generate the model after adding a frame. - Note that, since the added frame being the "last" of the model, the proxies are - still up-to-date and therefore it is unecessary to call 'reset'. */ - generateModelBiased(std::random_device{}); - - // Restore the current rotor inertias and effort limits - pinocchioModel_.rotorInertia.swap(rotorInertia); - pinocchioModel_.effortLimit.swap(effortLimit); - } - - void Model::addFrame(const std::string & frameName, - const std::string & parentBodyName, - const pinocchio::SE3 & framePlacement) - { - const pinocchio::FrameType frameType = pinocchio::FrameType::OP_FRAME; - return addFrame(frameName, parentBodyName, framePlacement, frameType); + // TODO: Do NOT re-allocate from scratch but update existing data for efficiency + initializePinocchioData(pinocchioModel_, pinocchioData_); + } } - void Model::removeFrames(const std::vector & frameNames) + void Model::removeFrames(const std::vector & frameNames, + const std::vector & filter) { - /* Check that the frame can be safely removed from the original rigid model. - If so, it is also the case for the original flexible models. */ - for (const std::string & frameName : frameNames) + /* Check that the frame can be safely removed from the theoretical model. + If so, then it holds true for the extended model. */ + if (!filter.empty()) { - const pinocchio::FrameType frameType = pinocchio::FrameType::OP_FRAME; - pinocchio::FrameIndex frameIndex = getFrameIndex(pinocchioModelOrig_, frameName); - if (pinocchioModelOrig_.frames[frameIndex].type != frameType) + for (const std::string & frameName : frameNames) { - THROW_ERROR(std::logic_error, "Only frames manually added can be removed."); + const pinocchio::FrameIndex frameIndex = + getFrameIndex(pinocchioModelTh_, frameName); + const pinocchio::FrameType frameType = pinocchioModelTh_.frames[frameIndex].type; + if (std::find(filter.begin(), filter.end(), frameType) != filter.end()) + { + JIMINY_THROW(std::logic_error, + "Not allowed to remove frame '", + frameName, + "' of type '", + frameType, + "'."); + } } } for (const std::string & frameName : frameNames) { - // Remove frame from original rigid model + // Remove frame from the theoretical model { const pinocchio::FrameIndex frameIndex = - getFrameIndex(pinocchioModelOrig_, frameName); - pinocchioModelOrig_.frames.erase(std::next(pinocchioModelOrig_.frames.begin(), - static_cast(frameIndex))); - pinocchioModelOrig_.nframes--; + getFrameIndex(pinocchioModelTh_, frameName); + pinocchioModelTh_.frames.erase( + std::next(pinocchioModelTh_.frames.begin(), frameIndex)); + pinocchioModelTh_.nframes--; } - // Remove frame from original flexible model + // Remove frame from the extended model { - const pinocchio::FrameIndex frameIndex = - getFrameIndex(pncModelFlexibleOrig_, frameName); - pncModelFlexibleOrig_.frames.erase( - std::next(pncModelFlexibleOrig_.frames.begin(), frameIndex)); - pncModelFlexibleOrig_.nframes--; + const pinocchio::FrameIndex frameIndex = getFrameIndex(pinocchioModel_, frameName); + pinocchioModel_.frames.erase( + std::next(pinocchioModel_.frames.begin(), frameIndex)); + pinocchioModel_.nframes--; } } // TODO: Do NOT re-allocate from scratch but update existing data for efficiency - pinocchioDataOrig_ = pinocchio::Data(pinocchioModelOrig_); + initializePinocchioData(pinocchioModel_, pinocchioData_); + initializePinocchioData(pinocchioModelTh_, pinocchioDataTh_); + } - // One must reset the model after removing a frame - reset(std::random_device{}); + void Model::addFrame(const std::string & frameName, + const std::string & parentBodyName, + const pinocchio::SE3 & framePlacement) + { + return addFrame(frameName, parentBodyName, framePlacement, pinocchio::FrameType::OP_FRAME); } - void Model::removeFrame(const std::string & frameName) + void Model::removeFrames(const std::vector & frameNames) { - return removeFrames({frameName}); + removeFrames(frameNames, {pinocchio::FrameType::OP_FRAME}); } void Model::addCollisionBodies(const std::vector & bodyNames, bool ignoreMeshes) { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Model not initialized."); + JIMINY_THROW(bad_control_flow, "Model not initialized."); } // Returning early if nothing to do @@ -469,24 +482,24 @@ namespace jiminy } // If successfully loaded, the ground should be available - if (collisionModelOrig_.ngeoms == 0) + if (collisionModelTh_.ngeoms == 0) { - THROW_ERROR(std::runtime_error, - "Collision geometry not available. Some collision meshes were " - "probably not found."); + JIMINY_THROW(std::runtime_error, + "Collision geometry not available. Some collision meshes were " + "probably not found."); } // Make sure that no body are duplicates if (checkDuplicates(bodyNames)) { - THROW_ERROR(std::invalid_argument, "Duplicated bodies found."); + JIMINY_THROW(std::invalid_argument, "Duplicated bodies found."); } // Make sure there is no collision already associated with any of the bodies in the list if (checkIntersection(collisionBodyNames_, bodyNames)) { - THROW_ERROR(std::invalid_argument, - "At least one of the bodies already associated with a collision."); + JIMINY_THROW(std::invalid_argument, + "At least one of the bodies already associated with a collision."); } // Make sure that all the bodies exist @@ -494,7 +507,7 @@ namespace jiminy { if (!pinocchioModel_.existBodyName(name)) { - THROW_ERROR(std::invalid_argument, "At least one of the bodies does not exist."); + JIMINY_THROW(std::invalid_argument, "At least one of the bodies does not exist."); } } @@ -502,7 +515,7 @@ namespace jiminy for (const std::string & name : bodyNames) { bool hasGeometry = false; - for (const pinocchio::GeometryObject & geom : collisionModelOrig_.geometryObjects) + for (const pinocchio::GeometryObject & geom : collisionModelTh_.geometryObjects) { const bool isGeomMesh = (geom.meshPath.find('/') != std::string::npos || geom.meshPath.find('\\') != std::string::npos); @@ -516,9 +529,9 @@ namespace jiminy } if (!hasGeometry) { - THROW_ERROR(std::invalid_argument, - "At least one of the bodies not associated with any collision " - "geometry of requested type."); + JIMINY_THROW(std::invalid_argument, + "At least one of the bodies not associated with any collision " + "geometry of requested type."); } } @@ -526,14 +539,14 @@ namespace jiminy collisionBodyNames_.insert(collisionBodyNames_.end(), bodyNames.begin(), bodyNames.end()); // Create the collision pairs and add them to the geometry model of the robot - const pinocchio::GeomIndex & groundIndex = collisionModelOrig_.getGeometryId("ground"); + const pinocchio::GeomIndex & groundIndex = collisionModelTh_.getGeometryId("ground"); for (const std::string & name : bodyNames) { // Add a collision pair for all geometries having the body as parent ConstraintMap collisionConstraintsMap; - for (std::size_t i = 0; i < collisionModelOrig_.geometryObjects.size(); ++i) + for (std::size_t i = 0; i < collisionModelTh_.geometryObjects.size(); ++i) { - const pinocchio::GeometryObject & geom = collisionModelOrig_.geometryObjects[i]; + const pinocchio::GeometryObject & geom = collisionModelTh_.geometryObjects[i]; const bool isGeomMesh = (geom.meshPath.find('/') != std::string::npos || geom.meshPath.find('\\') != std::string::npos); const std::string & frameName = pinocchioModel_.frames[geom.parentFrame].name; @@ -547,12 +560,12 @@ namespace jiminy Note that the ground always comes second for the normal to be consistently compute wrt the ground instead of the body. */ const pinocchio::CollisionPair collisionPair(i, groundIndex); - collisionModelOrig_.addCollisionPair(collisionPair); + collisionModelTh_.addCollisionPair(collisionPair); /* Add dedicated frame. - Note that 'BODY' type is used instead of default 'OP_FRAME' to it - clear it is not consider as manually added to the model, and - therefore cannot be deleted by the user. */ + Note that 'FIXED_JOINT' type is used instead of default 'OP_FRAME' to + avoid considering it as manually added to the model, and therefore + prevent its deletion by the user. */ const pinocchio::FrameType frameType = pinocchio::FrameType::FIXED_JOINT; addFrame(geom.name, frameName, geom.placement, frameType); @@ -584,20 +597,20 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Model not initialized."); + JIMINY_THROW(bad_control_flow, "Model not initialized."); } // Make sure that no body are duplicates if (checkDuplicates(bodyNames)) { - THROW_ERROR(std::invalid_argument, "Duplicated bodies found."); + JIMINY_THROW(std::invalid_argument, "Duplicated bodies found."); } // Make sure that every body in the list is associated with a collision if (!checkInclusion(collisionBodyNames_, bodyNames)) { - THROW_ERROR(std::invalid_argument, - "At least one of the bodies not associated with any collision."); + JIMINY_THROW(std::invalid_argument, + "At least one of the bodies not associated with any collision."); } /* Remove the list of bodies from the set of collision bodies, then remove the associated @@ -619,18 +632,18 @@ namespace jiminy // Get indices of corresponding collision pairs in geometry model of robot and remove them std::vector collisionConstraintNames; - const pinocchio::GeomIndex & groundIndex = collisionModelOrig_.getGeometryId("ground"); + const pinocchio::GeomIndex & groundIndex = collisionModelTh_.getGeometryId("ground"); for (const std::string & name : bodyNames) { // Remove the collision pair for all the geometries having the body as parent - for (std::size_t i = 0; i < collisionModelOrig_.geometryObjects.size(); ++i) + for (std::size_t i = 0; i < collisionModelTh_.geometryObjects.size(); ++i) { - const pinocchio::GeometryObject & geom = collisionModelOrig_.geometryObjects[i]; + const pinocchio::GeometryObject & geom = collisionModelTh_.geometryObjects[i]; if (pinocchioModel_.frames[geom.parentFrame].name == name) { // Remove the collision pair with the ground const pinocchio::CollisionPair collisionPair(i, groundIndex); - collisionModelOrig_.removeCollisionPair(collisionPair); + collisionModelTh_.removeCollisionPair(collisionPair); // Append collision geometry to the list of constraints to remove if (constraints_.exist(geom.name, ConstraintNodeType::COLLISION_BODIES)) @@ -643,7 +656,7 @@ namespace jiminy // Remove the constraints and associated frames removeConstraints(collisionConstraintNames, ConstraintNodeType::COLLISION_BODIES); - removeFrames(collisionConstraintNames); + removeFrames(collisionConstraintNames, {pinocchio::FrameType::FIXED_JOINT}); // Refresh proxies associated with the collisions only refreshGeometryProxies(); @@ -653,20 +666,20 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Model not initialized."); + JIMINY_THROW(bad_control_flow, "Model not initialized."); } // Make sure that no frame are duplicates if (checkDuplicates(frameNames)) { - THROW_ERROR(std::invalid_argument, "Duplicated frames found."); + JIMINY_THROW(std::invalid_argument, "Duplicated frames found."); } // Make sure that there is no contact already associated with any of the frames in the list if (checkIntersection(contactFrameNames_, frameNames)) { - THROW_ERROR(std::invalid_argument, - "At least one of the frames already associated with a contact."); + JIMINY_THROW(std::invalid_argument, + "At least one of the frames already associated with a contact."); } // Make sure that all the frames exist @@ -674,7 +687,7 @@ namespace jiminy { if (!pinocchioModel_.existFrame(name)) { - THROW_ERROR(std::invalid_argument, "At least one of the frames does not exist."); + JIMINY_THROW(std::invalid_argument, "At least one of the frames does not exist."); } } @@ -701,20 +714,20 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Model not initialized."); + JIMINY_THROW(bad_control_flow, "Model not initialized."); } // Make sure that no frame are duplicates if (checkDuplicates(frameNames)) { - THROW_ERROR(std::invalid_argument, "Duplicated frames found."); + JIMINY_THROW(std::invalid_argument, "Duplicated frames found."); } // Make sure that every frame in the list is associated with a contact if (!checkInclusion(contactFrameNames_, frameNames)) { - THROW_ERROR(std::invalid_argument, - "At least one of the frames not associated with a contact."); + JIMINY_THROW(std::invalid_argument, + "At least one of the frames not associated with a contact."); } /* Remove the constraint associated with contact frame, then remove the list of frames from @@ -741,17 +754,17 @@ namespace jiminy { if (!constraintPtr) { - THROW_ERROR(std::invalid_argument, - "Constraint named '", - constraintName, - "' is undefined."); + JIMINY_THROW(std::invalid_argument, + "Constraint named '", + constraintName, + "' is undefined."); } if (constraints_.exist(constraintName)) { - THROW_ERROR(std::invalid_argument, - "A constraint named '", - constraintName, - "' already exists."); + JIMINY_THROW(std::invalid_argument, + "A constraint named '", + constraintName, + "' already exists."); } } @@ -797,15 +810,15 @@ namespace jiminy { if (node == ConstraintNodeType::USER) { - THROW_ERROR(std::invalid_argument, - "No user-registered constraint with name '", - constraintName, - "' exists."); + JIMINY_THROW(std::invalid_argument, + "No user-registered constraint with name '", + constraintName, + "' exists."); } - THROW_ERROR(std::invalid_argument, - "No internal constraint with name '", - constraintName, - "' exists."); + JIMINY_THROW(std::invalid_argument, + "No internal constraint with name '", + constraintName, + "' exists."); } } @@ -839,7 +852,7 @@ namespace jiminy std::shared_ptr constraint = constraints_.get(constraintName); if (!constraint) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "No constraint with name '", constraintName, "' exists."); } return constraint; @@ -853,7 +866,7 @@ namespace jiminy const_cast(constraints_).get(constraintName)); if (!constraint.lock()) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "No constraint with name '", constraintName, "' exists."); } return constraint; @@ -881,110 +894,127 @@ namespace jiminy ConstraintNodeType /* node */) { constraint->disable(); }); } - void Model::generateModelFlexible() + void Model::generateModelExtended(const uniform_random_bit_generator_ref & g) { - // Copy the original model - pncModelFlexibleOrig_ = pinocchioModelOrig_; + // Make sure the model is initialized + if (!isInitialized_) + { + JIMINY_THROW(bad_control_flow, "Model not initialized."); + } + + // Initialize the extended model + initializeExtendedModel(); + + // Add flexibility joints to the extended model if requested + if (modelOptions_->dynamics.enableFlexibility) + { + addFlexibilityJointsToExtendedModel(); + } + + // Add biases to the dynamics properties of the model + addBiasedToExtendedModel(g); + + // Refresh internal proxies + refreshProxies(); + } + + void Model::initializeExtendedModel() + { + // Clear some proxies + backlashJointIndices_.clear(); + backlashJointNames_.clear(); + flexibilityJointIndices_.clear(); + flexibilityJointNames_.clear(); + + // Simply copy the theoretical model by default + pinocchioModel_ = pinocchioModelTh_; + + // Initially set effortLimit to zero systematically + pinocchioModel_.effortLimit.setZero(); + } + void Model::addFlexibilityJointsToExtendedModel() + { // Check that the frames exist - for (const FlexibleJointData & flexibleJoint : modelOptions_->dynamics.flexibilityConfig) + for (const FlexibilityJointConfig & flexibilityJoint : + modelOptions_->dynamics.flexibilityConfig) { - const std::string & frameName = flexibleJoint.frameName; - if (!pinocchioModelOrig_.existFrame(frameName)) + const std::string & frameName = flexibilityJoint.frameName; + if (!pinocchioModelTh_.existFrame(frameName)) { - THROW_ERROR(std::logic_error, - "Frame '", - frameName, - "' does not exists. Impossible to insert flexible joint on it."); + JIMINY_THROW(std::logic_error, + "Frame '", + frameName, + "' does not exists. Impossible to insert flexibility joint on it."); } } - // Add all the flexible joints - flexibleJointNames_.clear(); - for (const FlexibleJointData & flexibleJoint : modelOptions_->dynamics.flexibilityConfig) + // Add all the flexibility joints + for (const FlexibilityJointConfig & flexibilityJoint : + modelOptions_->dynamics.flexibilityConfig) { // Extract some proxies - const std::string & frameName = flexibleJoint.frameName; + const std::string & frameName = flexibilityJoint.frameName; std::string flexName = frameName; - const pinocchio::FrameIndex frameIndex = - getFrameIndex(pncModelFlexibleOrig_, frameName); - const pinocchio::Frame & frame = pncModelFlexibleOrig_.frames[frameIndex]; + const pinocchio::FrameIndex frameIndex = getFrameIndex(pinocchioModel_, frameName); + const pinocchio::Frame & frame = pinocchioModel_.frames[frameIndex]; // Add joint to model, differently depending on its type if (frame.type == pinocchio::FrameType::FIXED_JOINT) { - // Insert flexible joint at fixed frame, splitting "composite" body inertia - insertFlexibilityAtFixedFrameInModel(pncModelFlexibleOrig_, frameName); + // Insert flexibility joint at fixed frame, splitting "composite" body inertia + addFlexibilityJointAtFixedFrame(pinocchioModel_, frameName); } else if (frame.type == pinocchio::FrameType::JOINT) { flexName += FLEXIBLE_JOINT_SUFFIX; - insertFlexibilityBeforeJointInModel(pncModelFlexibleOrig_, frameName, flexName); + addFlexibilityJointBeforeMechanicalJoint(pinocchioModel_, frameName, flexName); } else { - THROW_ERROR(std::logic_error, - "Flexible joint can only be inserted at fixed or joint frames."); + JIMINY_THROW(std::logic_error, + "Flexible joint can only be inserted at fixed or joint frames."); } - flexibleJointNames_.push_back(flexName); + flexibilityJointNames_.push_back(flexName); } - // Compute flexible joint indices - flexibleJointIndices_ = getJointIndices(pncModelFlexibleOrig_, flexibleJointNames_); + // Compute flexibility joint indices + std::vector flexibilityJointIndices = + getJointIndices(pinocchioModel_, flexibilityJointNames_); // Add flexibility armature-like inertia to the model - for (std::size_t i = 0; i < flexibleJointIndices_.size(); ++i) + for (std::size_t i = 0; i < flexibilityJointIndices.size(); ++i) { - const FlexibleJointData & flexibleJoint = modelOptions_->dynamics.flexibilityConfig[i]; + const FlexibilityJointConfig & flexibilityJoint = + modelOptions_->dynamics.flexibilityConfig[i]; const pinocchio::JointModel & jmodel = - pncModelFlexibleOrig_.joints[flexibleJointIndices_[i]]; - jmodel.jointVelocitySelector(pncModelFlexibleOrig_.rotorInertia) = - flexibleJoint.inertia; + pinocchioModel_.joints[flexibilityJointIndices[i]]; + jmodel.jointVelocitySelector(pinocchioModel_.rotorInertia) = flexibilityJoint.inertia; } // Check that the armature inertia is valid - for (pinocchio::JointIndex flexibleJointIndex : flexibleJointIndices_) + for (pinocchio::JointIndex flexibilityJointIndex : flexibilityJointIndices) { - const pinocchio::Inertia & flexibleInertia = - pncModelFlexibleOrig_.inertias[flexibleJointIndex]; - const pinocchio::JointModel & jmodel = - pncModelFlexibleOrig_.joints[flexibleJointIndex]; + const pinocchio::Inertia & flexibilityInertia = + pinocchioModel_.inertias[flexibilityJointIndex]; + const pinocchio::JointModel & jmodel = pinocchioModel_.joints[flexibilityJointIndex]; const Eigen::Vector3d inertiaDiag = - jmodel.jointVelocitySelector(pncModelFlexibleOrig_.rotorInertia) + - flexibleInertia.inertia().matrix().diagonal(); + jmodel.jointVelocitySelector(pinocchioModel_.rotorInertia) + + flexibilityInertia.inertia().matrix().diagonal(); if ((inertiaDiag.array() < 1e-5).any()) { - THROW_ERROR(std::runtime_error, - "The subtree diagonal inertia for flexibility joint ", - flexibleJointIndex, - " must be larger than 1e-5 for numerical stability: ", - inertiaDiag.transpose()); + JIMINY_THROW(std::runtime_error, + "The subtree diagonal inertia for flexibility joint ", + flexibilityJointIndex, + " must be larger than 1e-5 for numerical stability: ", + inertiaDiag.transpose()); } } } - void Model::generateModelBiased(const uniform_random_bit_generator_ref & g) + void Model::addBiasedToExtendedModel(const uniform_random_bit_generator_ref & g) { - // Make sure the model is initialized - if (!isInitialized_) - { - THROW_ERROR(bad_control_flow, "Model not initialized."); - } - - // Reset the robot either with the original rigid or flexible model - if (modelOptions_->dynamics.enableFlexibleModel) - { - pinocchioModel_ = pncModelFlexibleOrig_; - } - else - { - pinocchioModel_ = pinocchioModelOrig_; - } - - // Initially set effortLimit to zero systematically - pinocchioModel_.effortLimit.setZero(); - - for (const std::string & jointName : rigidJointNames_) + for (const std::string & jointName : mechanicalJointNames_) { const pinocchio::JointIndex jointIndex = ::jiminy::getJointIndex(pinocchioModel_, jointName); @@ -1050,18 +1080,8 @@ namespace jiminy } } - // Re-allocate rigid data from scratch - pinocchioData_ = pinocchio::Data(pinocchioModel_); - - // Initialize Pinocchio Data internal state - const Eigen::VectorXd qNeutral = pinocchio::neutral(pinocchioModel_); - pinocchio::forwardKinematics( - pinocchioModel_, pinocchioData_, qNeutral, Eigen::VectorXd::Zero(pinocchioModel_.nv)); - pinocchio::updateFramePlacements(pinocchioModel_, pinocchioData_); - pinocchio::centerOfMass(pinocchioModel_, pinocchioData_, qNeutral); - - // Refresh internal proxies - refreshProxies(); + // Re-allocate data to be consistent with new extended model + initializePinocchioData(pinocchioModel_, pinocchioData_); } void Model::computeConstraints(const Eigen::VectorXd & q, const Eigen::VectorXd & v) @@ -1117,9 +1137,10 @@ namespace jiminy void Model::refreshProxies() { + // Make sure that the model is initialized if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Model not initialized."); + JIMINY_THROW(bad_control_flow, "Model not initialized."); } // Extract the dimensions of the configuration and velocity vectors @@ -1127,12 +1148,16 @@ namespace jiminy nv_ = pinocchioModel_.nv; nx_ = nq_ + nv_; - // Extract some rigid joints indices in the model - rigidJointIndices_ = getJointIndices(pinocchioModel_, rigidJointNames_); - rigidJointPositionIndices_ = - getJointsPositionIndices(pinocchioModel_, rigidJointNames_, false); - rigidJointVelocityIndices_ = - getJointsVelocityIndices(pinocchioModel_, rigidJointNames_, false); + // Extract some mechanical joints indices in the model + mechanicalJointIndices_ = getJointIndices(pinocchioModel_, mechanicalJointNames_); + mechanicalJointPositionIndices_ = + getJointsPositionIndices(pinocchioModel_, mechanicalJointNames_, false); + mechanicalJointVelocityIndices_ = + getJointsVelocityIndices(pinocchioModel_, mechanicalJointNames_, false); + + // Compute flexibility and backlash joint indices + backlashJointIndices_ = getJointIndices(pinocchioModel_, backlashJointNames_); + flexibilityJointIndices_ = getJointIndices(pinocchioModel_, flexibilityJointNames_); /* Generate the fieldnames associated with the configuration vector, velocity, acceleration and external force vectors. */ @@ -1189,34 +1214,37 @@ namespace jiminy } } - /* Get the joint position limits from the URDF or the user options. - Do NOT use robot_->pinocchioModel_.(lower|upper)PositionLimit. */ + // Get mechanical joint position limits from the URDF or user options positionLimitMin_.setConstant(pinocchioModel_.nq, -INF); positionLimitMax_.setConstant(pinocchioModel_.nq, +INF); - - if (modelOptions_->joints.enablePositionLimit) + if (modelOptions_->joints.positionLimitFromUrdf) { - if (modelOptions_->joints.positionLimitFromUrdf) + for (Eigen::Index positionIndex : mechanicalJointPositionIndices_) { - for (Eigen::Index positionIndex : rigidJointPositionIndices_) - { - positionLimitMin_[positionIndex] = - pinocchioModel_.lowerPositionLimit[positionIndex]; - positionLimitMax_[positionIndex] = - pinocchioModel_.upperPositionLimit[positionIndex]; - } + positionLimitMin_[positionIndex] = + pinocchioModel_.lowerPositionLimit[positionIndex]; + positionLimitMax_[positionIndex] = + pinocchioModel_.upperPositionLimit[positionIndex]; } - else + } + else + { + for (std::size_t i = 0; i < mechanicalJointPositionIndices_.size(); ++i) { - for (std::size_t i = 0; i < rigidJointPositionIndices_.size(); ++i) - { - Eigen::Index positionIndex = rigidJointPositionIndices_[i]; - positionLimitMin_[positionIndex] = modelOptions_->joints.positionLimitMin[i]; - positionLimitMax_[positionIndex] = modelOptions_->joints.positionLimitMax[i]; - } + Eigen::Index positionIndex = mechanicalJointPositionIndices_[i]; + positionLimitMin_[positionIndex] = modelOptions_->joints.positionLimitMin[i]; + positionLimitMax_[positionIndex] = modelOptions_->joints.positionLimitMax[i]; } } + // Get the backlash joint position limits + for (pinocchio::JointIndex jointIndex : backlashJointIndices_) + { + const Eigen::Index positionIndex = pinocchioModel_.idx_qs[jointIndex]; + positionLimitMin_[positionIndex] = pinocchioModel_.lowerPositionLimit[positionIndex]; + positionLimitMax_[positionIndex] = pinocchioModel_.upperPositionLimit[positionIndex]; + } + /* Overwrite the position bounds for some specific joint type, mainly due to quaternion normalization and cos/sin representation. */ Eigen::Index idx_q, nq; @@ -1233,11 +1261,11 @@ namespace jiminy idx_q = joint.idx_q() + 3; nq = 4; break; - case JointModelType::UNSUPPORTED: case JointModelType::LINEAR: case JointModelType::ROTARY: case JointModelType::PLANAR: case JointModelType::TRANSLATION: + case JointModelType::UNSUPPORTED: default: continue; } @@ -1251,16 +1279,16 @@ namespace jiminy { if (modelOptions_->joints.velocityLimitFromUrdf) { - for (Eigen::Index & velocityIndex : rigidJointVelocityIndices_) + for (Eigen::Index & velocityIndex : mechanicalJointVelocityIndices_) { velocityLimit_[velocityIndex] = pinocchioModel_.velocityLimit[velocityIndex]; } } else { - for (std::size_t i = 0; i < rigidJointVelocityIndices_.size(); ++i) + for (std::size_t i = 0; i < mechanicalJointVelocityIndices_.size(); ++i) { - Eigen::Index velocityIndex = rigidJointVelocityIndices_[i]; + Eigen::Index velocityIndex = mechanicalJointVelocityIndices_[i]; velocityLimit_[velocityIndex] = modelOptions_->joints.velocityLimit[i]; } } @@ -1275,52 +1303,46 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Model not initialized."); + JIMINY_THROW(bad_control_flow, "Model not initialized."); } // Restore collision and visual models - collisionModel_ = collisionModelOrig_; - visualModel_ = visualModelOrig_; + collisionModel_ = collisionModelTh_; + visualModel_ = visualModelTh_; - // Update joint/frame fix for every geometry objects - if (modelOptions_->dynamics.enableFlexibleModel && - !modelOptions_->dynamics.flexibilityConfig.empty()) + // Update joint/frame for every geometry objects + for (pinocchio::GeometryModel * model : std::array{&collisionModel_, &visualModel_}) { - for (pinocchio::GeometryModel * model : std::array{&collisionModel_, &visualModel_}) + for (pinocchio::GeometryObject & geom : model->geometryObjects) { - for (pinocchio::GeometryObject & geom : model->geometryObjects) + // Only the frame name remains unchanged no matter what + const pinocchio::Frame & frameOrig = pinocchioModelTh_.frames[geom.parentFrame]; + const std::string parentJointName = pinocchioModelTh_.names[frameOrig.parent]; + pinocchio::FrameType frameType = + static_cast(pinocchio::FIXED_JOINT | pinocchio::BODY); + const pinocchio::FrameIndex frameIndex = + getFrameIndex(pinocchioModel_, frameOrig.name, frameType); + const pinocchio::Frame & frame = pinocchioModel_.frames[frameIndex]; + const pinocchio::JointIndex newParentModelIndex = frame.parent; + const pinocchio::JointIndex oldParentModelIndex = + pinocchioModel_.getJointId(parentJointName); + + geom.parentFrame = frameIndex; + geom.parentJoint = newParentModelIndex; + + /* Compute the relative displacement between the new and old joint + placement wrt their common parent joint. */ + pinocchio::SE3 geomPlacementRef = pinocchio::SE3::Identity(); + for (pinocchio::JointIndex i = newParentModelIndex; i > oldParentModelIndex; + i = pinocchioModel_.parents[i]) { - // Only the frame name remains unchanged no matter what - const pinocchio::Frame & frameOrig = - pinocchioModelOrig_.frames[geom.parentFrame]; - const std::string parentJointName = - pinocchioModelOrig_.names[frameOrig.parent]; - pinocchio::FrameType frameType = static_cast( - pinocchio::FIXED_JOINT | pinocchio::BODY); - const pinocchio::FrameIndex frameIndex = - getFrameIndex(pinocchioModel_, frameOrig.name, frameType); - const pinocchio::Frame & frame = pinocchioModel_.frames[frameIndex]; - const pinocchio::JointIndex newParentModelIndex = frame.parent; - const pinocchio::JointIndex oldParentModelIndex = - pinocchioModel_.getJointId(parentJointName); - - geom.parentFrame = frameIndex; - geom.parentJoint = newParentModelIndex; - - /* Compute the relative displacement between the new and old joint - placement wrt their common parent joint. */ - pinocchio::SE3 geomPlacementRef = pinocchio::SE3::Identity(); - for (pinocchio::JointIndex i = newParentModelIndex; i > oldParentModelIndex; - i = pinocchioModel_.parents[i]) - { - geomPlacementRef = pinocchioModel_.jointPlacements[i] * geomPlacementRef; - } - geom.placement = geomPlacementRef.actInv(geom.placement); + geomPlacementRef = pinocchioModel_.jointPlacements[i] * geomPlacementRef; } + geom.placement = geomPlacementRef.actInv(geom.placement); } } - /* Update geometry data object after changing the collision pairs + /* Update geometry data object after changing the collision pairs. Note that copy assignment is used to avoid changing memory pointers, which would result in dangling reference at Python-side. */ collisionData_ = pinocchio::GeometryData(collisionModel_); @@ -1362,7 +1384,7 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Model not initialized."); + JIMINY_THROW(bad_control_flow, "Model not initialized."); } // Reset the contact force internal buffer @@ -1391,44 +1413,45 @@ namespace jiminy // Check dimensions consistency if (J.cols() != pinocchioModel_.nv) { - THROW_ERROR(std::logic_error, - "Constraint has inconsistent jacobian and drift (size mismatch)."); + JIMINY_THROW( + std::logic_error, + "Constraint has inconsistent jacobian and drift (size mismatch)."); } }); } - void Model::setOptions(GenericConfig modelOptions) + void Model::setOptions(const GenericConfig & modelOptions) { bool internalBuffersMustBeUpdated = false; - bool areModelsInvalid = false; + bool isExtendedModelInvalid = false; bool isCollisionDataInvalid = false; if (isInitialized_) { /* Check that the following user parameters has the right dimension, then update the required internal buffers to reflect changes, if any. */ - GenericConfig & jointOptionsHolder = + const GenericConfig & jointOptionsHolder = boost::get(modelOptions.at("joints")); bool positionLimitFromUrdf = boost::get(jointOptionsHolder.at("positionLimitFromUrdf")); if (!positionLimitFromUrdf) { - Eigen::VectorXd & jointsPositionLimitMin = + const Eigen::VectorXd & jointsPositionLimitMin = boost::get(jointOptionsHolder.at("positionLimitMin")); - if (rigidJointPositionIndices_.size() != + if (mechanicalJointPositionIndices_.size() != static_cast(jointsPositionLimitMin.size())) { - THROW_ERROR(std::invalid_argument, - "Wrong vector size for 'positionLimitMin'."); + JIMINY_THROW(std::invalid_argument, + "Wrong vector size for 'positionLimitMin'."); } - Eigen::VectorXd & jointsPositionLimitMax = + const Eigen::VectorXd & jointsPositionLimitMax = boost::get(jointOptionsHolder.at("positionLimitMax")); - if (rigidJointPositionIndices_.size() != + if (mechanicalJointPositionIndices_.size() != static_cast(jointsPositionLimitMax.size())) { - THROW_ERROR(std::invalid_argument, - "Wrong vector size for 'positionLimitMax'."); + JIMINY_THROW(std::invalid_argument, + "Wrong vector size for 'positionLimitMax'."); } - if (rigidJointPositionIndices_.size() == + if (mechanicalJointPositionIndices_.size() == static_cast(modelOptions_->joints.positionLimitMin.size())) { auto jointsPositionLimitMinDiff = @@ -1449,14 +1472,14 @@ namespace jiminy boost::get(jointOptionsHolder.at("velocityLimitFromUrdf")); if (!velocityLimitFromUrdf) { - Eigen::VectorXd & jointsVelocityLimit = + const Eigen::VectorXd & jointsVelocityLimit = boost::get(jointOptionsHolder.at("velocityLimit")); - if (rigidJointVelocityIndices_.size() != + if (mechanicalJointVelocityIndices_.size() != static_cast(jointsVelocityLimit.size())) { - THROW_ERROR(std::invalid_argument, "Wrong vector size for 'velocityLimit'."); + JIMINY_THROW(std::invalid_argument, "Wrong vector size for 'velocityLimit'."); } - if (rigidJointVelocityIndices_.size() == + if (mechanicalJointVelocityIndices_.size() == static_cast(modelOptions_->joints.velocityLimit.size())) { auto jointsVelocityLimitDiff = @@ -1471,7 +1494,7 @@ namespace jiminy } // Check if deformation points are all associated with different joints/frames - GenericConfig & dynOptionsHolder = + const GenericConfig & dynOptionsHolder = boost::get(modelOptions.at("dynamics")); const FlexibilityConfig & flexibilityConfig = boost::get(dynOptionsHolder.at("flexibilityConfig")); @@ -1479,43 +1502,36 @@ namespace jiminy std::transform(flexibilityConfig.begin(), flexibilityConfig.end(), std::inserter(flexibilityNames, flexibilityNames.begin()), - [](const FlexibleJointData & flexiblePoint) -> std::string - { return flexiblePoint.frameName; }); + [](const FlexibilityJointConfig & flexibilityJoint) -> std::string + { return flexibilityJoint.frameName; }); if (flexibilityNames.size() != flexibilityConfig.size()) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "All joint or frame names in flexibility configuration must be unique."); } if (std::find(flexibilityNames.begin(), flexibilityNames.end(), "universe") != flexibilityNames.end()) { - THROW_ERROR(std::invalid_argument, - "No one can make the universe itself flexible."); + JIMINY_THROW(std::invalid_argument, + "No one can make the universe itself flexibility."); } - for (const FlexibleJointData & flexibleJoint : flexibilityConfig) + for (const FlexibilityJointConfig & flexibilityJoint : flexibilityConfig) { - if ((flexibleJoint.stiffness.array() < 0.0).any() || - (flexibleJoint.damping.array() < 0.0).any() || - (flexibleJoint.inertia.array() < 0.0).any()) + if ((flexibilityJoint.stiffness.array() < 0.0).any() || + (flexibilityJoint.damping.array() < 0.0).any() || + (flexibilityJoint.inertia.array() < 0.0).any()) { - THROW_ERROR(std::invalid_argument, - "All stiffness, damping and inertia parameters of flexible " - "joints must be positive."); + JIMINY_THROW(std::invalid_argument, + "All stiffness, damping and inertia parameters of flexibility " + "joints must be positive."); } } // Check if the position or velocity limits have changed, and refresh proxies if so - bool enablePositionLimit = - boost::get(jointOptionsHolder.at("enablePositionLimit")); bool enableVelocityLimit = boost::get(jointOptionsHolder.at("enableVelocityLimit")); - if (enablePositionLimit != modelOptions_->joints.enablePositionLimit) - { - internalBuffersMustBeUpdated = true; - } - else if (enablePositionLimit && - (positionLimitFromUrdf != modelOptions_->joints.positionLimitFromUrdf)) + if (positionLimitFromUrdf != modelOptions_->joints.positionLimitFromUrdf) { internalBuffersMustBeUpdated = true; } @@ -1529,30 +1545,31 @@ namespace jiminy internalBuffersMustBeUpdated = true; } - // Check if the flexible model and its proxies must be regenerated - bool enableFlexibleModel = - boost::get(dynOptionsHolder.at("enableFlexibleModel")); + // Check if the extended model must be regenerated + bool enableFlexibility = boost::get(dynOptionsHolder.at("enableFlexibility")); if (modelOptions_ && - (flexibilityConfig.size() != modelOptions_->dynamics.flexibilityConfig.size() || - !std::equal(flexibilityConfig.begin(), - flexibilityConfig.end(), - modelOptions_->dynamics.flexibilityConfig.begin()) || - enableFlexibleModel != modelOptions_->dynamics.enableFlexibleModel)) + ((enableFlexibility != modelOptions_->dynamics.enableFlexibility) || + (enableFlexibility && + ((flexibilityConfig.size() != + modelOptions_->dynamics.flexibilityConfig.size()) || + !std::equal(flexibilityConfig.begin(), + flexibilityConfig.end(), + modelOptions_->dynamics.flexibilityConfig.begin()))))) { - areModelsInvalid = true; + isExtendedModelInvalid = true; } } // Check that the collisions options are valid - GenericConfig & collisionOptionsHolder = + const GenericConfig & collisionOptionsHolder = boost::get(modelOptions.at("collisions")); uint32_t contactPointsPerBodyMax = boost::get(collisionOptionsHolder.at("contactPointsPerBodyMax")); if (contactPointsPerBodyMax < 1) { - THROW_ERROR(std::invalid_argument, - "Number of contact points by collision pair " - "'contactPointsPerBodyMax' must be strictly larger than 0."); + JIMINY_THROW(std::invalid_argument, + "Number of contact points by collision pair " + "'contactPointsPerBodyMax' must be strictly larger than 0."); } if (modelOptions_ && contactPointsPerBodyMax != modelOptions_->collisions.contactPointsPerBodyMax) @@ -1561,7 +1578,8 @@ namespace jiminy } // Check that the model randomization parameters are valid - GenericConfig & dynOptionsHolder = boost::get(modelOptions.at("dynamics")); + const GenericConfig & dynOptionsHolder = + boost::get(modelOptions.at("dynamics")); for (auto && field : std::array{"inertiaBodiesBiasStd", "massBodiesBiasStd", "centerOfMassPositionBodiesBiasStd", @@ -1570,22 +1588,22 @@ namespace jiminy const double value = boost::get(dynOptionsHolder.at(field)); if (0.9 < value || value < 0.0) { - THROW_ERROR(std::invalid_argument, - "'", - field, - "' must be positive, and lower than 0.9 to avoid physics issues."); + JIMINY_THROW(std::invalid_argument, + "'", + field, + "' must be positive, and lower than 0.9 to avoid physics issues."); } } - // Update the internal options - modelOptionsGeneric_ = modelOptions; + // Update class-specific "strongly typed" accessor for fast and convenient access + modelOptions_ = std::make_unique(modelOptions); - // Create a fast struct accessor - modelOptions_ = std::make_unique(modelOptionsGeneric_); + // Update inherited polymorphic accessor + deepUpdate(modelOptionsGeneric_, modelOptions); - if (areModelsInvalid) + if (isExtendedModelInvalid) { - // Trigger models regeneration + // Trigger extended regeneration reset(std::random_device{}); } else if (internalBuffersMustBeUpdated) @@ -1600,7 +1618,7 @@ namespace jiminy } } - GenericConfig Model::getOptions() const noexcept + const GenericConfig & Model::getOptions() const noexcept { return modelOptionsGeneric_; } @@ -1612,7 +1630,7 @@ namespace jiminy const std::string & Model::getName() const { - return pinocchioModelOrig_.name; + return pinocchioModelTh_.name; } const std::string & Model::getUrdfPath() const @@ -1635,151 +1653,139 @@ namespace jiminy return hasFreeflyer_; } - void Model::getFlexiblePositionFromRigid(const Eigen::VectorXd & qRigid, - Eigen::VectorXd & qFlex) const + void Model::getExtendedPositionFromTheoretical(const Eigen::VectorXd & qTheoretical, + Eigen::VectorXd & qExtended) const { - // Define some proxies - int nqRigid = pinocchioModelOrig_.nq; - // Check the size of the input state - if (qRigid.size() != nqRigid) + if (qTheoretical.size() != pinocchioModelTh_.nq) { - THROW_ERROR(std::invalid_argument, - "Size of qRigid inconsistent with theoretical model."); + JIMINY_THROW(std::invalid_argument, "Input size inconsistent with theoretical model."); } - // Initialize the flexible state - qFlex = pinocchio::neutral(pncModelFlexibleOrig_); + // Initialize the returned extended configuration + qExtended = pinocchio::neutral(pinocchioModel_); - // Compute the flexible state based on the rigid state - int idxRigid = 0; - int idxFlex = 0; - for (; idxRigid < pinocchioModelOrig_.njoints; ++idxFlex) + // Compute extended configuration from theoretical + int theoreticalJointIndex = 1; + int extendedJointIndex = 1; + for (; theoreticalJointIndex < pinocchioModelTh_.njoints; ++extendedJointIndex) { - const std::string & jointRigidName = pinocchioModelOrig_.names[idxRigid]; - const std::string & jointFlexName = pncModelFlexibleOrig_.names[idxFlex]; - if (jointRigidName == jointFlexName) + const std::string & jointTheoreticalName = + pinocchioModelTh_.names[theoreticalJointIndex]; + const std::string & jointExtendedName = pinocchioModel_.names[extendedJointIndex]; + if (jointTheoreticalName == jointExtendedName) { - const auto & jointRigid = pinocchioModelOrig_.joints[idxRigid]; - const auto & jointFlex = pncModelFlexibleOrig_.joints[idxFlex]; - if (jointRigid.idx_q() >= 0) + const auto & jointTheoretical = pinocchioModelTh_.joints[theoreticalJointIndex]; + const auto & jointExtended = pinocchioModel_.joints[extendedJointIndex]; + if (jointTheoretical.idx_q() >= 0) { - qFlex.segment(jointFlex.idx_q(), jointFlex.nq()) = - qRigid.segment(jointRigid.idx_q(), jointRigid.nq()); + qExtended.segment(jointExtended.idx_q(), jointExtended.nq()) = + qTheoretical.segment(jointTheoretical.idx_q(), jointTheoretical.nq()); } - ++idxRigid; + ++theoreticalJointIndex; } } } - void Model::getFlexibleVelocityFromRigid(const Eigen::VectorXd & vRigid, - Eigen::VectorXd & vFlex) const + void Model::getExtendedVelocityFromTheoretical(const Eigen::VectorXd & vTheoretical, + Eigen::VectorXd & vExtended) const { - // Define some proxies - uint32_t nvRigid = pinocchioModelOrig_.nv; - uint32_t nvFlex = pncModelFlexibleOrig_.nv; - // Check the size of the input state - if (vRigid.size() != nvRigid) + if (vTheoretical.size() != pinocchioModelTh_.nv) { - THROW_ERROR(std::invalid_argument, - "Size of vRigid inconsistent with theoretical model."); + JIMINY_THROW(std::invalid_argument, "Input size inconsistent with theoretical model."); } - // Initialize the flexible state - vFlex.setZero(nvFlex); + // Initialize the returned extended velocity + vExtended.setZero(pinocchioModel_.nv); - // Compute the flexible state based on the rigid state - int32_t idxRigid = 0; - int32_t idxFlex = 0; - for (; idxRigid < pinocchioModelOrig_.njoints; ++idxFlex) + // Compute extended velocity from theoretical + int32_t theoreticalJointIndex = 1; + int32_t extendedJointIndex = 1; + for (; theoreticalJointIndex < pinocchioModelTh_.njoints; ++extendedJointIndex) { - const std::string & jointRigidName = pinocchioModelOrig_.names[idxRigid]; - const std::string & jointFlexName = pncModelFlexibleOrig_.names[idxFlex]; - if (jointRigidName == jointFlexName) + const std::string & jointTheoreticalName = + pinocchioModelTh_.names[theoreticalJointIndex]; + const std::string & jointExtendedName = pinocchioModel_.names[extendedJointIndex]; + if (jointTheoreticalName == jointExtendedName) { - const auto & jointRigid = pinocchioModelOrig_.joints[idxRigid]; - const auto & jointFlex = pncModelFlexibleOrig_.joints[idxFlex]; - if (jointRigid.idx_q() >= 0) + const auto & jointTheoretical = pinocchioModelTh_.joints[theoreticalJointIndex]; + const auto & jointExtended = pinocchioModel_.joints[extendedJointIndex]; + if (jointTheoretical.idx_q() >= 0) { - vFlex.segment(jointFlex.idx_v(), jointFlex.nv()) = - vRigid.segment(jointRigid.idx_v(), jointRigid.nv()); + vExtended.segment(jointExtended.idx_v(), jointExtended.nv()) = + vTheoretical.segment(jointTheoretical.idx_v(), jointTheoretical.nv()); } - ++idxRigid; + ++theoreticalJointIndex; } } } - void Model::getRigidPositionFromFlexible(const Eigen::VectorXd & qFlex, - Eigen::VectorXd & qRigid) const + void Model::getTheoreticalPositionFromExtended(const Eigen::VectorXd & qExtended, + Eigen::VectorXd & qTheoretical) const { - // Define some proxies - uint32_t nqFlex = pncModelFlexibleOrig_.nq; - // Check the size of the input state - if (qFlex.size() != nqFlex) + if (qExtended.size() != pinocchioModel_.nq) { - THROW_ERROR(std::invalid_argument, "Size of qFlex inconsistent with flexible model."); + JIMINY_THROW(std::invalid_argument, "Input size inconsistent with extended model."); } - // Initialize the rigid state - qRigid = pinocchio::neutral(pinocchioModelOrig_); + // Initialize the returned theoretical configuration + qTheoretical = pinocchio::neutral(pinocchioModelTh_); - // Compute the rigid state based on the flexible state - int32_t idxRigid = 0; - int32_t idxFlex = 0; - for (; idxRigid < pinocchioModelOrig_.njoints; ++idxFlex) + // Compute theoretical configuration from extended + int32_t theoreticalJointIndex = 1; + int32_t extendedJointIndex = 1; + for (; theoreticalJointIndex < pinocchioModelTh_.njoints; ++extendedJointIndex) { - const std::string & jointRigidName = pinocchioModelOrig_.names[idxRigid]; - const std::string & jointFlexName = pncModelFlexibleOrig_.names[idxFlex]; - if (jointRigidName == jointFlexName) + const std::string & jointTheoreticalName = + pinocchioModelTh_.names[theoreticalJointIndex]; + const std::string & jointExtendedName = pinocchioModel_.names[extendedJointIndex]; + if (jointTheoreticalName == jointExtendedName) { - const auto & jointRigid = pinocchioModelOrig_.joints[idxRigid]; - const auto & jointFlex = pncModelFlexibleOrig_.joints[idxFlex]; - if (jointRigid.idx_q() >= 0) + const auto & jointTheoretical = pinocchioModelTh_.joints[theoreticalJointIndex]; + const auto & jointExtended = pinocchioModel_.joints[extendedJointIndex]; + if (jointTheoretical.idx_q() >= 0) { - qRigid.segment(jointRigid.idx_q(), jointRigid.nq()) = - qFlex.segment(jointFlex.idx_q(), jointFlex.nq()); + qTheoretical.segment(jointTheoretical.idx_q(), jointTheoretical.nq()) = + qExtended.segment(jointExtended.idx_q(), jointExtended.nq()); } - ++idxRigid; + ++theoreticalJointIndex; } } } - void Model::getRigidVelocityFromFlexible(const Eigen::VectorXd & vFlex, - Eigen::VectorXd & vRigid) const + void Model::getTheoreticalVelocityFromExtended(const Eigen::VectorXd & vExtended, + Eigen::VectorXd & vTheoretical) const { - // Define some proxies - uint32_t nvRigid = pinocchioModelOrig_.nv; - uint32_t nvFlex = pncModelFlexibleOrig_.nv; - // Check the size of the input state - if (vFlex.size() != nvFlex) + if (vExtended.size() != pinocchioModel_.nv) { - THROW_ERROR(std::invalid_argument, "Size of vFlex inconsistent with flexible model."); + JIMINY_THROW(std::invalid_argument, "Input size inconsistent with extended model."); } - // Initialize the rigid state - vRigid.setZero(nvRigid); + // Initialize the returned theoretical velocity + vTheoretical.setZero(pinocchioModelTh_.nv); - // Compute the rigid state based on the flexible state - int32_t idxRigid = 0; - int32_t idxFlex = 0; - for (; idxRigid < pinocchioModelOrig_.njoints; ++idxFlex) + // Compute theoretical velocity from extended + int32_t theoreticalJointIndex = 1; + int32_t extendedJointIndex = 1; + for (; theoreticalJointIndex < pinocchioModelTh_.njoints; ++extendedJointIndex) { - const std::string & jointRigidName = pinocchioModelOrig_.names[idxRigid]; - const std::string & jointFlexName = pncModelFlexibleOrig_.names[idxFlex]; - if (jointRigidName == jointFlexName) + const std::string & jointTheoreticalName = + pinocchioModelTh_.names[theoreticalJointIndex]; + const std::string & jointExtendedName = pinocchioModel_.names[extendedJointIndex]; + if (jointTheoreticalName == jointExtendedName) { - const auto & jointRigid = pinocchioModelOrig_.joints[idxRigid]; - const auto & jointFlex = pncModelFlexibleOrig_.joints[idxFlex]; - if (jointRigid.idx_q() >= 0) + const auto & jointTheoretical = pinocchioModelTh_.joints[theoreticalJointIndex]; + const auto & jointExtended = pinocchioModel_.joints[extendedJointIndex]; + if (jointTheoretical.idx_q() >= 0) { - vRigid.segment(jointRigid.idx_v(), jointRigid.nv()) = - vFlex.segment(jointFlex.idx_v(), jointFlex.nv()); + vTheoretical.segment(jointTheoretical.idx_v(), jointTheoretical.nv()) = + vExtended.segment(jointExtended.idx_v(), jointExtended.nv()); } - ++idxRigid; + ++theoreticalJointIndex; } } } @@ -1844,50 +1850,44 @@ namespace jiminy return logForceExternalFieldnames_; } - const std::vector & Model::getRigidJointNames() const + const std::vector & Model::getMechanicalJointNames() const { - return rigidJointNames_; + return mechanicalJointNames_; } - const std::vector & Model::getRigidJointIndices() const + const std::vector & Model::getMechanicalJointIndices() const { - return rigidJointIndices_; + return mechanicalJointIndices_; } - const std::vector & Model::getRigidJointPositionIndices() const + const std::vector & Model::getMechanicalJointPositionIndices() const { - return rigidJointPositionIndices_; + return mechanicalJointPositionIndices_; } - const std::vector & Model::getRigidJointVelocityIndices() const + const std::vector & Model::getMechanicalJointVelocityIndices() const { - return rigidJointVelocityIndices_; + return mechanicalJointVelocityIndices_; } - const std::vector & Model::getFlexibleJointNames() const + const std::vector & Model::getFlexibilityJointNames() const { - static const std::vector flexibleJointsNamesEmpty{}; - if (modelOptions_->dynamics.enableFlexibleModel) - { - return flexibleJointNames_; - } - else - { - return flexibleJointsNamesEmpty; - } + return flexibilityJointNames_; } - const std::vector & Model::getFlexibleJointIndices() const + const std::vector & Model::getFlexibilityJointIndices() const { - static const std::vector flexibleJointsModelIndexEmpty{}; - if (modelOptions_->dynamics.enableFlexibleModel) - { - return flexibleJointIndices_; - } - else - { - return flexibleJointsModelIndexEmpty; - } + return flexibilityJointIndices_; + } + + const std::vector & Model::getBacklashJointNames() const + { + return backlashJointNames_; + } + + const std::vector & Model::getBacklashJointIndices() const + { + return backlashJointIndices_; } /// \brief Returns true if at least one constraint is active on the robot. diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc index 2ee3f9b0b..f28af1419 100644 --- a/core/src/robot/robot.cc +++ b/core/src/robot/robot.cc @@ -9,6 +9,8 @@ #include "jiminy/core/hardware/abstract_sensor.h" #include "jiminy/core/control/abstract_controller.h" #include "jiminy/core/control/controller_functor.h" +#include "jiminy/core/constraints/abstract_constraint.h" +#include "jiminy/core/constraints/joint_constraint.h" #include "jiminy/core/robot/robot.h" @@ -19,6 +21,9 @@ namespace jiminy name_{name}, motorSharedStorage_{std::make_shared()} { + // Initialize options + robotOptionsGeneric_ = getDefaultRobotOptions(); + setOptions(getOptions()); } Robot::~Robot() @@ -28,31 +33,15 @@ namespace jiminy detachMotors(); } - std::shared_ptr MakeDefaultController(const std::shared_ptr & robot) - { - auto noop = [](double /* t */, - const Eigen::VectorXd & /* q */, - const Eigen::VectorXd & /* v */, - const SensorMeasurementTree & /* sensorMeasurements */, - Eigen::VectorXd & /* out */) - { - // Empty on purpose - }; - std::shared_ptr controller = - std::make_shared>(noop, noop); - controller->initialize(robot); - return controller; - } - template static void initializeImpl(Robot & robot, Args... args) { // Make sure that no simulation is already running if (robot.getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before calling 'initialize'."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before calling 'initialize'."); } // Detach all the motors and sensors @@ -92,12 +81,18 @@ namespace jiminy // Reset telemetry flag isTelemetryConfigured_ = false; + // Make sure that the robot is initialized + if (!isInitialized_) + { + JIMINY_THROW(bad_control_flow, "Robot not initialized."); + } + // Make sure that the robot is not locked if (getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before calling 'reset'."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before calling 'reset'."); } // Reset model @@ -126,7 +121,7 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot is initialized."); + JIMINY_THROW(bad_control_flow, "Robot is initialized."); } isTelemetryConfigured_ = false; @@ -134,11 +129,13 @@ namespace jiminy try { // Configure hardware telemetry + const GenericConfig & telemetryOptions = + boost::get(robotOptionsGeneric_.at("telemetry")); for (const auto & [sensorType, sensorGroup] : sensors_) { for (const auto & sensor : sensorGroup) { - if (telemetryOptions_[sensorType]) + if (boost::get(telemetryOptions.at(sensorType))) { sensor->configureTelemetry(telemetryData_, name_); } @@ -162,15 +159,15 @@ namespace jiminy // The robot must be initialized if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } // No simulation must be running if (getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before adding motors."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before adding motors."); } const std::string & motorName = motor->getName(); @@ -180,33 +177,42 @@ namespace jiminy { return (elem->getName() == motorName); }); if (motorIt != motors_.end()) { - THROW_ERROR(std::logic_error, - "Another motor with name '", - motorName, - "' is already attached."); + JIMINY_THROW(std::logic_error, + "Another motor with name '", + motorName, + "' is already attached."); } // Define robot notification method, responsible for updating the robot if // necessary after changing the motor parameters, for example the armature. - auto notifyRobot = - [robot_ = std::weak_ptr(shared_from_this())](AbstractMotorBase & motorIn) + auto notifyRobot = [robot_ = std::weak_ptr(shared_from_this())]( + AbstractMotorBase & motorIn, bool hasChanged) mutable { // Make sure the robot still exists auto robot = robot_.lock(); if (!robot) { - THROW_ERROR(std::runtime_error, - "Robot has been deleted. Impossible to notify motor update."); + JIMINY_THROW(std::runtime_error, + "Robot has been deleted. Impossible to notify motor update."); } - // Update rotor inertia and effort limit of pinocchio model - const Eigen::Index jointVelocityOrigIndex = - getJointVelocityFirstIndex(robot->pinocchioModelOrig_, motorIn.getJointName()); - robot->pinocchioModel_.rotorInertia[motorIn.getJointVelocityIndex()] = - robot->pinocchioModelOrig_.rotorInertia[jointVelocityOrigIndex] + - motorIn.getArmature(); - robot->pinocchioModel_.effortLimit[motorIn.getJointVelocityIndex()] = - motorIn.getCommandLimit(); + /* Trigger extended model regeneration. + * The whole robot must be reset as joint and frame indices would be corrupted. + * Skip reset if nothing has changed from this motor point of view. This is necessary + to prevent infinite recursive reset loop. */ + if (hasChanged) + { + robot->reset(std::random_device{}); + } + + // Update rotor inertia + const std::string & mechanicaljointName = motorIn.getJointName(); + std::string inertiaJointName = mechanicaljointName; + const Eigen::Index motorVelocityIndex = motorIn.getJointVelocityIndex(); + robot->pinocchioModel_.rotorInertia[motorVelocityIndex] += motorIn.getArmature(); + + // Update effort limit + robot->pinocchioModel_.effortLimit[motorVelocityIndex] = motorIn.getCommandLimit(); }; // Attach the motor @@ -219,52 +225,71 @@ namespace jiminy refreshMotorProxies(); } - void Robot::detachMotor(const std::string & motorName) + void Robot::detachMotor(const std::string & motorName, bool triggerReset) { // The robot must be initialized if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } // No simulation must be running if (getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before removing motors."); } - auto motorIt = std::find_if(motors_.cbegin(), - motors_.cend(), + // Get motor if it exists + auto motorIt = std::find_if(motors_.begin(), + motors_.end(), [&motorName](const auto & elem) { return (elem->getName() == motorName); }); - if (motorIt == motors_.cend()) + if (motorIt == motors_.end()) { - THROW_ERROR(std::logic_error, "No motor with name '", motorName, "' is attached."); + JIMINY_THROW(std::logic_error, "No motor with name '", motorName, "' is attached."); } - // Reset effortLimit and rotorInertia - const std::shared_ptr & motor = *motorIt; - const Eigen::Index jointVelocityOrigIndex = - ::jiminy::getJointVelocityFirstIndex(pinocchioModelOrig_, motor->getJointName()); - pinocchioModel_.rotorInertia[motor->getJointVelocityIndex()] = - pinocchioModelOrig_.rotorInertia[jointVelocityOrigIndex]; - pinocchioModel_.effortLimit[motor->getJointVelocityIndex()] = 0.0; + // Hold the motor alive before removing it + std::shared_ptr motor = *motorIt; + + // Remove the motor from the holder + motors_.erase(motorIt); // Detach the motor motor->detach(); - // Remove the motor from the holder - motors_.erase(motorIt); + // Skip reset and refresh if postponed + if (triggerReset) + { + // Early return if the model has been deleted already + try + { + (void)shared_from_this(); + } + catch (std::bad_weak_ptr & e) + { + return; + } - // Refresh the motors proxies - refreshMotorProxies(); + // Trigger extended model regeneration + reset(std::random_device{}); + + // Refresh the motors proxies + refreshMotorProxies(); + } } - void Robot::detachMotors(std::vector motorsNames) + void Robot::detachMotor(const std::string & motorName) { - if (motorsNames.empty()) + // Detach motor + detachMotor(motorName, true); + } + + void Robot::detachMotors(std::vector motorNames) + { + if (motorNames.empty()) { // Remove all sensors if none is specified if (!motorNames_.empty()) @@ -275,22 +300,25 @@ namespace jiminy else { // Make sure that no motor names are duplicates - if (checkDuplicates(motorsNames)) + if (checkDuplicates(motorNames)) { - THROW_ERROR(std::invalid_argument, "Duplicated motor names found."); + JIMINY_THROW(std::invalid_argument, "Duplicated motor names found."); } // Make sure that every motor name exist - if (!checkInclusion(motorNames_, motorsNames)) + if (!checkInclusion(motorNames_, motorNames)) { - THROW_ERROR(std::invalid_argument, - "At least one of the motor names does not exist."); + JIMINY_THROW(std::invalid_argument, + "At least one of the motor names does not exist."); } - // Detach motors one-by-one - for (const std::string & name : motorsNames) + /* Detach motors one-by-one, and reset proxies at the very end only. + Beware the motor names must be stored locally because the list + of motors will be updated in-place. */ + const std::size_t nMotors = motorNames.size(); + for (std::size_t i = 0; i < nMotors; ++i) { - detachMotor(name); + detachMotor(motorNames[i], i == (nMotors - 1)); } } } @@ -300,15 +328,15 @@ namespace jiminy // The robot must be initialized if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } // No simulation must be running if (getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before removing motors."); } // Attached sensors' names must be unique, even if their type is different. @@ -324,16 +352,18 @@ namespace jiminy { return (elem->getName() == sensorName); }); if (sensorIt != sensorGroupIt->second.end()) { - THROW_ERROR(std::invalid_argument, - "A sensor with the same type and name already exists."); + JIMINY_THROW(std::invalid_argument, + "A sensor with the same type and name already exists."); } } // Create a new sensor data holder if necessary + GenericConfig & telemetryOptions = + boost::get(robotOptionsGeneric_.at("telemetry")); if (sensorGroupIt == sensors_.end()) { sensorSharedStorageMap_.emplace(sensorType, std::make_shared()); - telemetryOptions_.emplace(sensorType, true); // Enable telemetry by default + telemetryOptions[sensorType] = true; // Enable telemetry by default } // Attach the sensor @@ -351,25 +381,25 @@ namespace jiminy // The robot must be initialized if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } // No simulation must be running if (getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before removing motors."); } // FIXME: remove explicit conversion to `std::string` when moving to C++20 auto sensorGroupIt = sensors_.find(std::string{sensorType}); if (sensorGroupIt == sensors_.end()) { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors has type '", + sensorType, + "'."); } SensorVector::iterator sensorIt; @@ -379,12 +409,12 @@ namespace jiminy { return (elem->getName() == sensorName); }); if (sensorIt == sensorGroupIt->second.end()) { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors of type '", - sensorType, - "' has name '", - sensorName, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors of type '", + sensorType, + "' has name '", + sensorName, + "'."); } // Detach the sensor @@ -394,11 +424,13 @@ namespace jiminy sensorGroupIt->second.erase(sensorIt); // Remove the sensor group if there is no more sensors left + GenericConfig & telemetryOptions = + boost::get(robotOptionsGeneric_.at("telemetry")); if (sensorGroupIt->second.empty()) { sensors_.erase(sensorType); sensorSharedStorageMap_.erase(sensorType); - telemetryOptions_.erase(sensorType); + telemetryOptions.erase(sensorType); } // Refresh the sensors proxies @@ -412,10 +444,10 @@ namespace jiminy auto sensorGroupIt = sensors_.find(sensorType); if (sensorGroupIt == sensors_.end()) { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors has type '", + sensorType, + "'."); } std::vector sensorGroupNames = @@ -445,21 +477,30 @@ namespace jiminy // The robot must be initialized if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } // No simulation must be running if (getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it before removing motors."); } // Reset controller to default if none was specified if (!controller) { - controller_ = MakeDefaultController(shared_from_this()); + auto noop = [](double /* t */, + const Eigen::VectorXd & /* q */, + const Eigen::VectorXd & /* v */, + const SensorMeasurementTree & /* sensorMeasurements */, + Eigen::VectorXd & /* out */) + { + // Empty on purpose + }; + controller_ = std::make_shared>(noop, noop); + controller_->initialize(shared_from_this()); return; } @@ -496,7 +537,7 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } Model::refreshProxies(); @@ -508,7 +549,7 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } // Determine the number of motors @@ -525,29 +566,27 @@ namespace jiminy // Generate the fieldnames associated with command logCommandFieldnames_.clear(); logCommandFieldnames_.reserve(nmotors_); - std::transform( - motors_.begin(), - motors_.end(), - std::back_inserter(logCommandFieldnames_), - [](const auto & elem) -> std::string - { return addCircumfix(elem->getName(), toString(JOINT_PREFIX_BASE, "Command")); }); + std::transform(motors_.begin(), + motors_.end(), + std::back_inserter(logCommandFieldnames_), + [](const auto & elem) -> std::string + { return toString(JOINT_PREFIX_BASE, "Command", elem->getName()); }); // Generate the fieldnames associated with motor efforts logMotorEffortFieldnames_.clear(); logMotorEffortFieldnames_.reserve(nmotors_); - std::transform( - motors_.begin(), - motors_.end(), - std::back_inserter(logMotorEffortFieldnames_), - [](const auto & elem) -> std::string - { return addCircumfix(elem->getName(), toString(JOINT_PREFIX_BASE, "Effort")); }); + std::transform(motors_.begin(), + motors_.end(), + std::back_inserter(logMotorEffortFieldnames_), + [](const auto & elem) -> std::string + { return toString(JOINT_PREFIX_BASE, "Effort", elem->getName()); }); } void Robot::refreshSensorProxies() { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } // Extract the motor names @@ -565,11 +604,61 @@ namespace jiminy } } + void Robot::initializeExtendedModel() + { + // Call base implementation + Model::initializeExtendedModel(); + + // Add backlash joint for each motor + for (const auto & motor : motors_) + { + // Extract some proxies + const std::string & jointName = motor->getJointName(); + std::string backlashName = jointName; + backlashName += BACKLASH_JOINT_SUFFIX; + + // Check if constraint a joint bounds constraint exist + const bool hasConstraint = + constraints_.exist(backlashName, ConstraintNodeType::BOUNDS_JOINTS); + + // Skip adding joint if no backlash + const double backlash = motor->getBacklash(); + if (backlash < EPS) + { + // Remove joint bounds constraint if any + if (hasConstraint) + { + removeConstraint(backlashName, ConstraintNodeType::BOUNDS_JOINTS); + } + + continue; + } + + // Add joint bounds constraint if necessary + if (!hasConstraint) + { + std::shared_ptr constraint = + std::make_shared(backlashName); + addConstraint(backlashName, constraint, ConstraintNodeType::BOUNDS_JOINTS); + } + + // Add backlash joint to the model + addBacklashJointAfterMechanicalJoint(pinocchioModel_, jointName, backlashName); + backlashJointNames_.push_back(backlashName); + + // Update position limits in model + const Eigen::Index positionIndex = + getJointPositionFirstIndex(pinocchioModel_, backlashName); + pinocchioModel_.lowerPositionLimit[positionIndex] = -backlash / 2.0; + pinocchioModel_.upperPositionLimit[positionIndex] = backlash / 2.0; + } + } + std::shared_ptr Robot::getMotor(const std::string & motorName) { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } auto motorIt = std::find_if(motors_.begin(), @@ -578,7 +667,7 @@ namespace jiminy { return (elem->getName() == motorName); }); if (motorIt == motors_.end()) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "None of the attached motors has name '", motorName, "'."); } return *motorIt; @@ -588,7 +677,7 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } auto motorIt = std::find_if(motors_.begin(), @@ -597,7 +686,7 @@ namespace jiminy { return (elem->getName() == motorName); }); if (motorIt == motors_.end()) { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "None of the attached motors has name '", motorName, "'."); } return std::const_pointer_cast(*motorIt); @@ -613,16 +702,16 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } auto sensorGroupIt = sensors_.find(sensorType); if (sensorGroupIt == sensors_.end()) { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors has type '", + sensorType, + "'."); } auto sensorIt = std::find_if(sensorGroupIt->second.begin(), @@ -631,12 +720,12 @@ namespace jiminy { return (elem->getName() == sensorName); }); if (sensorIt == sensorGroupIt->second.end()) { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors of type '", - sensorType, - "' has name '", - sensorName, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors of type '", + sensorType, + "' has name '", + sensorName, + "'."); } return *sensorIt; @@ -647,16 +736,16 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } auto sensorGroupIt = sensors_.find(sensorType); if (sensorGroupIt == sensors_.end()) { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors has type '", + sensorType, + "'."); } auto sensorIt = std::find_if(sensorGroupIt->second.begin(), @@ -665,12 +754,12 @@ namespace jiminy { return (elem->getName() == sensorName); }); if (sensorIt == sensorGroupIt->second.end()) { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors of type '", - sensorType, - "' has name '", - sensorName, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors of type '", + sensorType, + "' has name '", + sensorName, + "'."); } return std::const_pointer_cast(*sensorIt); @@ -683,137 +772,42 @@ namespace jiminy void Robot::setOptions(const GenericConfig & robotOptions) { - // Set model options - GenericConfig::const_iterator modelOptionsIt; - modelOptionsIt = robotOptions.find("model"); - if (modelOptionsIt == robotOptions.end()) - { - THROW_ERROR(std::invalid_argument, "'model' options are missing."); - } - - const GenericConfig & modelOptions = boost::get(modelOptionsIt->second); - setModelOptions(modelOptions); - - // Set motors options - GenericConfig::const_iterator motorsOptionsIt; - motorsOptionsIt = robotOptions.find("motors"); - if (motorsOptionsIt == robotOptions.end()) - { - THROW_ERROR(std::invalid_argument, "'motors' options are missing."); - } - - const GenericConfig & motorsOptions = boost::get(motorsOptionsIt->second); - setMotorsOptions(motorsOptions); - - // Set sensor options - GenericConfig::const_iterator sensorOptionsIt = robotOptions.find("sensors"); - if (sensorOptionsIt == robotOptions.end()) - { - THROW_ERROR(std::invalid_argument, "'sensors' options are missing."); - } - - const GenericConfig & sensorOptions = boost::get(sensorOptionsIt->second); - setSensorsOptions(sensorOptions); - - // Set controller options - GenericConfig::const_iterator controllerOptionsIt; - controllerOptionsIt = robotOptions.find("controller"); - if (controllerOptionsIt == robotOptions.end()) - { - THROW_ERROR(std::invalid_argument, "'model' options are missing."); - } - - const GenericConfig & controllerOptions = - boost::get(controllerOptionsIt->second); - setControllerOptions(controllerOptions); - - // Set telemetry options - GenericConfig::const_iterator telemetryOptionsIt = robotOptions.find("telemetry"); - if (telemetryOptionsIt == robotOptions.end()) - { - THROW_ERROR(std::invalid_argument, "'telemetry' options are missing."); - } - - const GenericConfig & telemetryOptions = - boost::get(telemetryOptionsIt->second); - setTelemetryOptions(telemetryOptions); - } - - GenericConfig Robot::getOptions() const noexcept - { - GenericConfig robotOptions; - robotOptions["model"] = getModelOptions(); - GenericConfig motorsOptions; - robotOptions["motors"] = getMotorsOptions(); - GenericConfig sensorOptions; - robotOptions["sensors"] = getSensorsOptions(); - GenericConfig controllerOptions; - robotOptions["controller"] = getControllerOptions(); - GenericConfig telemetryOptions; - robotOptions["telemetry"] = getTelemetryOptions(); - return robotOptions; - } - - void Robot::setModelOptions(const GenericConfig & modelOptions) - { + // Make sure that no simulation is running if (getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); + JIMINY_THROW(bad_control_flow, + "Robot already locked, probably because a simulation is running. " + "Please stop it setting options."); } - return Model::setOptions(modelOptions); - } - - GenericConfig Robot::getModelOptions() const - { - return Model::getOptions(); - } - - void Robot::setMotorsOptions(const GenericConfig & motorsOptions) - { - if (getIsLocked()) - { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); - } + // Set base model options + const GenericConfig & modelOptions = boost::get(robotOptions.at("model")); + setModelOptions(modelOptions); + // Set motor options + bool areMotorsOptionsShared{true}; + const GenericConfig & motorsOptions = boost::get(robotOptions.at("motors")); for (const auto & motor : motors_) { - auto motorOptionIt = motorsOptions.find(motor->getName()); - if (motorOptionIt != motorsOptions.end()) + auto motorOptionsIt = motorsOptions.find(motor->getName()); + if (motorOptionsIt == motorsOptions.end()) { - motor->setOptions(boost::get(motorOptionIt->second)); + if (areMotorsOptionsShared) + { + motor->setOptionsAll(motorsOptions); + break; + } } else { - motor->setOptionsAll(motorsOptions); - break; + motor->setOptions(boost::get(motorOptionsIt->second)); + areMotorsOptionsShared = false; } } - } - - GenericConfig Robot::getMotorsOptions() const - { - GenericConfig motorsOptions; - for (const auto & motor : motors_) - { - motorsOptions[motor->getName()] = motor->getOptions(); - } - return motorsOptions; - } - - void Robot::setSensorsOptions(const GenericConfig & sensorsOptions) - { - if (getIsLocked()) - { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); - } + // Set sensor options + const GenericConfig & sensorsOptions = + boost::get(robotOptions.at("sensors")); for (const auto & [sensorType, sensorGroup] : sensors_) { // FIXME: remove explicit conversion to `std::string` when moving to C++20 @@ -822,11 +816,9 @@ namespace jiminy { const GenericConfig & sensorGroupOptions = boost::get(sensorGroupOptionsIt->second); - for (const auto & sensor : sensorGroup) { const std::string & sensorName = sensor->getName(); - auto sensorOptionsIt = sensorGroupOptions.find(sensorName); if (sensorOptionsIt != sensorGroupOptions.end()) { @@ -834,102 +826,92 @@ namespace jiminy } else { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors of type '", - sensorType, - "' has name '", - sensorName, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors of type '", + sensorType, + "' has name '", + sensorName, + "'."); } } } else { - THROW_ERROR(std::invalid_argument, - "None of the attached sensors has type '", - sensorType, - "'."); + JIMINY_THROW(std::invalid_argument, + "None of the attached sensors has type '", + sensorType, + "'."); } } - } - GenericConfig Robot::getSensorsOptions() const - { - GenericConfig sensorsOptions; - for (const auto & [sensorType, sensorGroup] : sensors_) + // Set controller options if any + if (controller_) { - GenericConfig sensorGroupOptions; - for (const auto & sensor : sensorGroup) - { - sensorGroupOptions[sensor->getName()] = sensor->getOptions(); - } - sensorsOptions[sensorType] = sensorGroupOptions; + const GenericConfig & controllerOptions = + boost::get(robotOptions.at("controller")); + controller_->setOptions(controllerOptions); } - return sensorsOptions; + + // Update inherited polymorphic accessor + deepUpdate(robotOptionsGeneric_, robotOptions); } - void Robot::setControllerOptions(const GenericConfig & controllerOptions) + const GenericConfig & Robot::getOptions() const noexcept { - if (getIsLocked()) + /* Return options without refreshing all options if and only if the robot has not been + unlock since the last time they were considered valid. */ + if (areRobotOptionsRefreshed_ && getIsLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); + return robotOptionsGeneric_; } - return controller_->setOptions(controllerOptions); - } - - GenericConfig Robot::getControllerOptions() const - { - return controller_->getOptions(); - } + // Refresh model options + robotOptionsGeneric_["model"] = getModelOptions(); - void Robot::setTelemetryOptions(const GenericConfig & telemetryOptions) - { - if (getIsLocked()) + // Refresh motors options + GenericConfig & motorsOptions = boost::get(robotOptionsGeneric_["motors"]); + motorsOptions.clear(); + for (const auto & motor : motors_) { - THROW_ERROR(bad_control_flow, - "Robot already locked, probably because a simulation is running. " - "Please stop it before removing motors."); + motorsOptions[motor->getName()] = motor->getOptions(); } - for (auto & [sensorType, sensorGroupTelemetryOption] : telemetryOptions_) + // Refresh sensor options + GenericConfig & sensorsOptions = + boost::get(robotOptionsGeneric_["sensors"]); + sensorsOptions.clear(); + for (const auto & [sensorType, sensorGroup] : sensors_) { - const std::string optionTelemetryName = toString("enable", sensorType, "s"); - auto sensorTelemetryOptionIt = telemetryOptions.find(optionTelemetryName); - if (sensorTelemetryOptionIt == telemetryOptions.end()) + GenericConfig sensorGroupOptions; + for (const auto & sensor : sensorGroup) { - THROW_ERROR(std::invalid_argument, "Missing field '", optionTelemetryName, "'."); + sensorGroupOptions[sensor->getName()] = sensor->getOptions(); } - sensorGroupTelemetryOption = boost::get(sensorTelemetryOptionIt->second); + sensorsOptions[sensorType] = sensorGroupOptions; } - } - GenericConfig Robot::getTelemetryOptions() const - { - GenericConfig telemetryOptions; - for (const auto & [sensorType, sensorGroupTelemetryOption] : telemetryOptions_) + // Refresh controller options + GenericConfig & controllerOptions = + boost::get(robotOptionsGeneric_["controller"]); + controllerOptions.clear(); + if (controller_) { - const std::string optionTelemetryName = toString("enable", sensorType, "s"); - telemetryOptions[optionTelemetryName] = sensorGroupTelemetryOption; + controllerOptions = controller_->getOptions(); } - return telemetryOptions; + + // Options are now considered "valid" + areRobotOptionsRefreshed_ = true; + + return robotOptionsGeneric_; } - void Robot::dumpOptions(const std::string & filepath) const + void Robot::setModelOptions(const GenericConfig & modelOptions) { - std::shared_ptr device = std::make_shared(filepath); - return jsonDump(getOptions(), device); + Model::setOptions(modelOptions); } - - void Robot::loadOptions(const std::string & filepath) + const GenericConfig & Robot::getModelOptions() const noexcept { - std::shared_ptr device = std::make_shared(filepath); - GenericConfig robotOptions; - jsonLoad(robotOptions, device); - - setOptions(robotOptions); + return Model::getOptions(); } bool Robot::getIsTelemetryConfigured() const @@ -965,7 +947,7 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } auto motorIt = std::find_if(motors_.begin(), @@ -974,7 +956,7 @@ namespace jiminy { return (elem->getName() == motorName); }); if (motorIt == motors_.end()) { - THROW_ERROR( + JIMINY_THROW( std::logic_error, "No motor with name '", motorName, "' attached to the robot."); } @@ -990,7 +972,7 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } for (const auto & sensorGroupItem : sensors_) @@ -1006,7 +988,7 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } SensorMeasurementTree data; @@ -1032,13 +1014,13 @@ namespace jiminy { if (!isInitialized_) { - THROW_ERROR(bad_control_flow, "Robot not initialized."); + JIMINY_THROW(bad_control_flow, "Robot not initialized."); } auto sensorGroupIt = sensors_.find(sensorType); if (sensorGroupIt == sensors_.end()) { - THROW_ERROR( + JIMINY_THROW( std::logic_error, "No sensor of type '", sensorType, "' attached to the robot."); } @@ -1048,12 +1030,12 @@ namespace jiminy { return (elem->getName() == sensorName); }); if (sensorIt == sensorGroupIt->second.end()) { - THROW_ERROR(std::logic_error, - "No sensor of type '", - sensorType, - "' with name '", - sensorName, - "' attached to the robot."); + JIMINY_THROW(std::logic_error, + "No sensor of type '", + sensorType, + "' with name '", + sensorName, + "' attached to the robot."); } return (*sensorIt)->get(); @@ -1076,12 +1058,18 @@ namespace jiminy std::unique_ptr Robot::getLock() { + // Make sure that the robot is not already locked if (mutexLocal_->isLocked()) { - THROW_ERROR(bad_control_flow, - "Robot already locked. Please release it first prior requesting lock."); + JIMINY_THROW(bad_control_flow, + "Robot already locked. Please release it first prior requesting lock."); } + // Make sure that the options are not already considered valid as it was impossible to + // guarantee it before locking the robot. + areRobotOptionsRefreshed_ = false; + + // Return lock return std::make_unique(*mutexLocal_); } diff --git a/core/src/stepper/abstract_stepper.cc b/core/src/stepper/abstract_stepper.cc index d9c2853df..41f1a1408 100644 --- a/core/src/stepper/abstract_stepper.cc +++ b/core/src/stepper/abstract_stepper.cc @@ -13,12 +13,15 @@ namespace jiminy { } - bool AbstractStepper::tryStep(std::vector & qSplit, - std::vector & vSplit, - std::vector & aSplit, - double & t, - double & dt) + stepper::StatusInfo AbstractStepper::tryStep(std::vector & qSplit, + std::vector & vSplit, + std::vector & aSplit, + double & t, + double & dt) { + // Initialize return status + stepper::StatusInfo status{stepper::ReturnCode::IS_SUCCESS, {}}; + // Update buffers double t_next = t + dt; state_.q = qSplit; @@ -26,31 +29,37 @@ namespace jiminy stateDerivative_.v = vSplit; stateDerivative_.a = aSplit; - // Try doing a single step - bool result = tryStepImpl(state_, stateDerivative_, t, dt); - - // Make sure everything went fine - if (result) + // Do a single step + try { + // Try doing step + if (!tryStepImpl(state_, stateDerivative_, t, dt)) + { + return {stepper::ReturnCode::IS_FAILURE, {}}; + } + + // Make sure everything went fine for (const Eigen::VectorXd & a : stateDerivative_.a) { if ((a.array() != a.array()).any()) { - dt = qNAN; - result = false; + JIMINY_THROW(std::runtime_error, + "The integrated acceleration contains 'nan'."); } } } - - // Update output if successful - if (result) + catch (...) { - t = t_next; - qSplit = state_.q; - vSplit = state_.v; - aSplit = stateDerivative_.a; + return {stepper::ReturnCode::IS_ERROR, std::current_exception()}; } - return result; + + // Update output if successful + t = t_next; + qSplit = state_.q; + vSplit = state_.v; + aSplit = stateDerivative_.a; + + return {stepper::ReturnCode::IS_SUCCESS, {}}; } const StateDerivative & AbstractStepper::f(double t, const State & state) diff --git a/core/src/stepper/euler_explicit_stepper.cc b/core/src/stepper/euler_explicit_stepper.cc index 1a27bf361..2209cc148 100644 --- a/core/src/stepper/euler_explicit_stepper.cc +++ b/core/src/stepper/euler_explicit_stepper.cc @@ -12,10 +12,11 @@ namespace jiminy // Compute the next state derivative stateDerivative = f(t, state); - // By default INF is returned in case of fixed timestep. It must be managed externally. + /* By default INF is returned no matter what for fixed-timestep integrators. + The user is responsible for managing it externally. */ dt = INF; - // Scheme never considers failure. + // Scheme never considers failure return true; } } diff --git a/core/src/stepper/runge_kutta_dopri_stepper.cc b/core/src/stepper/runge_kutta_dopri_stepper.cc index 5b1e4dcd9..42fd77091 100644 --- a/core/src/stepper/runge_kutta_dopri_stepper.cc +++ b/core/src/stepper/runge_kutta_dopri_stepper.cc @@ -18,43 +18,13 @@ namespace jiminy bool RungeKuttaDOPRIStepper::adjustStep( const State & initialState, const State & solution, double & dt) { + // Estimate the integration error const double error = computeError(initialState, solution, dt); - return adjustStepImpl(error, dt); - } - - double RungeKuttaDOPRIStepper::computeError( - const State & initialState, const State & solution, double dt) - { - // Compute error scale given absolute and relative tolerance - otherSolution_.setZero(); - initialState.difference(otherSolution_, scale_); - scale_.absInPlace(); - scale_ *= tolRel_; - scale_ += tolAbs_; - - // Compute alternative solution - stateIncrement_.setZero(); - for (std::size_t i = 0; i < ki_.size(); ++i) - { - stateIncrement_.sumInPlace(ki_[i], dt * DOPRI::e[i]); - } - initialState.sum(stateIncrement_, otherSolution_); - - // Evaluate error between both states to adjust step - solution.difference(otherSolution_, error_); - // Return element-wise maximum rescaled error - error_ /= scale_; - return error_.normInf(); - } - - bool RungeKuttaDOPRIStepper::adjustStepImpl(double error, double & dt) - { - // Make sure the error is defined, otherwise rely on simple heuristic + // Make sure the error is well defined, otherwise throw an exception if (std::isnan(error)) { - dt *= 0.1; - return false; + JIMINY_THROW(std::runtime_error, "The estimated integration error contains 'nan'."); } /* Adjustment algorithm from boost implementation. @@ -84,4 +54,30 @@ namespace jiminy return false; } } + + double RungeKuttaDOPRIStepper::computeError( + const State & initialState, const State & solution, double dt) + { + // Compute error scale given absolute and relative tolerance + otherSolution_.setZero(); + initialState.difference(otherSolution_, scale_); + scale_.absInPlace(); + scale_ *= tolRel_; + scale_ += tolAbs_; + + // Compute alternative solution + stateIncrement_.setZero(); + for (std::size_t i = 0; i < ki_.size(); ++i) + { + stateIncrement_.sumInPlace(ki_[i], dt * DOPRI::e[i]); + } + initialState.sum(stateIncrement_, otherSolution_); + + // Evaluate error between both states to adjust step + solution.difference(otherSolution_, error_); + + // Return element-wise maximum rescaled error + error_ /= scale_; + return error_.normInf(); + } } diff --git a/core/src/telemetry/telemetry_data.cc b/core/src/telemetry/telemetry_data.cc index d16b818ed..4b0871961 100644 --- a/core/src/telemetry/telemetry_data.cc +++ b/core/src/telemetry/telemetry_data.cc @@ -18,7 +18,7 @@ namespace jiminy // Check if registration is possible if (!isRegisteringAvailable_) { - THROW_ERROR(bad_control_flow, "Registration already locked."); + JIMINY_THROW(bad_control_flow, "Registration already locked."); } // Check if already in memory @@ -29,7 +29,7 @@ namespace jiminy { return element.first == name; }); if (variableIt != constantRegistry_.end()) { - THROW_ERROR(bad_control_flow, "Entry '", name, "' already exists."); + JIMINY_THROW(bad_control_flow, "Entry '", name, "' already exists."); } // Register new constant diff --git a/core/src/telemetry/telemetry_recorder.cc b/core/src/telemetry/telemetry_recorder.cc index a78be898b..86f71e9d4 100644 --- a/core/src/telemetry/telemetry_recorder.cc +++ b/core/src/telemetry/telemetry_recorder.cc @@ -23,7 +23,7 @@ namespace jiminy { if (isInitialized_) { - THROW_ERROR(bad_control_flow, "TelemetryRecorder already initialized."); + JIMINY_THROW(bad_control_flow, "TelemetryRecorder already initialized."); } // Log the time unit as constant @@ -153,9 +153,9 @@ namespace jiminy myFile.open(OpenMode::WRITE_ONLY | OpenMode::TRUNCATE); if (!myFile.isOpen()) { - THROW_ERROR(std::ios_base::failure, - "Impossible to create the log file. Check if root folder " - "exists and if you have writing permissions."); + JIMINY_THROW(std::ios_base::failure, + "Impossible to create the log file. Check if root folder " + "exists and if you have writing permissions."); } for (auto & flow : flows_) @@ -207,7 +207,7 @@ namespace jiminy flow->read(version); if (version != TELEMETRY_VERSION) { - THROW_ERROR( + JIMINY_THROW( std::runtime_error, "Log telemetry version not supported. Impossible to read log."); } @@ -353,15 +353,15 @@ namespace jiminy std::ifstream file = std::ifstream(filename, std::ios::in | std::ifstream::binary); if (!file.is_open()) { - THROW_ERROR(std::ios_base::failure, - "Impossible to open the log file. Check that the file " - "exists and that you have reading permissions."); + JIMINY_THROW(std::ios_base::failure, + "Impossible to open the log file. Check that the file " + "exists and that you have reading permissions."); } // Make sure the log file is not corrupted if (!file.good()) { - THROW_ERROR(std::ios_base::failure, "Corrupted log file."); + JIMINY_THROW(std::ios_base::failure, "Corrupted log file."); } // Skip the version flag @@ -411,4 +411,4 @@ namespace jiminy flows.push_back(&device); return parseLogDataRaw(flows, integerSectionSize, floatSectionSize, headerSize); } -} \ No newline at end of file +} diff --git a/core/src/utilities/helpers.cc b/core/src/utilities/helpers.cc index 09221613d..aee08733c 100644 --- a/core/src/utilities/helpers.cc +++ b/core/src/utilities/helpers.cc @@ -64,6 +64,70 @@ namespace jiminy #endif } + // ********************************* GenericConfig helpers ********************************* // + + struct DeepUpdateVisitor : public boost::static_visitor<> + { + explicit DeepUpdateVisitor(bool strict) noexcept : + strict_{strict} + { + } + + template + std::enable_if_t && std::is_same_v, void> + operator()(T1 & dst, const T2 & src) const + { + deepUpdate(dst, src, strict_); + } + + template + std::enable_if_t && !std::is_same_v, void> + operator()(T1 & dst, const T2 & src) const + { + // Simply copy-assign source to destination + dst = src; + } + + template + [[noreturn]] std::enable_if_t, void> + operator()(T1 & /* dst */, const T2 & /* src */) const + { + JIMINY_THROW(std::invalid_argument, + "Value type mismatch between source and destination."); + } + + private: + bool strict_; + }; + + void deepUpdate(GenericConfig & dst, const GenericConfig & src, bool strict) + { + // Define visitor + auto visitor = DeepUpdateVisitor{strict}; + auto visit = boost::apply_visitor(visitor); + + // Loop over all top-level source items + for (const auto & [key, srcValue] : src) + { + // Get destination value + auto dstValueIt = dst.find(key); + if (dstValueIt == dst.end()) + { + // Move to the next key if missing from destination and strict not enforced + if (!strict) + { + continue; + } + + // Throw missing key exception + JIMINY_THROW(std::invalid_argument, "Missing destination key '", key, "'."); + } + + // Copy source to destination recursively + visit(dstValueIt->second, srcValue); + } + } + // ********************************** Telemetry utilities ********************************** // bool endsWith(const std::string & str, const std::string & substr) @@ -143,7 +207,7 @@ namespace jiminy auto fieldnameIt = std::find(firstFieldnameIt, logData.variableNames.end(), fieldname); if (fieldnameIt == logData.variableNames.end()) { - THROW_ERROR(lookup_error, "Variable '", fieldname, "' does not exist."); + JIMINY_THROW(lookup_error, "Variable '", fieldname, "' does not exist."); } const int64_t varIndex = std::distance(firstFieldnameIt, fieldnameIt); const Eigen::Index numInt = logData.integerValues.rows(); diff --git a/core/src/utilities/json.cc b/core/src/utilities/json.cc index 08390520e..9e092f5fb 100644 --- a/core/src/utilities/json.cc +++ b/core/src/utilities/json.cc @@ -12,7 +12,7 @@ namespace jiminy // *************** Convertion to JSON utilities ***************** template<> - Json::Value convertToJson(const FlexibleJointData & value) + Json::Value convertToJson(const FlexibilityJointConfig & value) { Json::Value flex; flex["frameName"] = convertToJson(value.frameName); @@ -134,7 +134,7 @@ namespace jiminy } template<> - FlexibleJointData convertFromJson(const Json::Value & value) + FlexibilityJointConfig convertFromJson(const Json::Value & value) { return {convertFromJson(value["frameName"]), convertFromJson(value["stiffness"]), @@ -191,7 +191,7 @@ namespace jiminy } else { - THROW_ERROR( + JIMINY_THROW( std::invalid_argument, "Unknown data type: std::vector<", type, ">"); } } @@ -242,10 +242,10 @@ namespace jiminy } else { - THROW_ERROR(std::invalid_argument, - "Unknown data type: std::vector<", - it->type(), - ">"); + JIMINY_THROW(std::invalid_argument, + "Unknown data type: std::vector<", + it->type(), + ">"); } } else @@ -255,7 +255,7 @@ namespace jiminy } else { - THROW_ERROR(std::invalid_argument, "Unknown data type: ", root->type()); + JIMINY_THROW(std::invalid_argument, "Unknown data type: ", root->type()); } config[root.key().asString()] = field; diff --git a/core/src/utilities/pinocchio.cc b/core/src/utilities/pinocchio.cc index 5cd14bb82..c12387a9f 100644 --- a/core/src/utilities/pinocchio.cc +++ b/core/src/utilities/pinocchio.cc @@ -111,7 +111,7 @@ namespace jiminy { if (model.njoints < static_cast(jointIndex) - 1) { - THROW_ERROR(lookup_error, "Joint index '", jointIndex, "' is out of range."); + JIMINY_THROW(lookup_error, "Joint index '", jointIndex, "' is out of range."); } return getJointType(model.joints[jointIndex]); @@ -135,7 +135,7 @@ namespace jiminy } } - THROW_ERROR(lookup_error, "Position index out of range."); + JIMINY_THROW(lookup_error, "Position index out of range."); } std::string getJointNameFromVelocityIndex(const pinocchio::Model & model, @@ -156,7 +156,7 @@ namespace jiminy } } - THROW_ERROR(lookup_error, "Velocity index out of range."); + JIMINY_THROW(lookup_error, "Velocity index out of range."); } std::vector getJointTypePositionSuffixes(JointModelType jointType) @@ -178,7 +178,7 @@ namespace jiminy return {"TransX", "TransY", "TransZ", "QuatX", "QuatY", "QuatZ", "QuatW"}; case JointModelType::UNSUPPORTED: default: - THROW_ERROR(lookup_error, "Joints of type 'UNSUPPORTED' do not have fieldnames."); + JIMINY_THROW(lookup_error, "Joints of type 'UNSUPPORTED' do not have fieldnames."); } } @@ -200,7 +200,7 @@ namespace jiminy return {"LinX", "LinY", "LinZ", "AngX", "AngY", "AngZ"}; case JointModelType::UNSUPPORTED: default: - THROW_ERROR(lookup_error, "Joints of type 'UNSUPPORTED' do not have fieldnames."); + JIMINY_THROW(lookup_error, "Joints of type 'UNSUPPORTED' do not have fieldnames."); } } @@ -210,7 +210,7 @@ namespace jiminy { if (!model.existFrame(frameName, frameType)) { - THROW_ERROR(lookup_error, "Frame '", frameName, "' not found in robot model."); + JIMINY_THROW(lookup_error, "Frame '", frameName, "' not found in robot model."); } return model.getFrameId(frameName, frameType); } @@ -232,7 +232,7 @@ namespace jiminy { if (!model.existJointName(jointName)) { - THROW_ERROR(lookup_error, "Joint '", jointName, "' not found in robot model."); + JIMINY_THROW(lookup_error, "Joint '", jointName, "' not found in robot model."); } return model.getJointId(jointName); @@ -351,7 +351,7 @@ namespace jiminy return pinocchio::isNormalized(model, q, tolAbs); } - void swapJoints(pinocchio::Model & model, + void swapJointIndices(pinocchio::Model & model, pinocchio::JointIndex jointIndex1, pinocchio::JointIndex jointIndex2) { @@ -364,7 +364,7 @@ namespace jiminy // Enforce that the second joint index always comes after the first one if (jointIndex1 > jointIndex2) { - return swapJoints(model, jointIndex2, jointIndex1); + return swapJointIndices(model, jointIndex2, jointIndex1); } // Swap references to the joint indices themself @@ -457,20 +457,18 @@ namespace jiminy } } - void insertFlexibilityBeforeJointInModel(pinocchio::Model & model, - const std::string & childJointName, - const std::string & newJointName) + void addFlexibilityJointBeforeMechanicalJoint(pinocchio::Model & model, + const std::string & childJointName, + const std::string & newJointName) { - using namespace pinocchio; - - const pinocchio::JointIndex childJointIndex = getJointIndex(model, childJointName); - // Flexible joint is placed at the same position as the child joint, in its parent frame - const SE3 & jointPlacement = model.jointPlacements[childJointIndex]; + const pinocchio::JointIndex childJointIndex = getJointIndex(model, childJointName); + const pinocchio::SE3 & jointPlacement = model.jointPlacements[childJointIndex]; - // Create flexible joint + // Add new joint before the original joint + pinocchio::JointModel newJointModel = pinocchio::JointModelSpherical(); const pinocchio::JointIndex newJointIndex = model.addJoint( - model.parents[childJointIndex], JointModelSpherical(), jointPlacement, newJointName); + model.parents[childJointIndex], newJointModel, jointPlacement, newJointName); // Set child joint to be a child of the new joint, at the origin model.parents[childJointIndex] = newJointIndex; @@ -485,39 +483,111 @@ namespace jiminy model.frames[childFrameIndex].previousFrame = newFrameIndex; model.frames[childFrameIndex].placement.setIdentity(); - // Update new joint subtree to include all the joints below it - for (std::size_t i = 0; i < model.subtrees[childJointIndex].size(); ++i) - { - model.subtrees[newJointIndex].push_back(model.subtrees[childJointIndex][i]); - } + // Update new joint subtree to append all the joints below it + const std::vector & childSubtree = model.subtrees[childJointIndex]; + std::vector & newSubtree = model.subtrees[newJointIndex]; + newSubtree.insert(newSubtree.end(), childSubtree.begin(), childSubtree.end()); // Add weightless body - model.appendBodyToJoint(newJointIndex, pinocchio::Inertia::Zero(), SE3::Identity()); + model.appendBodyToJoint( + newJointIndex, pinocchio::Inertia::Zero(), pinocchio::SE3::Identity()); - /* Pinocchio requires that joints are in increasing order as we move to the leaves of the - kinematic tree. Here this is no longer the case, as an intermediate joint was appended - at the end. We put the joint back in order by doing successive permutations. */ + /* Pinocchio expects joint indices to be sorted as we move from the root to the leaves of + the kinematic tree. This is no longer the case, as an intermediate joint was appended at + the end. Therefore, we put the joints back in order by doing successive permutations. */ for (pinocchio::JointIndex i = childJointIndex; i < newJointIndex; ++i) { - swapJoints(model, i, newJointIndex); + swapJointIndices(model, i, newJointIndex); + } + } + + void addBacklashJointAfterMechanicalJoint(pinocchio::Model & model, + const std::string & parentJointName, + const std::string & newJointName) + { + // Get parent joint model + const pinocchio::JointIndex parentJointIndex = getJointIndex(model, parentJointName); + const pinocchio::JointModel & parentJointModel = model.joints[parentJointIndex]; + + // Backlash are only supported for 1DoF joints (same as motors) + const JointModelType parentJointType = getJointType(parentJointModel); + if (parentJointType != JointModelType::LINEAR && + parentJointType != JointModelType::ROTARY && + parentJointType != JointModelType::ROTARY_UNBOUNDED) + { + JIMINY_THROW(std::logic_error, + "Backlash can only be associated with a 1-dof linear or rotary joint."); + } + + // Add new joint after the original joint + pinocchio::JointIndex newJointIndex = model.addJoint( + parentJointIndex, parentJointModel, pinocchio::SE3::Identity(), newJointName); + + // Add new joint to frame list + const pinocchio::FrameIndex parentFrameIndex = getFrameIndex(model, parentJointName); + model.addJointFrame(newJointIndex, static_cast(parentFrameIndex)); + + // Update original joint subtree to include the new joint + std::vector & parentSubtree = model.subtrees[parentJointIndex]; + std::vector & newSubtree = model.subtrees[newJointIndex]; + newSubtree.insert(newSubtree.end(), parentSubtree.begin() + 1, parentSubtree.end()); + parentSubtree.insert(parentSubtree.begin() + 1, newJointIndex); + + // Move the inertia of the orginal joint to the new joint, which is now weightless + model.appendBodyToJoint( + newJointIndex, model.inertias[parentJointIndex], pinocchio::SE3::Identity()); + model.inertias[parentJointIndex].setZero(); + + /* Set child joint to be a child of the new joint, at the origin. + Loop over joints starting from universe (1) to new joint excluded (njoints - 1). */ + for (pinocchio::JointIndex i = 1 ; + i < static_cast(model.njoints -1) ; ++i) + { + if (model.parents[i] == parentJointIndex) + { + model.parents[i] = newJointIndex; + } + } + + // Putting the joints back in order + for (pinocchio::JointIndex i = parentJointIndex + 1; i < newJointIndex; ++i) + { + swapJointIndices(model, i, newJointIndex); + } + + // Reparent non-joint frames having the original joint as parent in favor of the new joint + newJointIndex = getJointIndex(model, newJointName); + const pinocchio::FrameIndex newFrameIndex = getFrameIndex(model, newJointName); + for (auto & frame : model.frames) + { + if (frame.type != pinocchio::FrameType::JOINT) + { + if (frame.parent == parentJointIndex) + { + frame.parent = newJointIndex; + } + if (frame.previousFrame == parentFrameIndex) + { + frame.previousFrame = newFrameIndex; + } + } } } - void insertFlexibilityAtFixedFrameInModel(pinocchio::Model & model, - const std::string & frameName) + void addFlexibilityJointAtFixedFrame(pinocchio::Model & model, const std::string & frameName) { using namespace pinocchio; // Make sure the frame exists and is fixed if (!model.existFrame(frameName)) { - THROW_ERROR(lookup_error, "No frame with name '", frameName, "' found in model."); + JIMINY_THROW(lookup_error, "No frame with name '", frameName, "' found in model."); } const pinocchio::FrameIndex frameIndex = getFrameIndex(model, frameName); Model::Frame & frame = model.frames[frameIndex]; if (frame.type != pinocchio::FrameType::FIXED_JOINT) { - THROW_ERROR(std::logic_error, "Frame must be associated with fixed joint."); + JIMINY_THROW(std::logic_error, "Frame must be associated with fixed joint."); } /* Get the parent and child actual joints. @@ -566,19 +636,19 @@ namespace jiminy // Remove inertia of child body from composite body if (childBodyInertia.mass() < 0.0) { - THROW_ERROR(std::runtime_error, "Child body mass must be positive."); + JIMINY_THROW(std::runtime_error, "Child body mass must be positive."); } if (model.inertias[parentJointIndex].mass() - childBodyInertia.mass() < 0.0) { - THROW_ERROR(std::runtime_error, - "Child body mass too large to be subtracted to joint mass."); + JIMINY_THROW(std::runtime_error, + "Child body mass too large to be subtracted to joint mass."); } const Inertia childBodyInertiaInv(-childBodyInertia.mass(), childBodyInertia.lever(), Symmetric3(-childBodyInertia.inertia().data())); model.inertias[parentJointIndex] += childBodyInertiaInv; - // Create flexible joint + // Create flexibility joint const pinocchio::JointIndex newJointIndex = model.addJoint(parentJointIndex, JointModelSpherical(), frame.placement, frame.name); model.inertias[newJointIndex] = childBodyInertia.se3Action(frame.placement.inverse()); @@ -642,7 +712,7 @@ namespace jiminy We move it back this at the correct place by doing successive permutations. */ for (pinocchio::JointIndex i = childJointIndexMin; i < newJointIndex; ++i) { - swapJoints(model, i, newJointIndex); + swapJointIndices(model, i, newJointIndex); } } @@ -656,14 +726,14 @@ namespace jiminy if (!std::is_sorted(timesIn.data(), timesIn.data() + timesIn.size()) || !std::is_sorted(timesOut.data(), timesOut.data() + timesOut.size())) { - THROW_ERROR(std::invalid_argument, "Input and output time sequences must be sorted."); + JIMINY_THROW(std::invalid_argument, "Input and output time sequences must be sorted."); } if (timesIn.size() != positionsIn.cols() || model.nq != positionsIn.rows()) { - THROW_ERROR(std::invalid_argument, - "Input position matrix not consistent with model and/or " - "time sequence. Time expected as second dimension."); + JIMINY_THROW(std::invalid_argument, + "Input position matrix not consistent with model and/or " + "time sequence. Time expected as second dimension."); } // Nothing to do. Return early. @@ -771,10 +841,10 @@ namespace jiminy } catch (const std::exception & e) { - THROW_ERROR(std::ios_base::failure, - "Something is wrong with the URDF. Impossible to load the collision " - "geometries.\nRaised from exception: ", - e.what()); + JIMINY_THROW(std::ios_base::failure, + "Something is wrong with the URDF. Impossible to load the collision " + "geometries.\nRaised from exception: ", + e.what()); } // Replace the mesh geometry object by its convex representation if necessary @@ -816,7 +886,7 @@ namespace jiminy // Make sure the URDF file exists if (!std::ifstream(urdfPath).good()) { - THROW_ERROR(std::ios_base::failure, "The URDF file '", urdfPath, "' is invalid."); + JIMINY_THROW(std::ios_base::failure, "The URDF file '", urdfPath, "' is invalid."); } // Build physics model @@ -834,10 +904,10 @@ namespace jiminy } catch (const std::exception & e) { - THROW_ERROR(std::ios_base::failure, - "Something is wrong with the URDF. Impossible to build a model from " - "it.\nRaised from exception: ", - e.what()); + JIMINY_THROW(std::ios_base::failure, + "Something is wrong with the URDF. Impossible to build a model from " + "it.\nRaised from exception: ", + e.what()); } // Build collision model diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index 48b8cd62a..6b00d2e1f 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -649,10 +649,12 @@ namespace jiminy double orientation, uint32_t seed) { - if ((0.01 <= interpDelta.array()).all() && - (interpDelta.array() <= size.array() / 2.0).all()) + if ((0.01 < interpDelta.array()).any() || (interpDelta.array() > size.array() / 2.0).any()) { - PRINT_WARNING("'interpDelta' must be in range [0.01, 'size'/2.0]."); + PRINT_WARNING( + "All components of 'interpDelta' must be in range [0.01, 'size'/2.0]. Value: ", + interpDelta.transpose(), + "'."); } Eigen::Vector2d interpThr = interpDelta.cwiseMax(0.01).cwiseMin(size / 2.0); @@ -682,7 +684,7 @@ namespace jiminy double dheight_x; std::tie(height, dheight_x) = tile2dInterp1d( posIndices, posRel, 0, size, sparsity, heightMax, interpThr, seed); - double const norm_inv = 1.0 / std::sqrt(dheight_x * dheight_x + 1.0); + const double norm_inv = 1.0 / std::sqrt(dheight_x * dheight_x + 1.0); normal << -dheight_x * norm_inv, 0.0, norm_inv; } else if (!is_edge[0] && is_edge[1]) @@ -690,23 +692,23 @@ namespace jiminy double dheight_y; std::tie(height, dheight_y) = tile2dInterp1d( posIndices, posRel, 1, size, sparsity, heightMax, interpThr, seed); - double const norm_inv = 1.0 / std::sqrt(dheight_y * dheight_y + 1.0); + const double norm_inv = 1.0 / std::sqrt(dheight_y * dheight_y + 1.0); normal << 0.0, -dheight_y * norm_inv, norm_inv; } else if (is_edge[0] && is_edge[1]) { - auto const [height_0, dheight_x_0] = tile2dInterp1d( + const auto [height_0, dheight_x_0] = tile2dInterp1d( posIndices, posRel, 0, size, sparsity, heightMax, interpThr, seed); if (posRel[1] < interpThr[1]) { posIndices[1] -= 1; - auto const [height_m, dheight_x_m] = tile2dInterp1d( + const auto [height_m, dheight_x_m] = tile2dInterp1d( posIndices, posRel, 0, size, sparsity, heightMax, interpThr, seed); - double const ratio = (1.0 - posRel[1] / interpThr[1]) / 2.0; + const double ratio = (1.0 - posRel[1] / interpThr[1]) / 2.0; height = height_0 + (height_m - height_0) * ratio; - double const dheight_x = dheight_x_0 + (dheight_x_m - dheight_x_0) * ratio; - double const dheight_y = + const double dheight_x = dheight_x_0 + (dheight_x_m - dheight_x_0) * ratio; + const double dheight_y = (height_0 - height_m) / (2.0 * size[1] * interpThr[1]); normal << -dheight_x, -dheight_y, 1.0; normal.normalize(); @@ -714,13 +716,13 @@ namespace jiminy else { posIndices[1] += 1; - auto const [height_p, dheight_x_p] = tile2dInterp1d( + const auto [height_p, dheight_x_p] = tile2dInterp1d( posIndices, posRel, 0, size, sparsity, heightMax, interpThr, seed); - double const ratio = (1.0 + (posRel[1] - 1.0) / interpThr[1]) / 2.0; + const double ratio = (1.0 + (posRel[1] - 1.0) / interpThr[1]) / 2.0; height = height_0 + (height_p - height_0) * ratio; - double const dheight_x = dheight_x_0 + (dheight_x_p - dheight_x_0) * ratio; - double const dheight_y = + const double dheight_x = dheight_x_0 + (dheight_x_p - dheight_x_0) * ratio; + const double dheight_y = (height_p - height_0) / (2.0 * size[1] * interpThr[1]); normal << -dheight_x, -dheight_y, 1.0; normal.normalize(); diff --git a/core/unit/engine_sanity_check.cc b/core/unit/engine_sanity_check.cc index 4de859b50..ff0cab20c 100755 --- a/core/unit/engine_sanity_check.cc +++ b/core/unit/engine_sanity_check.cc @@ -61,21 +61,24 @@ TEST(EngineSanity, EnergyConservation) motor->initialize(jointName); } + // Get all robot options + GenericConfig robotOptions = robot->getOptions(); + // Disable velocity and position limits - GenericConfig modelOptions = robot->getModelOptions(); + GenericConfig & modelOptions = boost::get(robotOptions.at("model")); GenericConfig & jointsOptions = boost::get(modelOptions.at("joints")); - boost::get(jointsOptions.at("enablePositionLimit")) = false; boost::get(jointsOptions.at("enableVelocityLimit")) = false; - robot->setModelOptions(modelOptions); // Disable torque limits - GenericConfig motorsOptions = robot->getMotorsOptions(); - for (auto & options : motorsOptions) + GenericConfig & motorsOptions = boost::get(robotOptions.at("motors")); + for (auto & motorOptionsItem : motorsOptions) { - GenericConfig & motorOptions = boost::get(options.second); + GenericConfig & motorOptions = boost::get(motorOptionsItem.second); boost::get(motorOptions.at("enableCommandLimit")) = false; } - robot->setMotorsOptions(motorsOptions); + + // Set all robot options + robot->setOptions(robotOptions); // Instantiate the controller robot->setController( @@ -112,7 +115,7 @@ TEST(EngineSanity, EnergyConservation) std::shared_ptr logDataPtr = engine.getLog(); const Eigen::VectorXd timesCont = getLogVariable(*logDataPtr, "Global.Time"); ASSERT_DOUBLE_EQ(timesCont[timesCont.size() - 1], tf); - const Eigen::VectorXd energyCont = getLogVariable(*logDataPtr, "HighLevelController.energy"); + const Eigen::VectorXd energyCont = getLogVariable(*logDataPtr, "energy"); ASSERT_GT(energyCont.size(), 0); // Check that energy is constant @@ -140,7 +143,7 @@ TEST(EngineSanity, EnergyConservation) logDataPtr = engine.getLog(); const Eigen::VectorXd timesDisc = getLogVariable(*logDataPtr, "Global.Time"); ASSERT_DOUBLE_EQ(timesDisc[timesDisc.size() - 1], tf); - const Eigen::VectorXd energyDisc = getLogVariable(*logDataPtr, "HighLevelController.energy"); + const Eigen::VectorXd energyDisc = getLogVariable(*logDataPtr, "energy"); ASSERT_GT(energyDisc.size(), 0); // Check that energy is constant diff --git a/core/unit/model_test.cc b/core/unit/model_test.cc index 068133dd0..02cf5d84a 100644 --- a/core/unit/model_test.cc +++ b/core/unit/model_test.cc @@ -39,9 +39,9 @@ TEST_P(ModelTestFixture, CreateFlexible) pinocchio::framesForwardKinematics(model->pinocchioModel_, pinocchioData, q); // Model is rigid, so configuration should not change - Eigen::VectorXd qflex; - model->getFlexiblePositionFromRigid(q, qflex); - ASSERT_TRUE(qflex.isApprox(q)); + Eigen::VectorXd qExtended; + model->getExtendedPositionFromTheoretical(q, qExtended); + ASSERT_TRUE(qExtended.isApprox(q)); auto visualData = pinocchio::GeometryData(model->visualModel_); pinocchio::updateGeometryPlacements( @@ -61,12 +61,12 @@ TEST_P(ModelTestFixture, CreateFlexible) boost::get(dynamicsOptions.at("flexibilityConfig")) = flexConfig; model->setOptions(options); - model->getFlexiblePositionFromRigid(q, qflex); - ASSERT_EQ(qflex.size(), + model->getExtendedPositionFromTheoretical(q, qExtended); + ASSERT_EQ(qExtended.size(), q.size() + Eigen::Quaterniond::Coefficients::RowsAtCompileTime * flexConfig.size()); // Recompute frame, geometry and collision pose, and check that nothing has moved. - pinocchio::framesForwardKinematics(model->pinocchioModel_, model->pinocchioData_, qflex); + pinocchio::framesForwardKinematics(model->pinocchioModel_, model->pinocchioData_, qExtended); pinocchio::updateGeometryPlacements( model->pinocchioModel_, model->pinocchioData_, model->visualModel_, model->visualData_); pinocchio::updateGeometryPlacements(model->pinocchioModel_, @@ -74,9 +74,9 @@ TEST_P(ModelTestFixture, CreateFlexible) model->collisionModel_, model->collisionData_); - for (uint32_t i = 0; i < model->pinocchioModelOrig_.frames.size(); i++) + for (uint32_t i = 0; i < model->pinocchioModelTh_.frames.size(); i++) { - const pinocchio::Frame & frame = model->pinocchioModelOrig_.frames[i]; + const pinocchio::Frame & frame = model->pinocchioModelTh_.frames[i]; const pinocchio::FrameIndex flexIndex = model->pinocchioModel_.getFrameId(frame.name); ASSERT_TRUE(pinocchioData.oMf[i].isApprox(model->pinocchioData_.oMf[flexIndex])); } diff --git a/data/bipedal_robots/atlas/atlas_v4_options.toml b/data/bipedal_robots/atlas/atlas_v4_options.toml index f86fea921..8a5bfa81a 100644 --- a/data/bipedal_robots/atlas/atlas_v4_options.toml +++ b/data/bipedal_robots/atlas/atlas_v4_options.toml @@ -26,5 +26,4 @@ torsion = 0.0 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = true diff --git a/data/bipedal_robots/cassie/cassie_options.toml b/data/bipedal_robots/cassie/cassie_options.toml index 9932dc2f3..7ba101ac3 100644 --- a/data/bipedal_robots/cassie/cassie_options.toml +++ b/data/bipedal_robots/cassie/cassie_options.toml @@ -25,5 +25,4 @@ friction = 0.5 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = true diff --git a/data/bipedal_robots/digit/digit_options.toml b/data/bipedal_robots/digit/digit_options.toml index 9932dc2f3..7ba101ac3 100644 --- a/data/bipedal_robots/digit/digit_options.toml +++ b/data/bipedal_robots/digit/digit_options.toml @@ -25,5 +25,4 @@ friction = 0.5 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = true diff --git a/data/quadrupedal_robots/anymal/anymal_options.toml b/data/quadrupedal_robots/anymal/anymal_options.toml index 6269f5675..eeeeb6fc0 100644 --- a/data/quadrupedal_robots/anymal/anymal_options.toml +++ b/data/quadrupedal_robots/anymal/anymal_options.toml @@ -25,5 +25,4 @@ friction = 1.0 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = true diff --git a/data/toys_models/ant/ant_options.toml b/data/toys_models/ant/ant_options.toml index 0a3debfef..4ae169e62 100644 --- a/data/toys_models/ant/ant_options.toml +++ b/data/toys_models/ant/ant_options.toml @@ -23,5 +23,4 @@ friction = 1.0 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = false diff --git a/docs/spec/src/tlmc_format_specification.md b/docs/spec/src/tlmc_format_specification.md index 027d68eb4..ffee01fac 100644 --- a/docs/spec/src/tlmc_format_specification.md +++ b/docs/spec/src/tlmc_format_specification.md @@ -58,7 +58,7 @@ GROUP "/" { (0): 1 } } - DATASET "HighLevelController.controlOffsetTimestamp" { + DATASET "controlOffsetTimestamp" { DATATYPE H5T_STRING { STRSIZE 8; STRPAD H5T_STR_NULLPAD; @@ -73,7 +73,7 @@ GROUP "/" { ... } GROUP "variables" { - GROUP "HighLevelController.currentPositionLeftSagittalHip" { + GROUP "currentPositionLeftSagittalHip" { DATASET "time" { DATATYPE H5T_STD_I64LE DATASPACE SIMPLE { ( 338623 ) / ( 338623 ) } diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 7259a7a52..76943d879 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -99,7 +99,11 @@ buildPythonWheel("python/${LIBRARY_NAME}_py" "${CMAKE_BINARY_DIR}/pypi/dist/${LIBRARY_NAME}_py") # Install Python package jiminy_py -deployPythonPackage("${LIBRARY_NAME}_py[meshcat,plot,dev]") +if(DEFINED CMAKE_SUPPRESS_DEVELOPER_WARNINGS AND NOT CMAKE_SUPPRESS_DEVELOPER_WARNINGS) + deployPythonPackage("${LIBRARY_NAME}_py[meshcat,plot,dev]") +else() + deployPythonPackage("${LIBRARY_NAME}_py[meshcat,plot]") +endif() ################ gym_jiminy ################ @@ -109,8 +113,9 @@ if(INSTALL_GYM_JIMINY) "python/gym_${LIBRARY_NAME}/envs" "python/gym_${LIBRARY_NAME}/rllib" "${CMAKE_BINARY_DIR}/pypi/dist/gym_${LIBRARY_NAME}") - deployPythonPackageDevelop("python/gym_${LIBRARY_NAME}/common" - "python/gym_${LIBRARY_NAME}/toolbox" + deployPythonPackageDevelop("python/gym_${LIBRARY_NAME}/common" FALSE) + deployPythonPackageDevelop("python/gym_${LIBRARY_NAME}/toolbox" "python/gym_${LIBRARY_NAME}/envs" - "python/gym_${LIBRARY_NAME}/rllib") + "python/gym_${LIBRARY_NAME}/rllib" + TRUE) # ALLOW_FAILURE endif() diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/__init__.py b/python/gym_jiminy/common/gym_jiminy/common/bases/__init__.py index 43e77ef9a..8be5b1b91 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/__init__.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/__init__.py @@ -1,5 +1,8 @@ # pylint: disable=missing-module-docstring +from .quantity import (SharedCache, + QuantityCreator, + AbstractQuantity) from .interfaces import (DT_EPS, ObsT, ActT, @@ -23,6 +26,9 @@ __all__ = [ + 'SharedCache', + 'QuantityCreator', + 'AbstractQuantity', 'DT_EPS', 'ObsT', 'ActT', diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/blocks.py b/python/gym_jiminy/common/gym_jiminy/common/bases/blocks.py index 16c41f109..0b77cb5c6 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/blocks.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/blocks.py @@ -170,7 +170,7 @@ def _setup(self) -> None: "environment simulation timestep.") @property - def get_fieldnames(self) -> FieldNested: + def fieldnames(self) -> FieldNested: """Get mapping between each scalar element of the observation space of the observer block and the associated fieldname for logging. @@ -234,7 +234,7 @@ def _setup(self) -> None: "environment simulation timestep.") @property - def get_fieldnames(self) -> FieldNested: + def fieldnames(self) -> FieldNested: """Get mapping between each scalar element of the action space of the controller block and the associated fieldname for logging. diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/interfaces.py b/python/gym_jiminy/common/gym_jiminy/common/bases/interfaces.py index f81ebef9b..739277a99 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/interfaces.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/interfaces.py @@ -4,7 +4,7 @@ """ from abc import abstractmethod, ABC from collections import OrderedDict -from typing import Dict, Any, TypeVar, Generic, no_type_check +from typing import Dict, Any, TypeVar, Generic, no_type_check, TYPE_CHECKING from typing_extensions import TypeAlias import numpy as np @@ -12,11 +12,12 @@ import gymnasium as gym import jiminy_py.core as jiminy -from jiminy_py.core import array_copyto # pylint: disable=no-name-in-module from jiminy_py.simulator import Simulator from jiminy_py.viewer.viewer import is_display_available from ..utils import DataNested +if TYPE_CHECKING: + from ..quantities import QuantityManager # Temporal resolution of simulator steps @@ -109,7 +110,7 @@ def _initialize_action_space(self) -> None: """ @abstractmethod - def compute_command(self, action: ActT) -> BaseActT: + def compute_command(self, action: ActT, command: BaseActT) -> None: """Compute the command to send to the subsequent block, based on the action and current observation of the environment. @@ -118,11 +119,11 @@ def compute_command(self, action: ActT) -> BaseActT: automatically prior to calling this method. :param action: High-level target to achieve by means of the command. - - :returns: Command to send to the subsequent block. It corresponds to - the target features of another lower-level controller if any, - the target motors efforts of the environment to ultimately - control otherwise. + :param command: Command to send to the subsequent block. It corresponds + to the target features of another lower-level + controller if any, the target motors efforts of the + environment to ultimately control otherwise. It must be + updated in-place. """ def compute_reward(self, @@ -184,6 +185,8 @@ class InterfaceJiminyEnv( sensor_measurements: SensorMeasurementStackMap is_simulation_running: npt.NDArray[np.bool_] + quantities: "QuantityManager" + action: ActT def __init__(self, *args: Any, **kwargs: Any) -> None: @@ -234,11 +237,8 @@ def _observer_handle(self, This method is not supposed to be called manually nor overloaded. :param t: Current simulation time. - :param q: Current actual configuration of the robot. Note that it is - not the one of the theoretical model even if - 'use_theoretical_model' is enabled for the backend Python - `Simulator`. - :param v: Current actual velocity vector. + :param q: Current extended configuration vector of the robot. + :param v: Current extended velocity vector of the robot. :param sensor_measurements: Current sensor data. """ # Refresh the observation if not already done but only if a simulation @@ -272,6 +272,10 @@ def _controller_handle(self, """Thin wrapper around user-specified `refresh_observation` and `compute_command` methods. + .. note:: + The internal cache of managed quantities is cleared right away + systematically, before anything else. + .. warning:: This method is not supposed to be called manually nor overloaded. It will be used by the base environment to instantiate a @@ -281,11 +285,8 @@ def _controller_handle(self, command motor torques directly to the robot. :param t: Current simulation time. - :param q: Current actual configuration of the robot. Note that it is - not the one of the theoretical model even if - 'use_theoretical_model' is enabled for the backend Python - `Simulator`. - :param v: Current actual velocity vector. + :param q: Current extended configuration vector of the robot. + :param v: Current actual velocity vector of the robot. :param sensor_measurements: Current sensor measurements. :param command: Output argument corresponding to motors torques to apply on the robot. It must be updated by reference @@ -293,12 +294,25 @@ def _controller_handle(self, :returns: Motors torques to apply on the robot. """ + # Reset the quantity manager. + # In principle, the internal cache of quantities should be cleared not + # each time the state of the robot and/or its derivative changes. This + # is hard to do because there is no way to detect this specifically at + # the time being. However, `_controller_handle` is never called twice + # in the exact same state by the engine, so resetting quantities at the + # beginning of the method should cover most cases. Yet, quantities + # cannot be used reliably in the definition of profile forces because + # they are always updated before the controller gets called, no matter + # if either one or the other is time-continuous. Hacking the internal + # dynamics to clear quantities does not address this issue either. + self.quantities.clear() + # Refresh the observation self._observer_handle(t, q, v, sensor_measurements) # No need to check for breakpoints of the controller because it already # matches the update period by design. - array_copyto(command, self.compute_command(self.action)) + self.compute_command(self.action, command) # Always consider that the observation must be refreshed after calling # '_controller_handle' as it is never called more often than necessary. diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py index 279b05f13..231810024 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py @@ -14,10 +14,9 @@ from copy import deepcopy from abc import abstractmethod from collections import OrderedDict -from itertools import chain from typing import ( - Dict, Any, List, Optional, Tuple, Union, Iterable, Generic, TypeVar, - SupportsFloat, Callable, cast) + Dict, Any, List, Optional, Tuple, Union, Generic, TypeVar, SupportsFloat, + Callable, cast) import numpy as np import gymnasium as gym @@ -71,6 +70,7 @@ def __init__(self, env: InterfaceJiminyEnv[BaseObsT, BaseActT], **kwargs: Any) -> None: """ + :param env: Base or already wrapped jiminy environment. :param kwargs: Extra keyword arguments for multiple inheritance. """ # Initialize some proxies for fast lookup @@ -100,13 +100,13 @@ def __getattr__(self, name: str) -> Any: """ return getattr(self.__getattribute__('env'), name) - def __dir__(self) -> Iterable[str]: + def __dir__(self) -> List[str]: """Attribute lookup. It is mainly used by autocomplete feature of Ipython. It is overloaded to get consistent autocompletion wrt `getattr`. """ - return chain(super().__dir__(), dir(self.env)) + return [*super().__dir__(), *dir(self.env)] @property def spec(self) -> Optional[EnvSpec]: @@ -397,12 +397,12 @@ def __init__(self, else: observation['measurement'] = base_observation if (state := self.observer.get_state()) is not None: - observation.setdefault( - 'states', OrderedDict())[ # type: ignore[index] - self.observer.name] = state - observation.setdefault( - 'features', OrderedDict())[ # type: ignore[index] - self.observer.name] = self.observer.observation + states = observation.setdefault('states', OrderedDict()) + assert isinstance(states, OrderedDict) + states[self.observer.name] = state + features = observation.setdefault('features', OrderedDict()) + assert isinstance(features, OrderedDict) + features[self.observer.name] = self.observer.observation self.observation = cast(NestedObsT, observation) # Register the observer's internal state and feature to the telemetry @@ -447,12 +447,14 @@ def _initialize_observation_space(self) -> None: else: observation_space['measurement'] = base_observation_space if self.observer.state_space is not None: - observation_space.setdefault( # type: ignore[index] - 'states', gym.spaces.Dict())[ - self.observer.name] = self.observer.state_space - observation_space.setdefault( # type: ignore[index] - 'features', gym.spaces.Dict())[ - self.observer.name] = self.observer.observation_space + state_spaces = observation_space.setdefault( + 'states', gym.spaces.Dict()) + assert isinstance(state_spaces, gym.spaces.Dict) + state_spaces[self.observer.name] = self.observer.state_space + feature_spaces = observation_space.setdefault( + 'features', gym.spaces.Dict()) + assert isinstance(feature_spaces, gym.spaces.Dict) + feature_spaces[self.observer.name] = self.observer.observation_space self.observation_space = gym.spaces.Dict(observation_space) def refresh_observation(self, measurement: EngineObsType) -> None: @@ -477,15 +479,16 @@ def refresh_observation(self, measurement: EngineObsType) -> None: if is_breakpoint(self.stepper_state.t, self.observe_dt, DT_EPS): self.observer.refresh_observation(self.env.observation) - def compute_command(self, action: ActT) -> np.ndarray: + def compute_command(self, action: ActT, command: np.ndarray) -> None: """Compute the motors efforts to apply on the robot. It simply forwards the command computed by the wrapped environment without any processing. :param action: High-level target to achieve by means of the command. + :param command: Lower-level command to updated in-place. """ - return self.env.compute_command(action) + self.env.compute_command(action, command) class ControlledJiminyEnv( @@ -594,9 +597,6 @@ def __init__(self, # Initialize base wrapper super().__init__(env, **kwargs) - # Define specialized operator(s) for efficiency - self._copyto_env_action = build_copyto(self.env.action) - # Allocate action buffer self.action: ActT = zeros(self.action_space) @@ -614,13 +614,13 @@ def __init__(self, else: observation['measurement'] = base_observation if (state := self.controller.get_state()) is not None: - observation.setdefault( - 'states', OrderedDict())[ # type: ignore[index] - self.controller.name] = state + states = observation.setdefault('states', OrderedDict()) + assert isinstance(states, OrderedDict) + states[self.controller.name] = state if self.augment_observation: - observation.setdefault( - 'actions', OrderedDict())[ # type: ignore[index] - self.controller.name] = self.action + actions = observation.setdefault('actions', OrderedDict()) + assert isinstance(actions, OrderedDict) + actions[self.controller.name] = self.action self.observation = cast(NestedObsT, observation) # Register the controller's internal state and target to the telemetry @@ -671,13 +671,15 @@ def _initialize_observation_space(self) -> None: else: observation_space['measurement'] = base_observation_space if self.controller.state_space is not None: - observation_space.setdefault( # type: ignore[index] - 'states', gym.spaces.Dict())[ - self.controller.name] = self.controller.state_space + state_spaces = observation_space.setdefault( + 'states', gym.spaces.Dict()) + assert isinstance(state_spaces, gym.spaces.Dict) + state_spaces[self.controller.name] = self.controller.state_space if self.augment_observation: - observation_space.setdefault( # type: ignore[index] - 'actions', gym.spaces.Dict())[ - self.controller.name] = self.controller.action_space + action_spaces = observation_space.setdefault( + 'actions', gym.spaces.Dict()) + assert isinstance(action_spaces, gym.spaces.Dict) + action_spaces[self.controller.name] = self.controller.action_space self.observation_space = gym.spaces.Dict(observation_space) def refresh_observation(self, measurement: EngineObsType) -> None: @@ -698,7 +700,7 @@ def refresh_observation(self, measurement: EngineObsType) -> None: """ self.env.refresh_observation(measurement) - def compute_command(self, action: ActT) -> np.ndarray: + def compute_command(self, action: ActT, command: np.ndarray) -> None: """Compute the motors efforts to apply on the robot. In practice, it updates whenever necessary: @@ -708,21 +710,21 @@ def compute_command(self, action: ActT) -> np.ndarray: subsequent block :param action: High-level target to achieve by means of the command. + :param command: Lower-level command to update in-place. """ # 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 # measure argument without issue. if is_breakpoint(self.stepper_state.t, self.control_dt, DT_EPS): - target = self.controller.compute_command(action) - self._copyto_env_action(target) + self.controller.compute_command(action, self.env.action) # Update the command to send to the actuators of the robot. # Note that the environment itself is responsible of making sure to # update the command at the right period. Ultimately, this is done # automatically by the engine, which is calling `_controller_handle` at # the right period. - return self.env.compute_command(self.env.action) + self.env.compute_command(self.env.action, command) def compute_reward(self, terminated: bool, @@ -791,15 +793,16 @@ def _initialize_action_space(self) -> None: """ self.action_space = self.env.action_space - def compute_command(self, action: ActT) -> np.ndarray: + def compute_command(self, action: ActT, command: np.ndarray) -> None: """Compute the motors efforts to apply on the robot. It simply forwards the command computed by the wrapped environment without any processing. :param action: High-level target to achieve by means of the command. + :param command: Lower-level command to update in-place. """ - return self.env.compute_command(action) + self.env.compute_command(action, command) def refresh_observation(self, measurement: EngineObsType) -> None: """Compute high-level features based on the current wrapped @@ -909,7 +912,9 @@ def refresh_observation(self, measurement: EngineObsType) -> None: """ self.env.refresh_observation(measurement) - def compute_command(self, action: TransformedActT) -> np.ndarray: + def compute_command(self, + action: TransformedActT, + command: np.ndarray) -> None: """Compute the motors efforts to apply on the robot. It calls `transform_action` at `step_dt` update period, which will @@ -921,13 +926,14 @@ def compute_command(self, action: TransformedActT) -> np.ndarray: user prior to calling this method. :param action: High-level target to achieve by means of the command. + :param command: Lower-level command to update in-place. """ # Transform action at the beginning of the step only if is_breakpoint(self.stepper_state.t, self._step_dt, DT_EPS): self.transform_action(action) # Delegate command computation to wrapped environment - return self.env.compute_command(self.env.action) + self.env.compute_command(self.env.action, command) @abstractmethod def transform_action(self, action: TransformedActT) -> None: diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/quantity.py b/python/gym_jiminy/common/gym_jiminy/common/bases/quantity.py new file mode 100644 index 000000000..e2dbefc59 --- /dev/null +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/quantity.py @@ -0,0 +1,454 @@ +"""This module promotes quantities as first-class objects. + +Defining quantities this way allows for standardization of common intermediary +metrics for computation of reward components and and termination conditions, eg +the center of pressure or the average spatial velocity of a frame. Overall, it +greatly reduces code duplication and bugs. + +Apart from that, it offers a dedicated quantity manager that is responsible for +optimizing the computation path, which is expected to significantly increase +the step collection throughput. This speedup is achieved by caching already +computed that did not changed since then, computing redundant intermediary +quantities only once per step, and gathering similar quantities in a large +batch to leverage vectorization of math instructions. +""" +import weakref +from weakref import ReferenceType +from abc import ABC, abstractmethod +from collections.abc import MutableSet +from functools import partial +from typing import ( + Any, Dict, List, Optional, Tuple, Generic, TypeVar, Type, Iterator, + Callable, cast) + +from .interfaces import InterfaceJiminyEnv + + +ValueT = TypeVar('ValueT') + +QuantityCreator = Tuple[Type["AbstractQuantity"], Dict[str, Any]] + + +class WeakMutableCollection(MutableSet, Generic[ValueT]): + """Mutable unordered list container storing weak reference to objects. + Elements will be discarded when no strong reference to the value exists + anymore, and a user-specified callback will be triggered if any. + + Internally, it is implemented as a set for which uniqueness is + characterized by identity instead of equality operator. + """ + def __init__(self, callback: Optional[Callable[[ + "WeakMutableCollection[ValueT]", ReferenceType + ], None]] = None) -> None: + """ + :param callback: Callback that will be triggered every time an element + is discarded from the container. + Optional: None by default. + """ + self._callback = callback + self._ref_list: List[ReferenceType] = [] + + def __callback__(self, ref: ReferenceType) -> None: + """Internal method that will be called every time an element must be + discarded from the containers, either because it was requested by the + user or because no strong reference to the value exists anymore. + + If a callback has been specified by the user, it will be triggered + after removing the weak reference from the container. + """ + self._ref_list.remove(ref) + if self._callback is not None: + self._callback(self, ref) + + def __contains__(self, obj: Any) -> bool: + """Dunder method to check if a weak reference to a given object is + already stored in the container, which is characterized by identity + instead of equality operator. + + :param obj: Object to look for in the container. + """ + return any(ref() is obj for ref in self._ref_list) + + def __iter__(self) -> Iterator[ValueT]: + """Dunder method that returns an iterator over the objects of the + container for which a string reference still exist. + """ + for ref in self._ref_list: + obj = ref() + if obj is not None: + yield obj + + def __len__(self) -> int: + """Dunder method that returns the length of the container. + """ + return len(self._ref_list) + + def add(self, value: ValueT) -> None: + """Add a new element to the container if not already contained. + + This has no effect if the element is already present. + + :param obj: Object to add to the container. + """ + if value not in self: + self._ref_list.append(weakref.ref(value, self.__callback__)) + + def discard(self, value: ValueT) -> None: + """Remove an element from the container if stored in it. + + This method does not raise an exception when the element is missing. + + :param obj: Object to remove from the container. + """ + if value not in self: + self.__callback__(weakref.ref(value)) + + +class SharedCache(Generic[ValueT]): + """Basic thread local shared cache. + + Its API mimics `std::optional` from the Standard C++ library. All it does + is encapsulating any Python object as a mutable variable, plus exposing a + simple mechanism for keeping track of all "owners" of the cache. + + .. warning:: + This implementation is not thread safe. + """ + + owners: WeakMutableCollection["AbstractQuantity"] + """Owners of the shared buffer, ie quantities relying on it to store the + result of their evaluation. This information may be useful for determining + the most efficient computation path overall. + + .. note:: + Quantities must add themselves to this list when passing them a shared + cache instance. + + .. note:: + Internally, it stores weak references to avoid holding alive quantities + that could be garbage collected otherwise. `WeakSet` cannot be used + because owners are objects all having the same hash, eg "identical" + quantities. + """ + + def __init__(self) -> None: + """ + """ + # Cached value if any + self._value: Optional[ValueT] = None + + # Whether a value is stored in cached + self._has_value: bool = False + + # Initialize "owners" of the shared buffer. + # Define callback to reset part of the computation graph whenever a + # quantity owning the cache gets garbage collected, namely all + # quantities that may assume at some point the existence of this + # deleted owner to find the adjust their computation path. + def _callback(self: WeakMutableCollection["AbstractQuantity"], + ref: ReferenceType # pylint: disable=unused-argument + ) -> None: + for owner in self: + while owner.parent is not None: + owner = owner.parent + owner.reset(reset_tracking=True) + + self.owners = WeakMutableCollection(_callback) + + @property + def has_value(self) -> bool: + """Whether a value is stored in cache. + """ + return self._has_value + + def reset(self) -> None: + """Clear value stored in cache if any. + """ + self._value = None + self._has_value = False + + def set(self, value: ValueT) -> None: + """Set value in cache, silently overriding the existing value if any. + + .. warning: + Beware the value is stored by reference for efficiency. It is up to + the user to copy it if necessary. + + :param value: Value to store in cache. + """ + self._value = value + self._has_value = True + + def get(self) -> ValueT: + """Return cached value if any, otherwise raises an exception. + """ + if self._has_value: + return cast(ValueT, self._value) + raise ValueError( + "No value has been stored. Please call 'set' before 'get'.") + + +class AbstractQuantity(ABC, Generic[ValueT]): + """Interface for quantities that involved in reward or termination + conditions evaluation. + + .. note:: + Quantities are meant to be managed automatically via `QuantityManager`. + Dealing with quantities manually is error-prone, and as such, is + strongly discourage. Nonetheless, power-user that understand the risks + are allowed to do it. + + .. warning:: + Mutual dependency between quantities is disallowed. + + .. warning:: + The user is responsible for implementing the dunder methods `__eq__` + and `__hash__` that characterize identical quantities. This property is + used internally by `QuantityManager` to synchronize cache between + them. It is advised to use decorator `@dataclass(unsafe_hash=True)` for + convenience, but it can also be done manually. + """ + + requirements: Dict[str, "AbstractQuantity"] + """Intermediary quantities on which this quantity may rely on for its + evaluation at some point, depending on the optimal computation path at + runtime. There values will be exposed to the user as usual properties. + """ + + def __init__(self, + env: InterfaceJiminyEnv, + parent: Optional["AbstractQuantity"], + requirements: Dict[str, QuantityCreator]) -> None: + """ + :param env: Base or wrapped jiminy environment. + :param parent: Higher-level quantity from which this quantity is a + requirement if any, `None` otherwise. + :param requirements: Intermediary quantities on which this quantity + depends for its evaluation, as a dictionary + whose keys are tuple gathering their respective + class and all their constructor keyword-arguments + except the environment 'env'. + """ + # Backup some of user argument(s) + self.env = env + self.parent = parent + + # Instantiate intermediary quantities if any + self.requirements: Dict[str, AbstractQuantity] = { + name: cls(env, self, **kwargs) + for name, (cls, kwargs) in requirements.items()} + + # Define some proxies for fast access + self.pinocchio_model = env.robot.pinocchio_model + self.pinocchio_data = env.robot.pinocchio_data + + # Shared cache handling + self._cache: Optional[SharedCache[ValueT]] = None + self._has_cache = False + + # Track whether the quantity has been called since previous reset + self._is_active = False + + # Whether the quantity must be re-initialized + self._is_initialized: bool = False + + # Add getter of all intermediary quantities dynamically. + # This approach is hacky but much faster than any of other official + # approach, ie implementing custom a `__getattribute__` method or even + # worst a custom `__getattr__` method. + def get_value(name: str, quantity: AbstractQuantity) -> Any: + return quantity.requirements[name].get() + + for name in self.requirements.keys(): + setattr(type(self), name, property(partial(get_value, name))) + + def __getattr__(self, name: str) -> Any: + """Get access to intermediary quantities as first-class properties, + without having to do it through `requirements`. + + .. warning:: + Accessing quantities this way is convenient, but unfortunately + much slower than do it through `requirements` manually. As a + result, this approach is mainly intended for ease of use while + prototyping. + + :param name: Name of the requested quantity. + """ + return self.__getattribute__('requirements')[name].get() + + def __dir__(self) -> List[str]: + """Attribute lookup. + + It is mainly used by autocomplete feature of Ipython. It is overloaded + to get consistent autocompletion wrt `getattr`. + """ + return [*super().__dir__(), *self.requirements.keys()] + + @property + def cache(self) -> SharedCache[ValueT]: + """Get shared cache if available, otherwise raises an exception. + + .. warning:: + This method is not meant to be overloaded. + """ + if not self._has_cache: + raise RuntimeError( + "No shared cache has been set for this quantity. Make sure it " + "is managed by some `QuantityManager` instance.") + return cast(SharedCache[ValueT], self._cache) + + @cache.setter + def cache(self, cache: Optional[SharedCache[ValueT]]) -> None: + """Set optional cache variable. When specified, it is used to store + evaluated quantity and retrieve its value later one. + + .. warning:: + Value is stored by reference for efficiency. It is up to the user + to the copy to retain its current value for later use if necessary. + + .. note:: + One may overload this method to encapsulate the cache in a custom + wrapper with specialized 'get' and 'set' methods before passing it + to the base implementation. For instance, to enforce copy of the + cached value before returning it. + """ + # Withdraw this quantity from the owners of its current cache if any + if self._cache is not None: + self._cache.owners.discard(self) + + # Declare this quantity as owner of the cache if specified + if cache is not None: + cache.owners.add(self) + + # Update internal cache attribute + self._cache = cache + self._has_cache = cache is not None + + def is_active(self, any_cache_owner: bool = False) -> bool: + """Whether this quantity is considered active, namely `initialize` has + been called at least once since previous tracking reset. + + :param any_owner: False to check only if this exact instance is active, + True if any of the identical quantities (sharing the + same cache) is considered sufficient. + Optional: False by default. + """ + if not any_cache_owner or self._cache is None: + return self._is_active + return any(owner._is_active for owner in self._cache.owners) + + def get(self) -> ValueT: + """Get cached value of requested quantity if available, otherwise + evaluate it and store it in cache. + + This quantity is considered active as soon as this method has been + called at least once since previous tracking reset. The method + `is_active` will be return true even before calling `initialize`. + + .. warning:: + This method is not meant to be overloaded. + """ + # Get value in cache if available. + # Note that direct access to internal `_value` attribute is preferred + # over the public API `get` for speedup. The same cannot be done for + # `has_value` as it would prevent mocking it during running unit tests + # or benchmarks. + if (self._has_cache and + self._cache.has_value): # type: ignore[union-attr] + self._is_active = True + return self._cache._value # type: ignore[union-attr,return-value] + + # Evaluate quantity + try: + if not self._is_initialized: + self.initialize() + assert (self._is_initialized and + self._is_active) # type: ignore[unreachable] + value = self.refresh() + except RecursionError as e: + raise LookupError( + "Mutual dependency between quantities is disallowed.") from e + + # Return value after storing it in shared cache if available + if self._has_cache: + self._cache.set(value) # type: ignore[union-attr] + return value + + def reset(self, reset_tracking: bool = False) -> None: + """Consider that the quantity must be re-initialized before being + evaluated once again. + + If shared cache is available, then it will be cleared and all identity + quantities will jointly be reset. + + .. note:: + This method must be called right before performing agent steps, + otherwise this quantity will not be refreshed if it was evaluated + previously. + + .. warning:: + This method is not meant to be overloaded. + + :param reset_tracking: Do not consider this quantity as active anymore + until the `get` method gets called once again. + Optional: False by default. + """ + # No longer consider this exact instance as initialized + self._is_initialized = False + + # No longer consider this exact instance as active if requested + if reset_tracking: + self._is_active = False + + # Reset all requirements first + for quantity in self.requirements.values(): + quantity.reset(reset_tracking) + + # More work has to be done if shared cache is available and has value + if self._has_cache: + # Early return if shared cache has no value + if not self.cache.has_value: + return + + # Invalidate cache before looping over all identical properties + self.cache.reset() + + # Reset all identical quantities + for owner in self.cache.owners: + owner.reset() + + def initialize(self) -> None: + """Initialize internal buffers. + + This is typically useful to refresh shared memory proxies or to + re-initialize pre-allocated buffers. + + .. warning:: + Intermediary quantities 'requirements' are NOT initialized + automatically because they can be initialized lazily in most cases, + or are optional depending on the most efficient computation path at + run-time. It is up to the developer implementing quantities to take + care of it. + + .. note:: + This method must be called before starting a new episode. + + .. note:: + Lazy-initialization is used for efficiency, ie `initialize` will be + called before the first time `refresh` has to be called, which may + never be the case if cache is shared between multiple identical + instances of the same quantity. + """ + # Refresh some proxies + self.pinocchio_model = self.env.robot.pinocchio_model + self.pinocchio_data = self.env.robot.pinocchio_data + + # The quantity is now considered initialized and active unconditionally + self._is_initialized = True + self._is_active = True + + @abstractmethod + def refresh(self) -> ValueT: + """Evaluate this quantity based on the agent state at the end of the + current agent step. + """ diff --git a/python/gym_jiminy/common/gym_jiminy/common/blocks/__init__.py b/python/gym_jiminy/common/gym_jiminy/common/blocks/__init__.py index f272bd154..8a5c45655 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/blocks/__init__.py +++ b/python/gym_jiminy/common/gym_jiminy/common/blocks/__init__.py @@ -2,7 +2,7 @@ from .mahony_filter import MahonyFilter from .motor_safety_limit import MotorSafetyLimit -from .proportional_derivative_controller import PDController +from .proportional_derivative_controller import PDController, PDAdapter from .deformation_estimator import DeformationEstimator @@ -10,5 +10,6 @@ 'MahonyFilter', 'MotorSafetyLimit', 'PDController', + 'PDAdapter', 'DeformationEstimator' ] diff --git a/python/gym_jiminy/common/gym_jiminy/common/blocks/deformation_estimator.py b/python/gym_jiminy/common/gym_jiminy/common/blocks/deformation_estimator.py index ff88cb670..902b7bfd1 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/blocks/deformation_estimator.py +++ b/python/gym_jiminy/common/gym_jiminy/common/blocks/deformation_estimator.py @@ -31,8 +31,8 @@ def _compute_orientation_error(obs_imu_quats: np.ndarray, kin_imu_quats: np.ndarray, dev_imu_quats: np.ndarray, ignore_twist: bool) -> None: - """Compute total deviation of observed IMU data wrt to rigid model in world - frame. + """Compute total deviation of observed IMU data wrt to theoretical model in + world frame. """ # Re-order IMU data for i, imu_index in enumerate(obs_imu_indices): @@ -51,9 +51,9 @@ def _compute_orientation_error(obs_imu_quats: np.ndarray, # Extract observed tilt: w_R_obs.T @ e_z obs_imu_tilts = np.stack(compute_tilt_from_quat(inv_obs_imu_quats), 1) - # Apply rigid kinematic IMU rotation on observed tilt. + # Apply theoretical kinematic IMU rotation on observed tilt. # The result can be interpreted as the tilt error between observed - # and rigid kinematic IMU orientation in world frame, ie: + # and theoretical kinematic IMU orientation in world frame, ie: # w_tilt_err = (w_R_kin @ w_R_obs.T) @ e_z for i, kin_imu_rot in enumerate(kin_imu_rots): obs_imu_tilts[i] = kin_imu_rot @ obs_imu_tilts[i] @@ -70,7 +70,7 @@ def _compute_orientation_error(obs_imu_quats: np.ndarray, # Conjugate quaternion of IMU orientation inv_obs_imu_quats[-1] *= -1 - # Compute one-by-one difference between observed and rigid + # Compute one-by-one difference between observed and theoretical # kinematic IMU orientations. quat_multiply(kin_imu_quats, inv_obs_imu_quats, @@ -158,7 +158,7 @@ def flexibility_estimator(obs_imu_quats: np.ndarray, .. warning:: The local deformation at each flexibility frame must be observable, ie the flexibility and IMU frames interleave with each others in a unique - and contiguous sub-chain in the rigid kinematic tree of the robot. + and contiguous sub-chain in theoretical kinematic tree of the robot. :param obs_imu_quats: Orientation estimates of an unordered arbitrary set of IMUs as a 2D array whose first dimension gathers @@ -172,8 +172,7 @@ def flexibility_estimator(obs_imu_quats: np.ndarray, `obs_imu_indices`. :param kin_imu_rots: Tuple of M kinematic frame orientations corresponding to the ordered subset of IMUs `obs_imu_indices`, for - the rigid configuration of the theoretical model of - the robot. + the configuration of the theoretical robot model. :param kin_imu_quats: Pre-allocated memory for storing the kinematic frame orientations of the ordered subset of IMUs of interest as a 2D array whose first dimension gathers @@ -194,11 +193,11 @@ def flexibility_estimator(obs_imu_quats: np.ndarray, :param is_chain_orphan: 2-Tuple stating whether first and last flexibility point is orphan respectively, ie only a single IMU is available for estimating its local deformation. - :param dev_imu_quats: Total deviation observed IMU data wrt to rigid model - in world frame for the ordered subset of IMUs of - interest as a 2D array whose first dimension gathers - the 4 quaternion coordinates while the second - corresponds to the M independent IMUs. + :param dev_imu_quats: Total deviation of th observed IMU data wrt to the + theoretical model in world frame for the ordered + subset of IMUs of interest, as a 2D array whose first + dimension gathers the 4 quaternion coordinates while + the second corresponds to the M independent IMUs. :param inv_child_dev_imu_quats: Total deviation observed IMU data in child flexibility frame as a 2D array whose first dimension gathers the 4 quaternion coordinates while @@ -217,7 +216,7 @@ def flexibility_estimator(obs_imu_quats: np.ndarray, and incidentally the twist of deformation at the flexibility points. """ - # Compute error between observed and rigid kinematic IMU orientation + # Compute error between observed and theoretical kinematic IMU orientation _compute_orientation_error(obs_imu_quats, obs_imu_indices, inv_obs_imu_quats, @@ -242,11 +241,10 @@ def get_flexibility_imu_frame_chains( flex_joint_names: Sequence[str], imu_frame_names: Sequence[str]) -> Sequence[Tuple[ Sequence[str], Sequence[Optional[str]], Sequence[bool]]]: - """Extract the minimal set of contiguous sub-chains in the kinematic tree - of a given flexible model that goes through all the specified flexibility - and IMU frames. + """Extract the minimal set of contiguous sub-chains in kinematic tree of a + given model that goes through all the specified flexibility and IMU frames. - :param pinocchio_model: Model for which to extract sub-chains. + :param pinocchio_model: Model from which to extract sub-chains. :param flex_joint_names: Unordered sequence of joint names that must be considered as associated with flexibility frames. :param imu_frame_names: Unordered sequence of frame names that must be @@ -420,73 +418,73 @@ class DeformationEstimator( that are presumably responsible for most of the whole deformation of the mechanical structure. - .. details:: - The number of IMU sensors and flexibility frames must be consistent: - * If the robot has no freeflyer, there must be as many IMU sensors - as flexibility frames (0), ie - - *---o---x---o---x---o---x - | - | - x---o---x - - * Otherwise, it can either have one more IMU than flexibility - frames (1), the same number (2), or up to one less IMU per - branch in the kinematic tree (3). - - (1) x---o---x---o---x---o---x - | - | - x---o---x - - (2) +---o---x---o---x---o---x - | - | - x---o---x - - (3) +---o---x---o---x---o---+ - | - | - x---o---+ - - *: Fixed base, +: leaf frame, x: IMU frame, o: flexibility frame - - (1): The pose of the freeflyer is ignored when estimating the - deformation at the flexibility frames in local frame. Mathematically, - it is the same as (0) when considering a virtual IMU with fixed - orientation to identity for the root joint. - - (2): One has to compensate for the missing IMU by providing the - configuration of the freeflyer. More precisely, one should ensure that - the orientation of the parent frame of the orphan flexibility frame - matches the reality for the rigid configuration. This usually requires - making some assumptions to guess to pose of the frame that is not - directly observable. Any discrepancy will be aggregated to the - estimated deformation for the orphan flexibility frame specifically - since both cannot be distinguished. This issue typically happens when - there is no IMUs in the feet of a legged robot. In such a case, there - is no better option than assuming that one of the active contact bodies - remains flat on the ground. If the twist of the IMUs are ignored, then - the twist of the contact body does not matter either, otherwise it must - be set appropriately by the user to get a meaningless estimate for the - twist of the deformation. If it cannot be observed by some - exteroceptive sensor such as vision, then the most reasonable - assumption is to suppose that it matches the twist of the IMU coming - right after in the kinematic tree. This way, they will cancel out each - other without adding bias to the twist of the orphan flexibility frame. - - (3): This case is basically the same as (2), with the addition that - only the deformation of one of the orphan flexibility frames can - be estimated at once, namely the one whose parent frame is declared as - having known orientation. The other ones will be set to identity. For a - legged robot, this corresponds to one of the contact bodies, usually - the one holding most of the total weight. + The number of IMU sensors and flexibility frames must be consistent: + * If the robot has no freeflyer, there must be as many IMU sensors as + flexibility frames (0), ie + + *---o---x---o---x---o---x + | + | + x---o---x + + * Otherwise, it can either have one more IMU than flexibility frames + (1), the same number (2), or up to one less IMU per branch in the + kinematic tree (3). + + (1) x---o---x---o---x---o---x + | + | + x---o---x + + (2) +---o---x---o---x---o---x + | + | + x---o---x + + (3) +---o---x---o---x---o---+ + | + | + x---o---+ + + *: Fixed base, +: leaf frame, x: IMU frame, o: flexibility frame + + (1): The pose of the freeflyer is ignored when estimating the deformation + at the flexibility frames in local frame. Mathematically, it is the + same as (0) when considering a virtual IMU with fixed orientation to + identity for the root joint. + + (2): One has to compensate for the missing IMU by providing instead the + configuration of the freeflyer. More precisely, one should ensure that + the orientation of the parent frame of the orphan flexibility frame + matches the reality for the theoretical configuration. This usually + requires making some assumptions to guess to pose of the frame that is + not directly observable. Any discrepancy will be aggregated to the + estimated deformation for the orphan flexibility frame specifically + since both cannot be distinguished. This issue typically happens when + there is no IMUs in the feet of a legged robot. In such a case, there + is no better option than assuming that one of the active contact + bodies remains flat on the ground. If the twist of the IMUs are + ignored, then the twist of the contact body does not matter either, + otherwise it must be set appropriately by the user to get a + meaningless estimate for the twist of the deformation. If it cannot be + observed by some exteroceptive sensor such as vision, then the most + reasonable assumption is to suppose that it matches the twist of the + IMU coming right after in the kinematic tree. This way, they will + cancel out each other without adding bias to the twist of the orphan + flexibility frame. + + (3): This case is basically the same as (2), with the addition that only + the deformation of one of the orphan flexibility frames can be + estimated at once, namely the one whose parent frame is declared as + having known orientation. The other ones will be set to identity. For + a legged robot, this corresponds to one of the contact bodies, usually + the one holding most of the total weight. .. warning:: (2) and (3) are not supported for now, as it requires using one - additional observation layer responsible for estimating the rigid - configuration of the robot including its freeflyer, along with the - name of the reference frame, ie the one having known orientation. + additional observation layer responsible for estimating the theoretical + configuration of the robot including its freeflyer, along with the name + of the reference frame, ie the one having known orientation. .. seealso:: Matthieu Vigne, Antonio El Khoury, Marine Pétriaux, Florent Di Meglio, @@ -546,19 +544,16 @@ def __init__(self, # Backup some of the user-argument(s) self.ignore_twist = ignore_twist - # Initialize jiminy model - self.model = jiminy.Model() - self.model.initialize(env.robot.pinocchio_model_th) - # Define proxies for fast access - self.pinocchio_model_th = self.model.pinocchio_model_th - self.pinocchio_data_th = self.model.pinocchio_data_th + self.pinocchio_model_th = env.robot.pinocchio_model_th.copy() + self.pinocchio_data_th = env.robot.pinocchio_data_th.copy() - # Create flexible dynamical model. + # Create flexible dynamic model. # Dummy physical parameters are specified as they have no effect on - # kinematic computations. It is only used for computing the flexible - # configuration if requested. - options = self.model.get_options() + # kinematic computations. + model = jiminy.Model() + model.initialize(env.robot.pinocchio_model_th) + options = model.get_options() for frame_name in flex_frame_names: options["dynamics"]["flexibilityConfig"].append( { @@ -568,35 +563,35 @@ def __init__(self, "inertia": np.ones(3), } ) - self.model.set_options(options) + model.set_options(options) # Extract contiguous chains of flexibility and IMU frames for which # computations can be vectorized. It also stores the information of # whether or not the sign of the deformation must be reversed to be # consistent with standard convention. - flexible_joint_names = self.model.flexible_joint_names + flexibility_joint_names = model.flexibility_joint_names flex_imu_frame_names_chains = get_flexibility_imu_frame_chains( - self.model.pinocchio_model, flexible_joint_names, imu_frame_names) + model.pinocchio_model, flexibility_joint_names, imu_frame_names) # Replace actual flex joint name by corresponding rigid frame self.flex_imu_frame_names_chains = [] for flex_frame_names_, imu_frame_names_, is_flipped in ( flex_imu_frame_names_chains): flex_frame_names_ = [ - flex_frame_names[flexible_joint_names.index(name)] + flex_frame_names[flexibility_joint_names.index(name)] for name in flex_frame_names_] self.flex_imu_frame_names_chains.append( (flex_frame_names_, imu_frame_names_, is_flipped)) # Check if a freeflyer estimator is required - if self.model.has_freeflyer: + if model.has_freeflyer: for _, imu_frame_names_, _ in self.flex_imu_frame_names_chains: if None in imu_frame_names_: raise NotImplementedError( "Freeflyer estimator is not supported for now.") # Backup flexibility frame names - self.flexible_frame_names = [ + self.flexibility_frame_names = [ name for flex_frame_names, _, _ in self.flex_imu_frame_names_chains for name in flex_frame_names] @@ -646,14 +641,13 @@ def __init__(self, assert isinstance(sensor, imu) imu_frame_map[sensor.frame_name] = sensor.index - # Make sure that the robot has one encoder per rigid joint + # Make sure that the robot has one encoder per mechanical joint encoder_sensor_names = env.robot.sensor_names[encoder.type] - if len(encoder_sensor_names) < len(self.model.rigid_joint_indices): + if len(encoder_sensor_names) < len(model.mechanical_joint_indices): raise ValueError( - "The robot must have one encoder per joint in theoretical " - "model (excluding floating base if any).") + "The robot must have one encoder per mechanical joints.") - # Extract mapping from encoders to rigid configuration. + # Extract mapping from encoders to theoretical configuration. # Note that revolute unbounded joints are not supported for now. self.encoder_to_config = [-1 for _ in range(env.robot.nmotors)] for i, sensor_name in enumerate(encoder_sensor_names): @@ -669,8 +663,8 @@ def __init__(self, # Note that they will be initialized in `_setup` method. self.encoder_data = np.array([]) - # Buffer storing the rigid configuration. - self._q_rigid = pin.neutral(self.pinocchio_model_th) + # Buffer storing the theoretical configuration. + self._q_th = pin.neutral(self.pinocchio_model_th) # Whether the observer has been compiled already self._is_compiled = False @@ -705,6 +699,12 @@ def _setup(self) -> None: # Call base implementation super()._setup() + # Refresh the theoretical model of the robot. + # Even if the robot may change, the theoretical model of the robot is + # not supposed to change in a way that would break this observer. + self.pinocchio_model_th = self.env.robot.pinocchio_model_th + self.pinocchio_data_th = self.env.robot.pinocchio_data_th + # Fix initialization of the observation to be valid quaternions self.observation[-1] = 1.0 @@ -732,16 +732,16 @@ def _setup(self) -> None: @property def fieldnames(self) -> List[List[str]]: - return [[f"{name}.Quat{e}" for name in self.flexible_frame_names] + return [[f"{name}.Quat{e}" for name in self.flexibility_frame_names] for e in ("x", "y", "z", "w")] def refresh_observation(self, measurement: BaseObsT) -> None: - # Estimate the rigid configuration of the robot from encoder data - self._q_rigid[self.encoder_to_config] = self.encoder_data + # Estimate the theoretical configuration of the robot from encoder data + self._q_th[self.encoder_to_config] = self.encoder_data - # Update kinematic quantities according to estimate rigid configuration + # Update kinematic quantities according to the estimated configuration pin.framesForwardKinematics( - self.pinocchio_model_th, self.pinocchio_data_th, self._q_rigid) + self.pinocchio_model_th, self.pinocchio_data_th, self._q_th) # Estimate all the deformations in their local frame. # It loops over each flexibility-imu chain independently. diff --git a/python/gym_jiminy/common/gym_jiminy/common/blocks/motor_safety_limit.py b/python/gym_jiminy/common/gym_jiminy/common/blocks/motor_safety_limit.py index 9e30b6f44..6c17a6b72 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/blocks/motor_safety_limit.py +++ b/python/gym_jiminy/common/gym_jiminy/common/blocks/motor_safety_limit.py @@ -28,8 +28,8 @@ def apply_safety_limits(command: np.ndarray, motors_soft_position_lower: np.ndarray, motors_soft_position_upper: np.ndarray, motors_velocity_limit: np.ndarray, - motors_effort_limit: np.ndarray - ) -> np.ndarray: + motors_effort_limit: np.ndarray, + out: np.ndarray) -> None: """Clip the command torque to ensure safe operation. It acts on each actuator independently and only activate close to the @@ -60,6 +60,7 @@ def apply_safety_limits(command: np.ndarray, :param motors_effort_limit: Maximum effort that the actuators can output. The command torque cannot exceed this limits, not even if needed to enforce safe operation. + :param out: Pre-allocated memory to store the command motor torques. """ # Computes velocity bounds based on margin from soft joint limit if any safe_velocity_lower = motors_velocity_limit * np.minimum(np.maximum( @@ -74,7 +75,7 @@ def apply_safety_limits(command: np.ndarray, -kd * (v_measured - safe_velocity_upper), -1.0), 1.0) # Clip command according to safe effort bounds - return np.minimum(np.maximum( + out[:] = np.minimum(np.maximum( command, safe_effort_lower), safe_effort_upper) @@ -168,7 +169,7 @@ def __init__(self, self.q_measured, self.v_measured = np.array([]), np.array([]) # Initialize the controller - super().__init__(name, env, 1) + super().__init__(name, env, update_ratio=1) def _initialize_action_space(self) -> None: """Configure the action space of the controller. @@ -190,12 +191,15 @@ def fieldnames(self) -> List[str]: return [f"currentMotorTorque{name}" for name in self.env.robot.motor_names] - def compute_command(self, action: np.ndarray) -> np.ndarray: + def compute_command(self, + action: np.ndarray, + command: np.ndarray) -> None: """Apply safety limits to the desired motor torques right before sending it to the robot so as to avoid exceeded prescribed position and velocity limits. :param action: Desired motor torques to apply on the robot. + :param command: Current motor torques that will be updated in-place. """ # Extract motor positions and velocity from encoder data q_measured, v_measured = self.q_measured, self.v_measured @@ -204,12 +208,13 @@ def compute_command(self, action: np.ndarray) -> np.ndarray: v_measured = v_measured[self.encoder_to_motor] # Clip command according to safe effort bounds - return apply_safety_limits(action, - q_measured, - v_measured, - self.kp, - self.kd, - self.motors_position_lower, - self.motors_position_upper, - self.motors_velocity_limit, - self.motors_effort_limit) + apply_safety_limits(action, + q_measured, + v_measured, + self.kp, + self.kd, + self.motors_position_lower, + self.motors_position_upper, + self.motors_velocity_limit, + self.motors_effort_limit, + command) diff --git a/python/gym_jiminy/common/gym_jiminy/common/blocks/proportional_derivative_controller.py b/python/gym_jiminy/common/gym_jiminy/common/blocks/proportional_derivative_controller.py index e72cf013c..c72e7b30d 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/blocks/proportional_derivative_controller.py +++ b/python/gym_jiminy/common/gym_jiminy/common/blocks/proportional_derivative_controller.py @@ -1,14 +1,12 @@ """Implementation of basic Proportional-Derivative controller block compatible with gym_jiminy reinforcement learning pipeline environment design. """ -import math import warnings from typing import List, Union import numpy as np import numba as nb import gymnasium as gym -from numpy.lib.stride_tricks import as_strided import jiminy_py.core as jiminy from jiminy_py.core import ( # pylint: disable=no-name-in-module @@ -19,94 +17,85 @@ from ..utils import fill -# Pre-computed factorial for small integers -INV_FACTORIAL_TABLE = tuple(1.0 / math.factorial(i) for i in range(4)) - # Name of the n-th position derivative -N_ORDER_DERIVATIVE_NAMES = ("Position", "Velocity", "Acceleration", "Jerk") - -# Command velocity deadband to reduce vibrations and internal efforts -EVAL_DEADBAND = 5.0e-3 - - -@nb.jit(nopython=True, cache=True, inline='always') -def toeplitz(col: np.ndarray, row: np.ndarray) -> np.ndarray: - """Numba-compatible implementation of `scipy.linalg.toeplitz` method. - - .. note: - Special cases are ignored for efficiency, hence the input types - are more respective than originally. - - .. warning: - It returns a strided matrix instead of contiguous copy for efficiency. - - .. seealso:: - https://docs.scipy.org/doc/scipy/reference/generated/scipy.linalg.toeplitz.html - - :param col: First column of the matrix. - :param row: First row of the matrix. - """ # noqa: E501 # pylint: disable=line-too-long - vals = np.concatenate((col[::-1], row[1:])) - stride = vals.strides[0] # pylint: disable=E1136 - return as_strided(vals[len(col)-1:], - shape=(len(col), len(row)), - strides=(-stride, stride)) +N_ORDER_DERIVATIVE_NAMES = ("Position", "Velocity", "Acceleration") @nb.jit(nopython=True, cache=True, inline='always', fastmath=True) -def integrate_zoh(state_prev: np.ndarray, +def integrate_zoh(state: np.ndarray, state_min: np.ndarray, state_max: np.ndarray, - dt: float) -> np.ndarray: - """N-th order exact integration scheme assuming Zero-Order-Hold for the - highest-order derivative, taking state bounds into account. - - .. warning:: - This method tries its best to keep the state within bounds, but it is - not always possible if the order is strictly larger than 1. Indeed, the - bounds of different derivative order may be conflicting. In such a - case, it gives priority to lower orders. - - :param state_prev: Previous state update, ordered from lowest to highest - derivative order, which means: - s[i](t) = s[i](t-1) + integ_{t-1}^{t}(s[i+1](t)) - :param state_min: Lower bounds of the state. - :param state_max: Upper bounds of the state. + dt: float) -> None: + """Second order approximate integration scheme assuming Zero-Order-Hold for + the acceleration, taking position, velocity and acceleration bounds into + account. + + Internally, it simply chains two first-order integrators in cascade. The + acceleration will be updated in-place if clipping is necessary to satisfy + bounds. + + :param state: Current state, ordered from lowest to highest derivative + order, ie: s[i](t) = s[i](t-1) + integ_{t-1}^{t}(s[i+1](t)), + as a 2D array whose first dimension gathers the 3 derivative + orders. It will be updated in-place. + :param state_min: Lower bounds of the state as a 2D array. + :param state_max: Upper bounds of the state as a 2D array. :param dt: Integration delta of time since previous state update. """ # Make sure that dt is not negative - assert dt >= 0.0, "The integration timestep 'dt' must be positive." + assert dt >= 0.0, "Integration backward in time is not supported." - # Early return if the timestep is too small + # Early return if timestep is too small if abs(dt) < 1e-9: - return state_prev.copy() - - # Compute integration matrix - dim, size = state_prev.shape - integ_coeffs = np.array([ - pow(dt, k) * INV_FACTORIAL_TABLE[k] for k in range(dim)]) - integ_matrix = toeplitz(integ_coeffs, np.zeros(dim)).T - integ_zero = integ_matrix[:, :-1].copy() @ state_prev[:-1] - integ_drift = integ_matrix[:, -1:] - - # Propagate derivative bounds to compute highest-order derivative bounds - deriv_min_stack = (state_min - integ_zero) / integ_drift - deriv_max_stack = (state_max - integ_zero) / integ_drift - deriv_min, deriv_max = np.full((size,), -np.inf), np.full((size,), np.inf) - for deriv_min_i, deriv_max_i in zip(deriv_min_stack, deriv_max_stack): - for k, (deriv_min_k, deriv_max_k) in enumerate(zip( - deriv_min, deriv_max)): - if deriv_min_k < deriv_min_i[k] < deriv_max_k: - deriv_min[k] = deriv_min_i[k] - if deriv_min_k < deriv_max_i[k] < deriv_max_k: - deriv_max[k] = deriv_max_i[k] - - # Clip highest-order derivative to ensure every derivative are within - # bounds if possible, lowest orders in priority otherwise. - deriv = np.minimum(np.maximum(state_prev[-1], deriv_min), deriv_max) - - # Integrate, taking into account clipped highest derivative - return integ_zero + integ_drift * deriv + return + + # Split position, velocity and acceleration orders for convenience + position, velocity, acceleration = state + position_min, velocity_min, acceleration_min = state_min + position_max, velocity_max, acceleration_max = state_max + + # Clip acceleration + acceleration[:] = np.minimum( + np.maximum(acceleration, acceleration_min), acceleration_max) + + # Backup the initial velocity to later compute the clipped acceleration + velocity_prev = velocity.copy() + + # Integrate acceleration 1-step ahead + velocity += acceleration * dt + + # Make sure that "true" velocity bounds are satisfied + velocity[:] = np.minimum(np.maximum(velocity, velocity_min), velocity_max) + + # Force slowing down early enough to avoid violating acceleration limits + # when hitting position bounds. + horizon = np.maximum( + np.floor(np.abs(velocity_prev) / acceleration_max / dt) * dt, dt) + position_min_delta = position_min - position + position_max_delta = position_max - position + drift = 0.5 * (horizon[horizon > dt] * (horizon[horizon > dt] - dt)) + position_min_delta[horizon > dt] -= drift * acceleration_max[horizon > dt] + position_max_delta[horizon > dt] += drift * acceleration_max[horizon > dt] + velocity_min = position_min_delta / horizon + velocity_max = position_max_delta / horizon + velocity[:] = np.minimum(np.maximum(velocity, velocity_min), velocity_max) + + # Velocity after hitting bounds must be cancellable in a single time step + velocity_mask = np.abs(velocity) > dt * acceleration_max + velocity_min = - np.maximum( + position_min_delta[velocity_mask] / velocity[velocity_mask], dt + ) * acceleration_max[velocity_mask] + velocity_max = np.maximum( + position_max_delta[velocity_mask] / velocity[velocity_mask], dt + ) * acceleration_max[velocity_mask] + velocity[velocity_mask] = np.minimum( + np.maximum(velocity[velocity_mask], velocity_min), velocity_max) + + # Back-propagate velocity clipping at the acceleration-level + acceleration[:] = (velocity - velocity_prev) / dt + + # Integrate position 1-step ahead + position += dt * velocity @nb.jit(nopython=True, cache=True, fastmath=True) @@ -118,20 +107,19 @@ def pd_controller(q_measured: np.ndarray, kp: np.ndarray, kd: np.ndarray, motors_effort_limit: np.ndarray, - control_dt: float) -> np.ndarray: - """Compute command under discrete-time proportional-derivative feedback - control. - - Internally, it integrates the command state over the controller update - period in order to obtain the desired motor positions 'q_desired' and - velocities 'v_desired'. By computing them this way, the desired motor - positions and velocities can be interpreted as targets should be reached - right before updating the command once again. The integration takes into - account some lower and upper bounds that ideally should not be exceeded. - If not possible, priority is given to consistency of the integration, so - no clipping of the command state ever occurs. The lower order bounds will - be satisfied first, which means that position limits are the only one to be - guaranteed to never be violated. + control_dt: float, + out: np.ndarray) -> None: + """Compute command torques under discrete-time proportional-derivative + feedback control. + + Internally, it integrates the command state (position, velocity and + acceleration) at controller update period in order to obtain the desired + motor positions 'q_desired' and velocities 'v_desired', takes into account + some lower and upper bounds. By computing them this way, the target motor + positions and velocities can be interpreted as targets that has be to reach + right before updating the command once again. Enforcing consistency between + target position and velocity in such a way is a necessary but insufficient + condition for the motors to be able to track them. The command effort is computed as follows: @@ -146,36 +134,87 @@ def pd_controller(q_measured: np.ndarray, :param q_measured: Current position of the actuators. :param v_measured: Current velocity of the actuators. :param command_state: Current command state, namely, all the derivatives of - the target motors positions up to order N. The order - must be larger than 2 but can be arbitrarily large. - :param command_state_lower: Lower bound of the command state that should be - satisfied if possible, prioritizing lower order - derivatives. - :param command_state_upper: Upper bound of the command state that should be - satisfied if possible, prioritizing lower order - derivatives. + the target motors positions up to acceleration order. + :param command_state_lower: Lower bound of the command state that must be + satisfied at all cost. + :param command_state_upper: Upper bound of the command state that must be + satisfied at all cost. :param kp: PD controller position-proportional gain in motor order. :param kd: PD controller velocity-proportional gain in motor order. :param motors_effort_limit: Maximum effort that the actuators can output. :param control_dt: Controller update period. It will be involved in the integration of the command state. + :param out: Pre-allocated memory to store the command motor torques. """ - # Integrate command state - command_state[:] = integrate_zoh( - command_state, command_state_lower, command_state_upper, control_dt) + # Integrate target motor positions and velocities + integrate_zoh(command_state, + command_state_lower, + command_state_upper, + control_dt) # Extract targets motors positions and velocities from command state - q_target, v_target = command_state[:2] + q_target, v_target, _ = command_state # Compute the joint tracking error q_error, v_error = q_target - q_measured, v_target - v_measured # Compute PD command - u_command = kp * (q_error + kd * v_error) + out[:] = kp * (q_error + kd * v_error) # Clip the command motors torques before returning - return np.minimum(np.maximum( - u_command, -motors_effort_limit), motors_effort_limit) + out[:] = np.minimum(np.maximum( + out, -motors_effort_limit), motors_effort_limit) + + +@nb.jit(nopython=True, cache=True, fastmath=True) +def pd_adapter(action: np.ndarray, + order: int, + command_state: np.ndarray, + command_state_lower: np.ndarray, + command_state_upper: np.ndarray, + dt: float, + out: np.ndarray) -> None: + """Compute the target motor accelerations that must be held constant for a + given time interval in order to reach the desired value of some derivative + of the target motor positions at the end of that interval if possible. + + Internally, it applies backward in time the same integration scheme as the + PD controller. Knowing the initial and final value of the derivative over + the time interval, constant target motor accelerations can be uniquely + deduced. In practice, it consists in successive clipped finite difference + of that derivative up to acceleration-level. + + :param action: Desired value of the n-th derivative of the command motor + positions at the end of the controller update. + :param order: Derivative order of the position associated with the action. + :param command_state: Current command state, namely, all the derivatives of + the target motors positions up to acceleration order. + :param command_state_lower: Lower bound of the command state that must be + satisfied at all cost. + :param command_state_upper: Upper bound of the command state that must be + satisfied at all cost. + :param dt: Time interval during which the target motor accelerations will + be held constant. + :param out: Pre-allocated memory to store the target motor accelerations. + """ + # Update command accelerations based on the action and its derivative order + if order == 2: + # The action corresponds to the command motor accelerations + out[:] = action + else: + if order == 0: + # Compute command velocity + velocity = (action - command_state[0]) / dt + + # Clip command velocity + velocity = np.minimum(np.maximum( + velocity, command_state_lower[1]), command_state_upper[1]) + else: + # The action corresponds to the command motor velocities + velocity = action + + # Compute command acceleration + out[:] = (velocity - command_state[1]) / dt def get_encoder_to_motor_map(robot: jiminy.Robot) -> Union[slice, List[int]]: @@ -220,57 +259,55 @@ class PDController( BaseControllerBlock[np.ndarray, np.ndarray, BaseObsT, np.ndarray]): """Low-level Proportional-Derivative controller. - The action corresponds to a given derivative of the target motors - positions. All the lower-order derivatives are obtained by integration, - considering that the action is constant until the next controller update. + The action are the target motors accelerations. The latter are integrated + twice using two first-order integrators in cascade, considering that the + acceleration is constant until the next controller update: - .. note:: - The higher the derivative order of the action, the smoother the command - motors torques. Thus, a high order is generally beneficial for robotic - applications. However, it introduces some kind of delay between the - action and its effects. This phenomenon makes learning more difficult - but most importantly, it limits the responsiveness of the agent - and therefore impedes its optimal performance. + v_{t+1} = v_{t} + dt * a_{t} + q_{t+1} = q_{t} + dt * v_{t+1} .. note:: The position and velocity bounds on the command corresponds to the joint limits specified by the dynamical model of the robot. Then, lax - higher-order bounds are extrapolated. In a single timestep of the - environment, they are chosen to be sufficient either to span the whole - range of the state derivative directly preceding (ie acceleration - bounds are inferred from velocity bounds) or to allow reaching the - command effort limits depending on what is the most restrictive. + default acceleration bounds are extrapolated. More precisely, they are + chosen to be sufficient either to span the whole range of velocity or + to allow reaching the command effort limits depending on what is the + most restrictive. Position, velocity and acceleration. .. warning:: - It must be connected directly to the environment to control without - any intermediary controllers altering the action space. + It must be connected directly to the base environment to control + without any intermediary controllers altering its action space. """ def __init__(self, name: str, env: InterfaceJiminyEnv[BaseObsT, np.ndarray], *, update_ratio: int = 1, - order: int, kp: Union[float, List[float], np.ndarray], kd: Union[float, List[float], np.ndarray], target_position_margin: float = 0.0, - target_velocity_limit: float = float("inf")) -> None: + target_velocity_limit: float = float("inf"), + target_acceleration_limit: float = float("inf")) -> None: """ :param name: Name of the block. :param env: Environment to connect with. :param update_ratio: Ratio between the update period of the controller and the one of the subsequent controller. -1 to match the simulation timestep of the environment. - :param order: Derivative order of the action. + Optional: 1 by default. :param kp: PD controller position-proportional gains in motor order. :param kd: PD controller velocity-proportional gains in motor order. :param target_position_margin: Minimum distance of the motor target positions from their respective bounds. - :param target_velocity_limit: Maximum motor target velocities. + Optional: 0.0 by default. + :param target_velocity_limit: Restrict maximum motor target velocities + wrt their hardware specifications. + Optional: "inf" by default. + :param target_acceleration_limit: + Restrict maximum motor target accelerations wrt their hardware + specifications. + Optional: "inf" by default. """ - # Make sure that the specified derivative order is valid - assert (0 < order < 4), "Derivative order of command out-of-bounds" - # Make sure the action space of the environment has not been altered if env.action_space is not env.unwrapped.action_space: raise RuntimeError( @@ -286,7 +323,6 @@ def __init__(self, "PD gains inconsistent with number of motors.") from e # Backup some user argument(s) - self.order = order self.kp = kp self.kd = kd @@ -309,7 +345,7 @@ def __init__(self, self.motors_effort_limit = env.robot.command_limit[ env.robot.motor_velocity_indices] - # Extract the motors target position and velocity bounds from the model + # Extract the motors target position bounds from the model motors_position_lower: List[float] = [] motors_position_upper: List[float] = [] for motor_name in env.robot.motor_names: @@ -324,62 +360,59 @@ def __init__(self, upper = env.robot.position_limit_upper[motor_position_index] motors_position_lower.append(lower + target_position_margin) motors_position_upper.append(upper - target_position_margin) + + # Extract the motors target velocity bounds motors_velocity_limit = np.minimum( env.robot.velocity_limit[env.robot.motor_velocity_indices], target_velocity_limit) - command_state_lower = [ - np.array(motors_position_lower), -motors_velocity_limit] - command_state_upper = [ - np.array(motors_position_upper), motors_velocity_limit] - - # Try to infers bounds for higher-order derivatives if necessary. - # They are tuned to allow for bang-bang control without restriction. - step_dt = env.step_dt - for i in range(2, order + 1): - range_limit = ( - command_state_upper[-1] - command_state_lower[-1]) / step_dt - effort_limit = self.motors_effort_limit / ( - self.kp * step_dt ** (i - 1) * INV_FACTORIAL_TABLE[i - 1] * - np.maximum(step_dt / i, self.kd)) - n_order_limit = np.minimum(range_limit, effort_limit) - command_state_lower.append(-n_order_limit) - command_state_upper.append(n_order_limit) - self._command_state_lower = np.stack(command_state_lower, axis=0) - self._command_state_upper = np.stack(command_state_upper, axis=0) + + # Compute acceleration bounds allowing unrestricted bang-bang control + range_limit = 2 * motors_velocity_limit / env.step_dt + effort_limit = self.motors_effort_limit / ( + self.kp * env.step_dt * np.maximum(env.step_dt / 2, self.kd)) + acceleration_limit = np.minimum( + np.minimum(range_limit, effort_limit), target_acceleration_limit) + + # Compute command state bounds + self._command_state_lower = np.stack([np.array(motors_position_lower), + -motors_velocity_limit, + -acceleration_limit], axis=0) + self._command_state_upper = np.stack([np.array(motors_position_upper), + motors_velocity_limit, + acceleration_limit], axis=0) # Extract measured motor positions and velocities for fast access. # Note that they will be initialized in `_setup` method. self.q_measured, self.v_measured = np.array([]), np.array([]) # Allocate memory for the command state - self._command_state = np.zeros((order + 1, env.robot.nmotors)) + self._command_state = np.zeros((3, env.robot.nmotors)) # Initialize the controller super().__init__(name, env, update_ratio) - # Reference to highest-order derivative for fast access - self._action = self._command_state[-1] + # References to command acceleration for fast access + self._command_acceleration = self._command_state[2] def _initialize_state_space(self) -> None: """Configure the state space of the controller. - The state spaces corresponds to all the derivatives of the target - motors positions up to order N-1. + The state spaces corresponds to the target motors positions and + velocities. """ self.state_space = gym.spaces.Box( - low=self._command_state_lower[:-1], - high=self._command_state_upper[:-1], + low=self._command_state_lower[:2], + high=self._command_state_upper[:2], dtype=np.float64) def _initialize_action_space(self) -> None: """Configure the action space of the controller. - The action spaces corresponds to the N-th order derivative of the - target motors positions. + The action spaces corresponds to the target motors accelerations. """ self.action_space = gym.spaces.Box( - low=self._command_state_lower[-1], - high=self._command_state_upper[-1], + low=self._command_state_lower[2], + high=self._command_state_upper[2], dtype=np.float64) def _setup(self) -> None: @@ -400,14 +433,14 @@ def _setup(self) -> None: @property def fieldnames(self) -> List[str]: - return [f"target{N_ORDER_DERIVATIVE_NAMES[self.order]}{name}" + return [f"currentTarget{N_ORDER_DERIVATIVE_NAMES[2]}{name}" for name in self.env.robot.motor_names] def get_state(self) -> np.ndarray: - return self._command_state[:-1] + return self._command_state[:2] - def compute_command(self, action: np.ndarray) -> np.ndarray: - """Compute the motor torques using a PD controller. + def compute_command(self, action: np.ndarray, command: np.ndarray) -> None: + """Compute the target motor torques using a PD controller. It is proportional to the error between the observed motors positions/ velocities and the target ones. @@ -416,7 +449,8 @@ def compute_command(self, action: np.ndarray) -> np.ndarray: Calling this method manually while a simulation is running is forbidden, because it would mess with the controller update period. - :param action: Desired N-th order deriv. of the target motor positions. + :param action: Desired target motor acceleration. + :param command: Current motor torques that will be updated in-place. """ # Re-initialize the command state to the current motor state if the # simulation is not running. This must be done here because the @@ -430,23 +464,18 @@ def compute_command(self, action: np.ndarray) -> np.ndarray: self._command_state_upper[i], out=self._command_state[i]) - # Update the highest order derivative of the target motor positions to - # match the provided action. - array_copyto(self._action, action) - - # Dead band to avoid slow drift of target at rest for evaluation only - if not self.env.is_training: - self._action[np.abs(self._action) > EVAL_DEADBAND] = 0.0 - # Extract motor positions and velocity from encoder data q_measured, v_measured = self.q_measured, self.v_measured if not self._is_same_order: q_measured = q_measured[self.encoder_to_motor] v_measured = v_measured[self.encoder_to_motor] + # Update target motor accelerations + array_copyto(self._command_acceleration, action) + # Compute the motor efforts using PD control. # The command state must not be updated if no simulation is running. - return pd_controller( + pd_controller( q_measured, v_measured, self._command_state, @@ -455,4 +484,107 @@ def compute_command(self, action: np.ndarray) -> np.ndarray: self.kp, self.kd, self.motors_effort_limit, - self.control_dt if is_simulation_running else 0.0) + self.control_dt if is_simulation_running else 0.0, + command) + + +class PDAdapter( + BaseControllerBlock[np.ndarray, np.ndarray, BaseObsT, np.ndarray]): + """Adapt the action of a lower-level Proportional-Derivative controller + to be the target motor positions or velocities rather than accelerations. + + The action is the desired value of some derivative of the target motor + positions. The target motor accelerations are then deduced so as to reach + this value by the next update of this controller if possible without + exceeding the position, velocity and acceleration bounds. Finally, these + target position accelerations are passed to a lower-level PD controller, + usually running at a higher frequency. + + .. note:: + The higher the derivative order of the action, the smoother the command + motor torques. Thus, a high order is generally beneficial for robotic + applications. However, it introduces some kind of delay between the + action and its effects. This phenomenon limits the responsiveness of + the agent and therefore impedes its optimal performance. Besides, it + introduces addition layer of indirection between the action and its + effect which may be difficult to grasp for the agent. Finally, + exploration usually consists in addition temporally uncorrelated + gaussian random process at action-level. The effect of such random + processes tends to vanish when integrated, making exploration very + inefficient. + """ + def __init__(self, + name: str, + env: InterfaceJiminyEnv[BaseObsT, np.ndarray], + *, + update_ratio: int = -1, + order: int = 1) -> None: + """ + :param update_ratio: Ratio between the update period of the controller + and the one of the subsequent controller. -1 to + match the simulation timestep of the environment. + Optional: -1 by default. + :param order: Derivative order of the action. It accepts position or + velocity (respectively 0 or 1). + Optional: 1 by default. + """ + # Make sure that the specified derivative order is valid + assert order in (0, 1), "Derivative order out-of-bounds" + + # Make sure that a PD controller block is already connected + controller = env.controller # type: ignore[attr-defined] + if not isinstance(controller, PDController): + raise RuntimeError( + "This block must be directly connected to a lower-level " + "`PDController` block.") + + # Backup some user argument(s) + self.order = order + + # Define some proxies for convenience + self._pd_controller = controller + + # Allocate memory for the target motor accelerations + self._target_accelerations = np.zeros((env.robot.nmotors,)) + + # Initialize the controller + super().__init__(name, env, update_ratio) + + def _initialize_action_space(self) -> None: + """Configure the action space of the controller. + + The action spaces corresponds to the N-th order derivative of the + target motors positions. + """ + self.action_space = gym.spaces.Box( + low=self._pd_controller._command_state_lower[self.order], + high=self._pd_controller._command_state_upper[self.order], + dtype=np.float64) + + def _setup(self) -> None: + # Call base implementation + super()._setup() + + # Reset the target motor accelerations + fill(self._target_accelerations, 0) + + @property + def fieldnames(self) -> List[str]: + return [f"nextTarget{N_ORDER_DERIVATIVE_NAMES[self.order]}{name}" + for name in self.env.robot.motor_names] + + def compute_command(self, action: np.ndarray, command: np.ndarray) -> None: + """Compute the target motor accelerations from the desired value of + some derivative of the target motor positions. + + :param action: Desired target motor acceleration. + :param command: Current motor torques that will be updated in-place. + """ + pd_adapter( + action, + self.order, + self._pd_controller._command_state, + self._pd_controller._command_state_lower, + self._pd_controller._command_state_upper, + self.control_dt, + command) diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py index 67517b554..9d49ac2a6 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py @@ -10,11 +10,10 @@ from copy import deepcopy from collections import OrderedDict from collections.abc import Mapping -from itertools import chain from functools import partial from typing import ( - Dict, Any, List, cast, no_type_check, Optional, Tuple, Callable, Iterable, - Union, SupportsFloat, Iterator, Generic, Sequence, Mapping as MappingT, + Dict, Any, List, cast, no_type_check, Optional, Tuple, Callable, Union, + SupportsFloat, Iterator, Generic, Sequence, Mapping as MappingT, MutableMapping as MutableMappingT) import numpy as np @@ -56,6 +55,7 @@ SensorMeasurementStackMap, EngineObsType, InterfaceJiminyEnv) +from ..quantities import QuantityManager from .internal import loop_interactive @@ -152,6 +152,11 @@ def __init__(self, environments with multiple inheritance, and to allow automatic pipeline wrapper generation. """ + # Make sure that the simulator is single-robot + if tuple(robot.name for robot in simulator.robots) != ("",): + raise ValueError( + "`BaseJiminyEnv` only supports single-robot simulators.") + # Handling of default rendering mode viewer_backend = (simulator.viewer or Viewer).backend if render_mode is None: @@ -187,7 +192,8 @@ def __init__(self, self._robot_state_q = np.array([]) self._robot_state_v = np.array([]) self._robot_state_a = np.array([]) - self.sensor_measurements: SensorMeasurementStackMap = OrderedDict() + self.sensor_measurements: SensorMeasurementStackMap = OrderedDict( + self.robot.sensor_measurements) # Top-most block of the pipeline to which the environment is part of self._env_derived: InterfaceJiminyEnv = self @@ -237,6 +243,9 @@ def __init__(self, "`BaseJiminyEnv.compute_command` must be overloaded in case " "of custom action spaces.") + # Initialize a quantity manager for later use + self.quantities = QuantityManager(self) + # Define specialized operators for efficiency. # Note that a partial view of observation corresponding to measurement # must be extracted since only this one must be updated during refresh. @@ -290,13 +299,13 @@ def __getattr__(self, name: str) -> Any: """ return getattr(self.__getattribute__('simulator'), name) - def __dir__(self) -> Iterable[str]: + def __dir__(self) -> List[str]: """Attribute lookup. It is mainly used by autocomplete feature of Ipython. It is overloaded to get consistent autocompletion wrt `getattr`. """ - return chain(super().__dir__(), dir(self.simulator)) + return [*super().__dir__(), *dir(self.simulator)] def __del__(self) -> None: try: @@ -313,30 +322,17 @@ def _get_time_space(self) -> spaces.Box: shape=(), dtype=np.float64) - def _get_agent_state_space(self, - use_theoretical_model: Optional[bool] = None - ) -> spaces.Dict: + def _get_agent_state_space(self) -> spaces.Dict: """Get state space. This method is not meant to be overloaded in general since the definition of the state space is mostly consensual. One must rather overload `_initialize_observation_space` to customize the observation space as a whole. - - :param use_theoretical_model: Whether to compute the state space - corresponding to the theoretical model to - the actual one. `None` to use internal - value 'simulator.use_theoretical_model'. - Optional: `None` by default. """ - # Handling of default argument - if use_theoretical_model is None: - use_theoretical_model = self.simulator.use_theoretical_model - # Define some proxies for convenience model_options = self.robot.get_model_options() - joint_position_indices = self.robot.rigid_joint_position_indices - joint_velocity_indices = self.robot.rigid_joint_velocity_indices + joint_velocity_indices = self.robot.mechanical_joint_velocity_indices position_limit_upper = self.robot.position_limit_upper position_limit_lower = self.robot.position_limit_lower velocity_limit = self.robot.velocity_limit @@ -349,25 +345,16 @@ def _get_agent_state_space(self, velocity_limit[:3] = FREEFLYER_VEL_LIN_MAX velocity_limit[3:6] = FREEFLYER_VEL_ANG_MAX - for joint_index in self.robot.flexible_joint_indices: + for joint_index in self.robot.flexibility_joint_indices: joint_velocity_index = ( self.robot.pinocchio_model.joints[joint_index].idx_v) velocity_limit[ joint_velocity_index + np.arange(3)] = FLEX_VEL_ANG_MAX - if not model_options['joints']['enablePositionLimit']: - position_limit_lower[joint_position_indices] = -JOINT_POS_MAX - position_limit_upper[joint_position_indices] = JOINT_POS_MAX - if not model_options['joints']['enableVelocityLimit']: velocity_limit[joint_velocity_indices] = JOINT_VEL_MAX - # Define bounds of the state space - if use_theoretical_model: - position_limit_lower = position_limit_lower[joint_position_indices] - position_limit_upper = position_limit_upper[joint_position_indices] - velocity_limit = velocity_limit[joint_velocity_indices] - + # Aggregate position and velocity bounds to define state space return spaces.Dict(OrderedDict( q=spaces.Box(low=position_limit_lower, high=position_limit_upper, @@ -408,8 +395,7 @@ def _get_measurements_space(self) -> spaces.Dict: # Define some proxies for convenience sensor_measurements = self.robot.sensor_measurements command_limit = self.robot.command_limit - position_space, velocity_space = self._get_agent_state_space( - use_theoretical_model=False).values() + position_space, velocity_space = self._get_agent_state_space().values() assert isinstance(position_space, spaces.Box) assert isinstance(velocity_space, spaces.Box) @@ -688,6 +674,7 @@ def reset(self, # type: ignore[override] # Re-initialize some shared memories. # It is necessary because the robot may have changed. + self.robot = self.simulator.robot self.robot_state = self.simulator.robot_state self.sensor_measurements = OrderedDict(self.robot.sensor_measurements) @@ -717,7 +704,7 @@ def reset(self, # type: ignore[override] # Extract the observer/controller update period. # The controller update period is used by default for the observer if # it was not specify by the user in `_setup`. - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() self.control_dt = float( engine_options['stepper']['controllerUpdatePeriod']) if self.observe_dt < 0.0: @@ -743,8 +730,7 @@ def reset(self, # type: ignore[override] # Sample the initial state and reset the low-level engine q_init, v_init = self._sample_state() - if not jiminy.is_position_valid( - self.simulator.pinocchio_model, q_init): + if not jiminy.is_position_valid(self.robot.pinocchio_model, q_init): raise RuntimeError( "The initial state provided by `_sample_state` is " "inconsistent with the dimension or types of joints of the " @@ -764,6 +750,12 @@ def reset(self, # type: ignore[override] self.robot.compute_sensor_measurements( 0.0, q_init, v_init, a_init, u_motor, f_external) + # Re-initialize the quantity manager. + # Note that computation graph tracking is never reset automatically. + # It is the responsibility of the practitioner implementing a derived + # environment whenever it makes sense for its specific use-case. + self.quantities.reset(reset_tracking=False) + # Run the reset hook if any. # Note that the reset hook must be called after `_setup` because it # expects that the robot is not going to change anymore at this point. @@ -791,8 +783,7 @@ def reset(self, # type: ignore[override] register_variables(self.robot.controller, header, value) # Start the engine - self.simulator.start( - q_init, v_init, None, self.simulator.use_theoretical_model) + self.simulator.start(q_init, v_init) # Refresh robot_state proxies. It must be done here because memory is # only allocated by the engine when starting a simulation. @@ -846,9 +837,11 @@ def reset(self, # type: ignore[override] # Note that the viewer must be reset if available, otherwise it would # keep using the old robot model for display, which must be avoided. if self.simulator.is_viewer_available: - self.simulator.viewer._setup(self.robot) - if self.simulator.viewer.has_gui(): - self.simulator.viewer.refresh() + viewer = self.simulator.viewer + assert viewer is not None # Assert(s) for type checker + viewer._setup(self.robot) # type: ignore[attr-defined] + if viewer.has_gui(): + viewer.refresh() return obs, deepcopy(self._info) @@ -1008,12 +1001,18 @@ def render(self) -> Optional[Union[RenderFrame, List[RenderFrame]]]: return self.simulator.render( # type: ignore[return-value] return_rgb_array=self.render_mode == 'rgb_array') - def plot(self, **kwargs: Any) -> TabbedFigure: + def plot(self, + enable_block_states: bool = False, + **kwargs: Any) -> TabbedFigure: """Display common simulation data and action over time. .. Note: - It adds "Action" tab on top of original `Simulator.plot`. + It adds tabs for the base environment action plus all blocks + ((state, action) for controllers and (state, features) for + observers) on top of original `Simulator.plot`. + :param enable_block_states: Whether to display the internal state of + all blocks. :param kwargs: Extra keyword arguments to forward to `simulator.plot`. """ # Call base implementation @@ -1026,35 +1025,38 @@ def plot(self, **kwargs: Any) -> TabbedFigure: "Nothing to plot. Please run a simulation before calling " "`plot` method.") - # Extract action. - # If telemetry action fieldnames is a dictionary, it cannot be nested. - # In such a case, keys corresponds to subplots, and values are - # individual scalar data over time to be displayed to the same subplot. - t = log_vars["Global.Time"] - tab_data: Dict[str, Union[np.ndarray, Dict[str, np.ndarray]]] = {} - action_fieldnames = self.log_fieldnames.get("action") - if action_fieldnames is None: - # It was impossible to register the action to the telemetry, likely - # because of incompatible dtype. Early return without adding tab. - return figure - if isinstance(action_fieldnames, dict): - for group, fieldnames in action_fieldnames.items(): - if not isinstance(fieldnames, list): - LOGGER.error( - "Action space not supported by this method.") - return figure - tab_data[group] = { + # Plot all registered variables + for key, fieldnames in self.log_fieldnames.items(): + # Filter state if requested + if not enable_block_states and key.endswith(".state"): + continue + + # Extract action hierarchical time series. + # Fieldnames stored in a dictionary cannot be nested. In such a + # case, keys corresponds to subplots, and values are individual + # scalar data over time to be displayed to the same subplot. + t = log_vars["Global.Time"] + tab_data: Dict[str, Union[np.ndarray, Dict[str, np.ndarray]]] = {} + if isinstance(fieldnames, dict): + for group, subfieldnames in fieldnames.items(): + if not isinstance(subfieldnames, list): + LOGGER.error( + "Action space not supported by this method.") + return figure + value_map = extract_variables_from_log( + log_vars, subfieldnames, "controller", as_dict=True) + tab_data[group] = { + key.split(".", 2)[2]: value + for key, value in value_map.items()} + elif isinstance(fieldnames, list): + value_map = extract_variables_from_log( + log_vars, fieldnames, "controller", as_dict=True) + tab_data.update({ key.split(".", 2)[2]: value - for key, value in extract_variables_from_log( - log_vars, fieldnames, as_dict=True).items()} - elif isinstance(action_fieldnames, list): - tab_data.update({ - key.split(".", 2)[2]: value - for key, value in extract_variables_from_log( - log_vars, action_fieldnames, as_dict=True).items()}) + for key, value in value_map.items()}) - # Add action tab - figure.add_tab(" ".join(("Env", "Action")), t, tab_data) + # Add action tab + figure.add_tab(key.replace(".", " "), t, tab_data) # Return figure for convenience and consistency with Matplotlib return figure @@ -1132,8 +1134,8 @@ def play_interactive(self, # Make sure viewer gui is open, so that the viewer will shared external # forces with the robot automatically. - if not (self.simulator.is_viewer_available and - self.simulator.viewer.has_gui()): + viewer = self.simulator.viewer + if viewer is None or not viewer.has_gui(): self.simulator.render(update_ground_profile=False) # Reset the environnement @@ -1143,15 +1145,18 @@ def play_interactive(self, # Refresh the ground profile self.simulator.render(update_ground_profile=True) + viewer = self.simulator.viewer + assert viewer is not None # Assert(s) for type checker # Enable travelling if enable_travelling is None: - enable_travelling = \ - self.simulator.viewer.backend.startswith('panda3d') + backend = viewer.backend + assert backend is not None # Assert(s) for type checker + enable_travelling = backend.startswith('panda3d') enable_travelling = enable_travelling and self.robot.has_freeflyer if enable_travelling: tracked_frame = self.robot.pinocchio_model.frames[2].name - self.simulator.viewer.attach_camera(tracked_frame) + viewer.attach_camera(tracked_frame) # Refresh the scene once again to update camera placement self.render() @@ -1178,7 +1183,7 @@ def _interact(key: Optional[str] = None) -> bool: # Disable travelling if it enabled if enable_travelling: - self.simulator.viewer.detach_camera() + viewer.detach_camera() # Stop the simulation to unlock the robot. # It will enable to display contact forces for replay. @@ -1310,7 +1315,7 @@ def _setup(self) -> None: super()._setup() # Configure the low-level integrator - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options["stepper"]["iterMax"] = 0 if self.debug: engine_options["stepper"]["verbose"] = True @@ -1326,7 +1331,7 @@ def _setup(self) -> None: engine_options["telemetry"]["isPersistent"] = False # Update engine options - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def _initialize_observation_space(self) -> None: """Configure the observation of the environment. @@ -1366,17 +1371,13 @@ def _neutral(self) -> np.ndarray: # Get the neutral configuration of the actual model q = pin.neutral(self.robot.pinocchio_model) - # Make sure it is not out-of-bounds + # Make sure it is not out-of-bounds before returning position_limit_lower = self.robot.position_limit_lower position_limit_upper = self.robot.position_limit_upper for idx, val in enumerate(q): lo, hi = position_limit_lower[idx], position_limit_upper[idx] if hi < val or val < lo: q[idx] = 0.5 * (lo + hi) - - # Return rigid/flexible configuration - if self.simulator.use_theoretical_model: - return q[self.robot.rigid_joint_position_indices] return q def _sample_state(self) -> Tuple[np.ndarray, np.ndarray]: @@ -1406,7 +1407,7 @@ def _sample_state(self) -> Tuple[np.ndarray, np.ndarray]: # Make sure the robot impacts the ground if self.robot.has_freeflyer: - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() ground_fun = engine_options['world']['groundProfile'] compute_freeflyer_state_from_fixed_body( self.robot, q, ground_profile=ground_fun) @@ -1421,7 +1422,7 @@ def _initialize_buffers(self) -> None: avoid redundant computations. .. note:: - This method is called at `reset`, right after + This method is called at every `reset`, right after `self.simulator.start`. At this point, the simulation is running but `refresh_observation` has never been called, so that it can be used to initialize buffers involving the engine state but required @@ -1497,13 +1498,13 @@ def refresh_observation(self, measurement: EngineObsType) -> None: for sensor_type in self._sensors_types: array_copyto(sensors_out[sensor_type], sensors_in[sensor_type]) - def compute_command(self, action: ActT) -> np.ndarray: + def compute_command(self, action: ActT, command: np.ndarray) -> None: """Compute the motors efforts to apply on the robot. - By default, it is forward the input action as is, without performing - any processing. One is responsible of overloading this method if the - action space has been customized, or just to clip the action to make - sure it is never out-of-bounds if necessary. + By default, all it does is forwarding the input action as is, without + performing any processing. One is responsible of overloading this + method if the action space has been customized, or just to clip the + action to make sure it is never out-of-bounds if necessary. .. warning:: There is not good place to initialize buffers that are necessary to @@ -1518,7 +1519,7 @@ def compute_command(self, action: ActT) -> np.ndarray: LOGGER.warning("The action is out-of-bounds.") assert isinstance(action, np.ndarray) - return action + array_copyto(command, action) def has_terminated(self) -> Tuple[bool, bool]: """Determine whether the episode is over, because a terminal state of diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py index d3d8323d1..2af385360 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py @@ -1,6 +1,8 @@ """Generic environment to learn locomotion skills for legged robots using Jiminy simulator as physics engine. """ +import os +import pathlib from typing import Optional, Dict, Union, Any, Type, Sequence, Tuple import numpy as np @@ -13,6 +15,7 @@ ImuSensor as imu, PeriodicGaussianProcess, Robot) +from jiminy_py.robot import BaseJiminyRobot from jiminy_py.simulator import Simulator import pinocchio as pin @@ -179,12 +182,27 @@ def __init__(self, debug=debug, viewer_kwargs=viewer_kwargs, **{**dict( - has_freeflyer=True, - use_theoretical_model=False), + has_freeflyer=True), **kwargs}) else: - # Instantiate a simulator and load the options + # Instantiate a simulator simulator = Simulator(robot, viewer_kwargs=viewer_kwargs, **kwargs) + + # Load engine and robot options + if config_path is None: + if isinstance(robot, BaseJiminyRobot): + urdf_path = ( + robot._urdf_path_orig) # type: ignore[attr-defined] + else: + urdf_path = robot.urdf_path + if not urdf_path: + raise ValueError( + "'config_path' must be provided if the robot is not " + "associated with any URDF.") + config_path = str(pathlib.Path( + urdf_path).with_suffix('')) + '_options.toml' + if not os.path.exists(config_path): + config_path = "" simulator.import_options(config_path) # Initialize base class @@ -229,7 +247,7 @@ def _setup(self) -> None: # Get the options of robot and engine robot_options = self.robot.get_options() - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() # Make sure to log at least the required data for terminal reward # computation and log replay. @@ -272,7 +290,7 @@ def _setup(self) -> None: # Randomize the flexibility parameters if 'model' in self.std_ratio.keys(): - if self.robot.is_flexible: + if self.robot.is_flexibility_enabled: dynamics_options = robot_options["model"]["dynamics"] for flexibility in dynamics_options["flexibilityConfig"]: flexibility['stiffness'] += FLEX_STIFFNESS_SCALE * sample( @@ -316,7 +334,7 @@ def _setup(self) -> None: # Set the options, finally self.robot.set_options(robot_options) - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def _force_external_profile(self, t: float, @@ -406,8 +424,8 @@ def compute_reward(self, # Y-axis. It is equal to 0.0 if the frontal displacement is # perfectly symmetric wrt Y-axis over the whole trajectory. if 'direction' in reward_mixture_keys: - frontal_displacement = abs(np.mean(self.log_data[ - 'HighLevelController.currentFreeflyerPositionTransY'])) + frontal_displacement = abs(np.mean( + self.log_data['currentFreeflyerPositionTransY'])) reward_dict['direction'] = - frontal_displacement # Compute the total reward diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py new file mode 100644 index 000000000..17bef6054 --- /dev/null +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py @@ -0,0 +1,15 @@ +# pylint: disable=missing-module-docstring + +from .manager import QuantityManager +from .generic import (AverageSpatialVelocityFrame, + EulerAnglesFrame) +from .locomotion import CenterOfMass, ZeroMomentPoint + + +__all__ = [ + 'QuantityManager', + 'AverageSpatialVelocityFrame', + 'EulerAnglesFrame', + 'CenterOfMass', + 'ZeroMomentPoint', +] diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py new file mode 100644 index 000000000..34b371613 --- /dev/null +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py @@ -0,0 +1,276 @@ +"""Generic quantities that may be relevant for any kind of robot, regardless +its topology (multiple or single branch, fixed or floating base...) and the +application (locomotion, grasping...). +""" +from functools import partial +from dataclasses import dataclass +from typing import List, Dict, Set, Optional + +import numpy as np + +from jiminy_py.core import ( # pylint: disable=no-name-in-module + array_copyto, multi_array_copyto) +import pinocchio as pin + +from ..bases import InterfaceJiminyEnv, AbstractQuantity +from ..utils import fill, transforms_to_vector, matrix_to_rpy + + +@dataclass(unsafe_hash=True) +class AverageSpatialVelocityFrame(AbstractQuantity[np.ndarray]): + """Average spatial velocity of a given frame at the end of an agent step. + + The average spatial velocity is obtained by finite difference. More + precisely, it is defined here as the ratio of the geodesic distance in SE3 + Lie group between the pose of the frame at the end of previous and current + step over the time difference between them. Notably, under this definition, + the linear average velocity jointly depends on rate of change of the + translation and rotation of the frame, which may be undesirable in some + cases. Alternatively, the double geodesic distance could be used instead to + completely decouple the translation from the rotation. + """ + + frame_name: str + """Name of the frame on which to operate. + """ + + reference_frame: pin.ReferenceFrame + """Whether the spatial velocity must be computed in local reference frame + or re-aligned with world axes. + """ + + def __init__(self, + env: InterfaceJiminyEnv, + parent: Optional[AbstractQuantity], + frame_name: str, + reference_frame: pin.ReferenceFrame = pin.LOCAL + ) -> None: + """ + :param env: Base or wrapped jiminy environment. + :param parent: Higher-level quantity from which this quantity is a + requirement if any, `None` otherwise. + :param frame_name: Name of the frame on which to operate. + :param reference_frame: + Whether the spatial velocity must be computed in local reference + frame (aka 'pin.LOCAL') or re-aligned with world axes (aka + 'pin.LOCAL_WORLD_ALIGNED'). + """ + # Make sure at requested reference frame is supported + if reference_frame not in (pin.LOCAL, pin.LOCAL_WORLD_ALIGNED): + raise ValueError("Reference frame must be either 'pin.LOCAL' or " + "'pin.LOCAL_WORLD_ALIGNED'.") + + # Backup some user argument(s) + self.frame_name = frame_name + self.reference_frame = reference_frame + + # Call base implementation + super().__init__(env, parent, requirements={}) + + # Define specialize difference operator on SE3 Lie group + self._se3_diff = partial( + pin.LieGroup.difference, + pin.liegroups.SE3()) # pylint: disable=no-member + + # Inverse step size + self._inv_step_dt = 0.0 + + # Pre-allocate memory to store current and previous frame pose vector + self._xyzquat_prev, self._xyzquat = ( + np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) for _ in range(2)) + + # Define proxy to the current frame pose (translation, rotation matrix) + self._pose = (np.zeros(3), np.eye(3)) + + # Pre-allocate memory for the spatial velocity + self._v_spatial: np.ndarray = np.zeros(6) + + # Reshape linear plus angular velocity vector to vectorize rotation + self._v_lin_ang = np.reshape(self._v_spatial, (2, 3)).T + + def initialize(self) -> None: + # Call base implementation + super().initialize() + + # Compute inverse step size + self._inv_step_dt = 1.0 / self.env.step_dt + + # Refresh proxy to current frame pose + frame_index = self.pinocchio_model.getFrameId(self.frame_name) + transform = self.pinocchio_data.oMf[frame_index] + self._pose = (transform.translation, transform.rotation) + + # Re-initialize pre-allocated buffers + transforms_to_vector((self._pose,), self._xyzquat) + array_copyto(self._xyzquat_prev, self._xyzquat) + fill(self._v_spatial, 0) + + def refresh(self) -> np.ndarray: + # Convert current transform to (XYZ, Quat) convention + transforms_to_vector((self._pose,), self._xyzquat) + + # Compute average frame velocity in local frame since previous step + self._v_spatial[:] = self._se3_diff(self._xyzquat_prev, self._xyzquat) + self._v_spatial *= self._inv_step_dt + + # Translate local velocity to world frame + if self.reference_frame == pin.LOCAL_WORLD_ALIGNED: + # TODO: x2 speedup can be expected using `np.dot` with `nb.jit` + _, rot_mat = self._pose + self._v_lin_ang[:] = rot_mat @ self._v_lin_ang + + # Backup current frame pose + array_copyto(self._xyzquat_prev, self._xyzquat) + + return self._v_spatial + + +@dataclass(unsafe_hash=True) +class _BatchEulerAnglesFrame(AbstractQuantity[Dict[str, np.ndarray]]): + """Euler angles (Roll-Pitch-Yaw) representation of the orientation of all + frames involved in quantities relying on it and are active since last reset + of computation tracking if shared cache is available, its parent otherwise. + + This quantity only provides a performance benefit when managed by some + `QuantityManager`. It is not supposed to be instantiated manually but use + internally by `EulerAnglesFrame` as a requirement for vectorization of + computations for all frames at once. + + The orientation of all frames is exposed to the user as a dictionary whose + keys are the individual frame names. Internally, data are stored in batched + 2D contiguous array for efficiency. The first dimension are the 3 Euler + angles (roll, pitch, yaw) components, while the second one are individual + frames with the same ordering as 'self.frame_names'. + + .. note:: + This quantity does not allow for specifying frames directly. There is + no way to get the orientation of multiple frames at once for now. + """ + def __init__(self, + env: InterfaceJiminyEnv, + parent: Optional[AbstractQuantity]) -> None: + """ + :param env: Base or wrapped jiminy environment. + :param parent: Higher-level quantity from which this quantity is a + requirement if any, `None` otherwise. + :param frame_name: Name of the frame on which to operate. + """ + # Call base implementation + super().__init__(env, parent, requirements={}) + + # Initialize the ordered list of frame names + self.frame_names: Set[str] = set() + + # Store all rotation matrices at once + self._rot_mat_batch: np.ndarray = np.array([]) + + # Define proxy for slices of the batch storing all rotation matrices + self._rot_mat_views: List[np.ndarray] = [] + + # Define proxy for the rotation matrices of all frames + self._rot_mat_list: List[np.ndarray] = [] + + # Store Roll-Pitch-Yaw of all frames at once + self._rpy_batch: np.ndarray = np.array([]) + + # Mapping from frame name to individual Roll-Pitch-Yaw slices + self._rpy_map: Dict[str, np.ndarray] = {} + + def initialize(self) -> None: + # Call base implementation + super().initialize() + + # Update the list of frame names based on its cache owner list. + # Note that only active quantities are shared for efficiency. The list + # of active quantity may change dynamically. Re-initializing the class + # to take into account changes of the active set must be decided by + # `EulerAnglesFrame`. + assert isinstance(self.parent, EulerAnglesFrame) + self.frame_names = {self.parent.frame_name} + if self.cache: + for owner in self.cache.owners: + # We only consider active instances of `_BatchEulerAnglesFrame` + # instead of their corresponding parent `EulerAnglesFrame`. + # This is necessary because a derived quantity may feature + # `_BatchEulerAnglesFrame` as a requirement without actually + # relying on it depending on whether it is part of the optimal + # computation path at the time being or not. + if owner.is_active(any_cache_owner=False): + assert isinstance(owner.parent, EulerAnglesFrame) + self.frame_names.add(owner.parent.frame_name) + + # Re-allocate memory as the number of frames is not known in advance. + # Note that Fortran memory layout (column-major) is used for speed up + # because it preserves contiguity when copying frame data. + nframes = len(self.frame_names) + self._rot_mat_batch = np.zeros((3, 3, nframes), order='F') + self._rpy_batch = np.zeros((3, nframes)) + + # Refresh proxies + self._rot_mat_views.clear() + self._rot_mat_list.clear() + for i, frame_name in enumerate(self.frame_names): + frame_index = self.pinocchio_model.getFrameId(frame_name) + rot_matrix = self.pinocchio_data.oMf[frame_index].rotation + self._rot_mat_views.append(self._rot_mat_batch[..., i]) + self._rot_mat_list.append(rot_matrix) + + # Re-assign mapping from frame name to their corresponding data + self._rpy_map = dict(zip(self.frame_names, self._rpy_batch.T)) + + def refresh(self) -> Dict[str, np.ndarray]: + # Copy all rotation matrices in contiguous buffer + multi_array_copyto(self._rot_mat_views, self._rot_mat_list) + + # Convert all rotation matrices at once to Roll-Pitch-Yaw + matrix_to_rpy(self._rot_mat_batch, self._rpy_batch) + + # Return proxy directly without copy + return self._rpy_map + + +@dataclass(unsafe_hash=True) +class EulerAnglesFrame(AbstractQuantity[np.ndarray]): + """Euler angles (Roll-Pitch-Yaw) representation of the orientation of a + given frame in world reference frame at the end of an agent step. + """ + + frame_name: str + """Name of the frame on which to operate. + """ + + def __init__(self, + env: InterfaceJiminyEnv, + parent: Optional[AbstractQuantity], + frame_name: str, + ) -> None: + """ + :param env: Base or wrapped jiminy environment. + :param parent: Higher-level quantity from which this quantity is a + requirement if any, `None` otherwise. + :param frame_name: Name of the frame on which to operate. + """ + # Backup some user argument(s) + self.frame_name = frame_name + + # Call base implementation + super().__init__( + env, parent, requirements={"data": (_BatchEulerAnglesFrame, {})}) + + def initialize(self) -> None: + # Check if the quantity is already active + was_active = self._is_active + + # Call base implementation. + # The quantity is now considered active at this point. + super().initialize() + + # Force re-initializing shared data if the active set has changed + if not was_active: + self.requirements["data"].reset() + + def refresh(self) -> np.ndarray: + # Return a slice of batched data. + # Note that mapping from frame name to frame index in batched data + # cannot be pre-computed as it may changed dynamically. + return self.data[self.frame_name] diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py new file mode 100644 index 000000000..5cf397166 --- /dev/null +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py @@ -0,0 +1,125 @@ +"""Quantities mainly relevant for locomotion tasks on floating-base robots. +""" +from typing import Optional, Tuple +from dataclasses import dataclass + +import numpy as np +import pinocchio as pin + +from ..bases import InterfaceJiminyEnv, AbstractQuantity +from ..utils import fill + + +@dataclass(unsafe_hash=True) +class CenterOfMass(AbstractQuantity[np.ndarray]): + """Position, Velocity or Acceleration of the center of mass of the robot as + a whole. + """ + + kinematic_level: pin.KinematicLevel + """Kinematic level to compute, ie position, velocity or acceleration. + """ + + def __init__( + self, + env: InterfaceJiminyEnv, + parent: Optional[AbstractQuantity], + kinematic_level: pin.KinematicLevel = pin.POSITION + ) -> None: + """ + :param env: Base or wrapped jiminy environment. + :param parent: Higher-level quantity from which this quantity is a + requirement if any, `None` otherwise. + :para kinematic_level: Desired kinematic level, ie position, velocity + or acceleration. + """ + # Backup some user argument(s) + self.kinematic_level = kinematic_level + + # Call base implementation + super().__init__(env, parent, requirements={}) + + # Pre-allocate memory for the CoM quantity + self._com_data: np.ndarray = np.array([]) + + def initialize(self) -> None: + # Call base implementation + super().initialize() + + # Refresh CoM quantity proxy based on kinematic level + if self.kinematic_level == pin.POSITION: + self._com_data = self.pinocchio_data.com[0] + elif self.kinematic_level == pin.VELOCITY: + self._com_data = self.pinocchio_data.vcom[0] + else: + self._com_data = self.pinocchio_data.acom[0] + + def refresh(self) -> np.ndarray: + # Jiminy does not compute the CoM acceleration automatically + if self.kinematic_level == pin.ACCELERATION: + pin.centerOfMass(self.pinocchio_model, + self.pinocchio_data, + self.kinematic_level) + + # Return proxy directly without copy + return self._com_data + + +@dataclass(unsafe_hash=True) +class ZeroMomentPoint(AbstractQuantity[np.ndarray]): + """Zero Moment Point (ZMP), also called Divergent Component of Motion + (DCM). + + This quantity only makes sense for legged robots whose the inverted + pendulum is a relevant approximate dynamic model. It is involved in various + dynamic stability metrics (usually only on flat ground), such as N-steps + capturability. More precisely, the robot will keep balance if the ZMP is + maintained inside the support polygon. + """ + def __init__(self, + env: InterfaceJiminyEnv, + parent: Optional[AbstractQuantity]) -> None: + """ + :param env: Base or wrapped jiminy environment. + """ + # Call base implementation + super().__init__(env, parent, requirements={"com": (CenterOfMass, {})}) + + # Weight of the robot + self._robot_weight: float = -1 + + # Proxy for the derivative of the spatial centroidal momentum + self.dhg: Tuple[np.ndarray, np.ndarray] = (np.ndarray([]),) * 2 + + # Pre-allocate memory for the ZMP + self._zmp = np.zeros(2) + + def initialize(self) -> None: + # Call base implementation + super().initialize() + + # Compute the weight of the robot + gravity = abs(self.pinocchio_model.gravity.linear[2]) + robot_mass = self.pinocchio_data.mass[0] + self._robot_weight = robot_mass * gravity + + # Refresh derivative of the spatial centroidal momentum + self.dhg = ((dhg := self.pinocchio_data.dhg).linear, dhg.angular) + + # Re-initialized pre-allocated memory buffer + fill(self._zmp, 0) + + def refresh(self) -> np.ndarray: + # Extract intermediary quantities for convenience + (dhg_linear, dhg_angular), com = self.dhg, self.com + + # Compute the vertical force applied by the robot + f_z = dhg_linear[2] + self._robot_weight + + # Compute the center of pressure + self._zmp[:] = com[:2] * (self._robot_weight / f_z) + if abs(f_z) > np.finfo(np.float32).eps: + self._zmp[0] -= (dhg_angular[1] + dhg_linear[0] * com[2]) / f_z + self._zmp[1] += (dhg_angular[0] - dhg_linear[1] * com[2]) / f_z + + return self._zmp diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/manager.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/manager.py new file mode 100644 index 000000000..44b14a10d --- /dev/null +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/manager.py @@ -0,0 +1,164 @@ +"""This module provides a dedicated quantity manager. Although not necessary +for computing quantities, its usage is strongly recommended for optimal +performance. + +It is responsible for optimizing the computation path, which is expected to +significantly increase the step collection throughput. This speedup is achieved +by caching already computed that did not changed since then, computing +redundant intermediary quantities only once per step, and gathering similar +quantities in a large batch to leverage vectorization of math instructions. +""" +from collections.abc import MutableMapping +from typing import Any, Dict, List, Tuple, Iterator, Type + +from ..bases import InterfaceJiminyEnv, AbstractQuantity, SharedCache + + +QuantityCreator = Tuple[Type[AbstractQuantity], Dict[str, Any]] + + +class QuantityManager(MutableMapping): + """This class centralizes the evaluation of all quantities involved in + reward or termination conditions evaluation to redundant and unnecessary + computations. + + It is responsible for making sure all quantities are evaluated on the same + environment, and internal buffers are re-initialized whenever necessary. + + .. note:: + Individual quantities can be accessed either as instance properties or + items of a dictionary. Choosing one or the other is only a matter of + taste since both options have been heavily optimized to minimize + overhead and should be equally efficient. + """ + def __init__(self, env: InterfaceJiminyEnv) -> None: + """ + :param env: Base or wrapped jiminy environment. + """ + # Backup user argument(s) + self.env = env + + # List of already instantiated quantities to manager + self._quantities: Dict[str, AbstractQuantity] = {} + + # Initialize shared caches for all managed quantities. + # Note that keys are not quantities directly but pairs (class, hash). + # This is necessary because using a quantity as key directly would + # prevent its garbage collection and break automatic reset of + # computation tracking for all quantities sharing its cache. + self._caches: Dict[ + Tuple[Type[AbstractQuantity], int], SharedCache] = {} + + def reset(self, reset_tracking: bool = False) -> None: + """Consider that all managed quantity must be re-initialized before + being able to evaluate them once again. + + .. note:: + The cache is cleared automatically by the quantities themselves. + + :param reset_tracking: Do not consider any quantity as active anymore. + Optional: False by default. + """ + for quantity in self._quantities.values(): + quantity.reset(reset_tracking) + + def clear(self) -> None: + """Clear internal cache of quantities to force re-evaluating them the + next time their value is fetched. + + .. note:: + This method is supposed to be called every time the state of the + environment has changed (ie either the agent or world itself), + thereby invalidating the value currently stored in cache if any. + """ + for cache in self._caches.values(): + cache.reset() + + def __getattr__(self, name: str) -> Any: + """Get access managed quantities as first-class properties, rather than + dictionary-like values through `__getitem__`. + + .. warning:: + Getting quantities this way is convenient but unfortunately much + slower than doing it through `__getitem__`. It takes 40ns on + Python 3.12 and a whooping 180ns on Python 3.11. As a result, this + approach is mainly intended for ease of use while prototyping and + is not recommended in production, especially on Python<3.12. + + :param name: Name of the requested quantity. + """ + return self[name] + + def __dir__(self) -> List[str]: + """Attribute lookup. + + It is mainly used by autocomplete feature of Ipython. It is overloaded + to get consistent autocompletion wrt `getattr`. + """ + return [*super().__dir__(), *self._quantities.keys()] + + def __setitem__(self, + name: str, + quantity_creator: QuantityCreator) -> None: + """Instantiate a new top-level quantity to manage for now on. + + :param name: Desired name of the quantity after instantiation. It will + raise an exception if another quantity with the exact same + name exists. + :param quantity_creator: Tuple gathering the class of the new quantity + to manage plus its keyword-arguments except + the environment 'env' as a dictionary. + """ + # Make sure that no quantity with the same name is already managed to + # avoid silently overriding quantities being managed in user's back. + if name in self._quantities: + raise KeyError( + "A quantity with the exact same name already exists. Please " + "delete it first before adding a new one.") + + # Instantiate the new quantity + quantity_cls, quantity_kwargs = quantity_creator + top_quantity = quantity_cls(self.env, None, **(quantity_kwargs or {})) + + # Set a shared cache entry for all quantities involved in computations + quantities_all = [top_quantity] + while quantities_all: + quantity = quantities_all.pop() + key = (type(quantity), hash(quantity)) + quantity.cache = self._caches.setdefault(key, SharedCache()) + quantities_all += quantity.requirements.values() + + # Add it to the map of already managed quantities + self._quantities[name] = top_quantity + + def __getitem__(self, name: str) -> Any: + """Get the evaluated value of a given quantity. + + :param name: Name of the quantity for which to fetch the current value. + """ + return self._quantities[name].get() + + def __delitem__(self, name: str) -> None: + """Stop managing a quantity that is no longer relevant. + + .. warning:: + Deleting managed quantities modifies the computation graph, which + would affect quantities that detect the optimal computation path + dynamically. Computation tracking of all owners of a shared cache + will be reset at garbage collection by the cache itself to get the + opportunity to recover optimality. + + :param name: Name of the managed quantity to be discarded. It will + raise an exception if the specified name does not exists. + """ + del self._quantities[name] + + def __iter__(self) -> Iterator[str]: + """Iterate over names of managed quantities. + """ + return iter(self._quantities) + + def __len__(self) -> int: + """Number of quantities being managed. + """ + return len(self._quantities) diff --git a/python/gym_jiminy/common/gym_jiminy/common/utils/__init__.py b/python/gym_jiminy/common/gym_jiminy/common/utils/__init__.py index 3d36ea073..c6c42a02e 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/utils/__init__.py +++ b/python/gym_jiminy/common/gym_jiminy/common/utils/__init__.py @@ -13,6 +13,7 @@ quat_average, rpy_to_matrix, rpy_to_quat, + transforms_to_vector, compute_tilt_from_quat, swing_from_vector, remove_twist_from_quat) @@ -55,6 +56,7 @@ 'quat_average', 'rpy_to_matrix', 'rpy_to_quat', + 'transforms_to_vector', 'compute_tilt_from_quat', 'swing_from_vector', 'remove_twist_from_quat', diff --git a/python/gym_jiminy/common/gym_jiminy/common/utils/math.py b/python/gym_jiminy/common/gym_jiminy/common/utils/math.py index a3aa2cdd5..9721cd314 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/utils/math.py +++ b/python/gym_jiminy/common/gym_jiminy/common/utils/math.py @@ -24,7 +24,9 @@ def squared_norm_2(array: np.ndarray) -> float: @nb.jit(nopython=True, cache=True) -def matrix_to_yaw(mat: np.ndarray) -> float: +def matrix_to_yaw(mat: np.ndarray, + out: Optional[np.ndarray] = None + ) -> Union[float, np.ndarray]: """Compute the yaw from Yaw-Pitch-Roll Euler angles representation of a rotation matrix in 3D Euclidean space. @@ -32,7 +34,17 @@ def matrix_to_yaw(mat: np.ndarray) -> float: the 3-by-3 rotation matrix elements. """ assert mat.ndim >= 2 - return np.arctan2(mat[1, 0], mat[0, 0]) + + # Allocate memory for the output array + if out is None: + out_ = np.empty(mat.shape[2:]) + else: + assert out.shape == mat.shape[2:] + out_ = out + + out_[:] = np.arctan2(mat[1, 0], mat[0, 0]) + + return out_ @nb.jit(nopython=True, cache=True, inline='always') @@ -215,6 +227,45 @@ def matrices_to_quat(mat_list: Tuple[np.ndarray, ...], return out_ +@nb.jit(nopython=True, cache=True) +def transforms_to_vector( + transform_list: Tuple[Tuple[np.ndarray, np.ndarray], ...], + out: Optional[np.ndarray] = None) -> np.ndarray: + """Stack the translation vector [x, y, z] and the quaternion representation + [qx, qy, qz, qw] of the orientation of multiple transform tuples. + + .. note:: + Internally, it copies the translation unaffected and convert rotation + matrices to quaternions using `matrices_to_quat`. + + :param transform_list: Tuple of N transforms, each of which represented as + pairs gathering the translation as a vector and the + orientation as a 3D rotation matrix. + :param out: A pre-allocated array into which the result is stored. If not + provided, a new array is freshly-allocated, which is slower. + """ + # Allocate memory if necessart + if out is None: + out_ = np.empty((7, len(transform_list))) + else: + out2d = out[:, np.newaxis] if out.ndim == 1 else out + assert out2d.shape == (7, len(transform_list)) + out_ = out2d + + # Simply copy the translation + for i, (translation, _) in enumerate(transform_list): + out_[:3, i] = translation + + # Convert all rotation matrices to quaternions at once + rotation_list = [rotation for _, rotation in transform_list] + matrices_to_quat(rotation_list, out_[-4:]) + + # Revel extra dimension before returning if not present initially + if out is not None and out.ndim == 1: + return out_[:, 0] + return out_ + + @nb.jit(nopython=True, cache=True) def rpy_to_matrix(rpy: np.ndarray, out: Optional[np.ndarray] = None) -> np.ndarray: diff --git a/python/gym_jiminy/common/gym_jiminy/common/utils/misc.py b/python/gym_jiminy/common/gym_jiminy/common/utils/misc.py index 7533b5d26..90d722634 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/utils/misc.py +++ b/python/gym_jiminy/common/gym_jiminy/common/utils/misc.py @@ -26,8 +26,8 @@ class RandomDistribution(Protocol): - """Protocol to be satisfied when passing generic statistical distribution - callable to `sample` method. + """Protocol that must be satisfied for passing a generic callable as + custom statistical distribution to `sample` method. """ def __call__(self, rg: np.random.Generator, *args: Any, **kwargs: Any ) -> ArrayOrScalar: @@ -95,7 +95,7 @@ def get_fieldnames(structure: Union[gym.Space[DataNested], DataNested], else: # Tensor: basic numbering fieldname = np.array([ - ".".join(map(str, filter(None, (*fieldname_path, i)))) + ".".join(map(str, (*filter(None, fieldname_path), i))) for i in range(data.size)]).reshape(data.shape).tolist() fieldnames.append(fieldname) diff --git a/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py b/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py index 938e0e596..c85dde065 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py +++ b/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py @@ -250,7 +250,7 @@ def refresh_observation(self, measurement: EngineObsType) -> None: self.__n_last_stack = -1 self.wrapper.refresh_observation(self.env.observation) - def compute_command(self, action: ActT) -> np.ndarray: + def compute_command(self, action: ActT, command: np.ndarray) -> None: """Compute the motors efforts to apply on the robot. It simply forwards the command computed by the wrapped environment @@ -258,4 +258,4 @@ def compute_command(self, action: ActT) -> np.ndarray: :param action: High-level target to achieve by means of the command. """ - return self.env.compute_command(action) + self.env.compute_command(action, command) diff --git a/python/gym_jiminy/common/setup.py b/python/gym_jiminy/common/setup.py index e5e593466..0f1dce609 100644 --- a/python/gym_jiminy/common/setup.py +++ b/python/gym_jiminy/common/setup.py @@ -58,7 +58,7 @@ # - `gym` has been replaced by `gymnasium` for 0.26.0+ # - 0.28.0: fully typed # - bound version for resilience to recurrent API breakage - "gymnasium>=0.26,<0.29", + "gymnasium>=0.28,<1.0", # For backward compatibility of latest Python typing features # - TypeAlias has been added with Python 3.10 "typing_extensions" diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/acrobot.py b/python/gym_jiminy/envs/gym_jiminy/envs/acrobot.py index eef3f9449..6b4c3565c 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/acrobot.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/acrobot.py @@ -150,10 +150,10 @@ def _setup(self) -> None: # Enforce fixed-timestep integrator. # It ensures calling 'step' always takes the same amount of time. - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options["stepper"]["odeSolver"] = "runge_kutta_4" engine_options["stepper"]["dtMax"] = CONTROL_DT - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def _initialize_observation_space(self) -> None: """Configure the observation of the environment. @@ -222,7 +222,7 @@ def has_terminated(self) -> Tuple[bool, bool]: return terminated, truncated - def compute_command(self, action: np.ndarray) -> np.ndarray: + def compute_command(self, action: np.ndarray, command: np.ndarray) -> None: """Compute the motors efforts to apply on the robot. Convert a discrete action into its actual value if necessary, then add @@ -230,11 +230,9 @@ def compute_command(self, action: np.ndarray) -> np.ndarray: :param action: Desired motors efforts. """ - if not self.continuous: - action = self.AVAIL_CTRL[action] + command[:] = action if self.continuous else self.AVAIL_CTRL[action] if ACTION_NOISE > 0.0: - action += sample(scale=ACTION_NOISE, rg=self.np_random) - return action + command += sample(scale=ACTION_NOISE, rg=self.np_random) def compute_reward(self, terminated: bool, diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/ant.py b/python/gym_jiminy/envs/gym_jiminy/envs/ant.py index 461c52515..54d37b7aa 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/ant.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/ant.py @@ -44,8 +44,7 @@ def __init__(self, debug: bool = False, **kwargs: Any) -> None: # Configure the backend simulator simulator = Simulator.build( urdf_path, hardware_path, data_dir, - has_freeflyer=True, use_theoretical_model=False, - config_path=config_path, debug=debug, **kwargs) + has_freeflyer=True, config_path=config_path, debug=debug, **kwargs) # Get the list of independent bodies (not connected via fixed joint) self.body_indices = [0] # World is part of bodies list @@ -63,13 +62,13 @@ def __init__(self, debug: bool = False, **kwargs: Any) -> None: # Note that they will be initialized in `_initialize_buffers`. self._obs_slices: Tuple[np.ndarray, ...] = () - # Define base orientation and external forces proxies for fast access. + # Define base orientation and external forces proxies for fast access self._base_rot = np.array([]) self._f_external: Tuple[np.ndarray, ...] = () - # Rigid configuration and velocity of the robot. - self._q_rigid = np.array([]) - self._v_rigid = np.array([]) + # Theoretical state of the robot + self._q_th = np.array([]) + self._v_th = np.array([]) # Initialize base class super().__init__( @@ -132,8 +131,8 @@ def _initialize_observation_space(self) -> None: The observation space comprises: - * rigid configuration (absolute position (x, y) excluded), - * rigid velocity (with base linear velocity in world frame), + * theoretical configuration (absolute position (x, y) excluded), + * theoretical velocity (with base linear velocity in world frame), * flattened external forces applied on each body in local frame, ie centered at their respective center of mass. """ @@ -159,7 +158,7 @@ def _initialize_observation_space(self) -> None: def _initialize_buffers(self) -> None: # Extract observation from the robot state. # Note that this is only reliable with using a fixed step integrator. - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() if engine_options['stepper']['odeSolver'] in ('runge_kutta_dopri5',): raise ValueError( "This environment does not support adaptive step integrators. " @@ -171,7 +170,7 @@ def _initialize_buffers(self) -> None: # Initialize vector of external forces self._f_external = tuple( self.robot_state.f_external[joint_index].vector - for joint_index in self.robot.rigid_joint_indices) + for joint_index in self.robot.mechanical_joint_indices) # Refresh buffers manually to initialize them early self._refresh_buffers() @@ -182,9 +181,9 @@ def _initialize_buffers(self) -> None: obs_slices = [] obs_index_first = 0 for data in ( - self._q_rigid[2:], - self._v_rigid[:3], - self._v_rigid[3:], + self._q_th[2:], + self._v_th[:3], + self._v_th[3:], *self._f_external): obs_index_last = obs_index_first + len(data) obs_slices.append(self.observation[obs_index_first:obs_index_last]) @@ -195,15 +194,15 @@ def _initialize_buffers(self) -> None: self._xpos_prev = self._robot_state_q[0] def _refresh_buffers(self) -> None: - self._q_rigid = self.robot.get_rigid_position_from_flexible( + self._q_th = self.robot.get_theoretical_position_from_extended( self._robot_state_q) - self._v_rigid = self.robot.get_rigid_velocity_from_flexible( + self._v_th = self.robot.get_theoretical_velocity_from_extended( self._robot_state_v) def refresh_observation(self, measurement: EngineObsType) -> None: # Update observation copyto(self._obs_slices[:-1], ( - self._q_rigid[2:], self._v_rigid[3:], *self._f_external)) + self._q_th[2:], self._v_th[3:], *self._f_external)) # Transform observed linear velocity to be in world frame self._obs_slices[-1][:] = self._base_rot @ self._robot_state_v[:3] diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/anymal.py b/python/gym_jiminy/envs/gym_jiminy/envs/anymal.py index fa361ccd8..dda5ef4eb 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/anymal.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/anymal.py @@ -5,7 +5,7 @@ from typing import Any from gym_jiminy.common.envs import WalkerJiminyEnv -from gym_jiminy.common.blocks import PDController, MahonyFilter +from gym_jiminy.common.blocks import PDController, PDAdapter, MahonyFilter from gym_jiminy.common.utils import build_pipeline if sys.version_info < (3, 9): @@ -87,11 +87,24 @@ def __init__(self, debug: bool = False, **kwargs: Any) -> None: cls=PDController, kwargs=dict( update_ratio=HLC_TO_LLC_RATIO, - order=1, kp=PD_KP, kd=PD_KD, target_position_margin=0.0, - target_velocity_limit=float("inf") + target_velocity_limit=float("inf"), + target_acceleration_limit=float("inf") + ) + ), + wrapper=dict( + kwargs=dict( + augment_observation=False + ) + ) + ), dict( + block=dict( + cls=PDAdapter, + kwargs=dict( + update_ratio=-1, + order=1, ) ), wrapper=dict( diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/atlas.py b/python/gym_jiminy/envs/gym_jiminy/envs/atlas.py index d7d0aff58..99dfcd081 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/atlas.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/atlas.py @@ -13,6 +13,7 @@ from gym_jiminy.common.envs import WalkerJiminyEnv from gym_jiminy.common.blocks import (MotorSafetyLimit, PDController, + PDAdapter, MahonyFilter) from gym_jiminy.common.utils import build_pipeline from gym_jiminy.toolbox.math import ConvexHull @@ -29,15 +30,12 @@ # Default simulation duration (:float [s]) SIMULATION_DURATION = 20.0 -# Ratio between the High-level neural network PID target update and Low-level -# PID torque update (:int [NA]) -HLC_TO_LLC_RATIO = 1 - # Stepper update period (:float [s]) STEP_DT = 0.04 MOTOR_POSITION_MARGIN = 0.02 MOTOR_VELOCITY_MAX = 3.0 +MOTOR_ACCELERATION_MAX = 40.0 # PID proportional gains (one per actuated joint) PD_REDUCED_KP = ( @@ -255,12 +253,25 @@ def joint_position_index(joint_name: str) -> int: block=dict( cls=PDController, kwargs=dict( - update_ratio=HLC_TO_LLC_RATIO, - order=1, + update_ratio=1, kp=PD_FULL_KP, kd=PD_FULL_KD, target_position_margin=0.0, target_velocity_limit=MOTOR_VELOCITY_MAX, + target_acceleration_limit=MOTOR_ACCELERATION_MAX + ) + ), + wrapper=dict( + kwargs=dict( + augment_observation=False + ) + ) + ), dict( + block=dict( + cls=PDAdapter, + kwargs=dict( + update_ratio=-1, + order=1, ) ), wrapper=dict( @@ -293,19 +304,32 @@ def joint_position_index(joint_name: str) -> int: kp=1.0 / MOTOR_POSITION_MARGIN, kd=1.0 / MOTOR_VELOCITY_MAX, soft_position_margin=0.0, - soft_velocity_max=MOTOR_VELOCITY_MAX, + soft_velocity_max=MOTOR_VELOCITY_MAX ) ), ), dict( block=dict( cls=PDController, kwargs=dict( - update_ratio=HLC_TO_LLC_RATIO, - order=1, + update_ratio=1, kp=PD_REDUCED_KP, kd=PD_REDUCED_KD, target_position_margin=0.0, target_velocity_limit=MOTOR_VELOCITY_MAX, + target_acceleration_limit=MOTOR_ACCELERATION_MAX + ) + ), + wrapper=dict( + kwargs=dict( + augment_observation=False + ) + ) + ), dict( + block=dict( + cls=PDAdapter, + kwargs=dict( + update_ratio=-1, + order=1, ) ), wrapper=dict( diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py b/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py index 225c13c12..66dffee9f 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py @@ -155,10 +155,10 @@ def _setup(self) -> None: super()._setup() # OpenAI Gym implementation uses euler explicit integration scheme - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options["stepper"]["odeSolver"] = "euler_explicit" engine_options["stepper"]["dtMax"] = CONTROL_DT - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def _initialize_observation_space(self) -> None: """Configure the observation of the environment. @@ -207,16 +207,14 @@ def refresh_observation(self, measurement: EngineObsType) -> None: copyto(self.__state_view, measurement[ 'states']['agent'].values()) # type: ignore[index,union-attr] - def compute_command(self, action: np.ndarray) -> np.ndarray: + def compute_command(self, action: np.ndarray, command: np.ndarray) -> None: """Compute the motors efforts to apply on the robot. Convert a discrete action into its actual value if necessary. :param action: Desired motors efforts. """ - if not self.continuous: - action = self.AVAIL_CTRL[action] - return action + command[:] = action if self.continuous else self.AVAIL_CTRL[action] def compute_reward(self, terminated: bool, diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py b/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py index 4a6b6d164..b13c5572c 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py @@ -15,7 +15,7 @@ from pinocchio import neutral, SE3, buildReducedModel from gym_jiminy.common.envs import WalkerJiminyEnv -from gym_jiminy.common.blocks import PDController, MahonyFilter +from gym_jiminy.common.blocks import PDController, PDAdapter, MahonyFilter from gym_jiminy.common.utils import build_pipeline if sys.version_info < (3, 9): @@ -193,11 +193,24 @@ def set_joint_rotary_position(joint_name: str, theta: float) -> None: cls=PDController, kwargs=dict( update_ratio=HLC_TO_LLC_RATIO, - order=1, kp=PD_KP, kd=PD_KD, target_position_margin=0.0, - target_velocity_limit=float("inf") + target_velocity_limit=float("inf"), + target_acceleration_limit=float("inf") + ) + ), + wrapper=dict( + kwargs=dict( + augment_observation=False + ) + ) + ), dict( + block=dict( + cls=PDAdapter, + kwargs=dict( + update_ratio=-1, + order=1, ) ), wrapper=dict( diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/digit.py b/python/gym_jiminy/envs/gym_jiminy/envs/digit.py index ded255377..3a6c2895c 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/digit.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/digit.py @@ -15,7 +15,7 @@ from pinocchio import neutral, SE3, buildReducedModel from gym_jiminy.common.envs import WalkerJiminyEnv -from gym_jiminy.common.blocks import PDController, MahonyFilter +from gym_jiminy.common.blocks import PDController, PDAdapter, MahonyFilter from gym_jiminy.common.utils import build_pipeline if sys.version_info < (3, 9): @@ -208,11 +208,24 @@ def set_joint_rotary_position(joint_name: str, theta: float) -> None: cls=PDController, kwargs=dict( update_ratio=HLC_TO_LLC_RATIO, - order=1, kp=PD_KP, kd=PD_KD, target_position_margin=0.0, - target_velocity_limit=float("inf") + target_velocity_limit=float("inf"), + target_acceleration_limit=float("inf") + ) + ), + wrapper=dict( + kwargs=dict( + augment_observation=False + ) + ) + ), dict( + block=dict( + cls=PDAdapter, + kwargs=dict( + update_ratio=-1, + order=1, ) ), wrapper=dict( diff --git a/python/gym_jiminy/examples/quantity_benchmark.py b/python/gym_jiminy/examples/quantity_benchmark.py new file mode 100644 index 000000000..5c381d080 --- /dev/null +++ b/python/gym_jiminy/examples/quantity_benchmark.py @@ -0,0 +1,61 @@ +import timeit + +import numpy as np +import matplotlib.pyplot as plt +import gymnasium as gym + +import gym_jiminy.common.bases.quantity +from gym_jiminy.common.quantities import QuantityManager, EulerAnglesFrame + +# Define number of samples for benchmarking +N_SAMPLES = 50000 + +# Disable caching by forcing `SharedCache.has_value` to always return `False` +setattr(gym_jiminy.common.bases.quantity.SharedCache, + "has_value", + property(lambda self: False)) + +# Instantiate a dummy environment +env = gym.make("gym_jiminy.envs:atlas") +env.reset() +env.step(env.action) + +# Define quantity manager and add quantities to benchmark +nframes = len(env.robot.pinocchio_model.frames) +quantity_manager = QuantityManager( + env.simulator, + { + f"rpy_{i}": (EulerAnglesFrame, dict(frame_name=frame.name)) + for i, frame in enumerate(env.robot.pinocchio_model.frames) + }) + +# Run the benchmark for all batch size +time_per_frame_all = [] +for i in range(1, nframes): + # Reset tracking + quantity_manager.reset(reset_tracking=True) + + # Fetch all quantities once to update dynamic computation graph + for j, quantity in enumerate(quantity_manager._quantities.values()): + quantity.get() + if i == j + 1: + break + + # Extract batched data buffer of `EulerAnglesFrame` quantities + shared_data = quantity.requirements['data'] + + # Benchmark computation of batched data buffer + duration = timeit.timeit( + 'shared_data.get()', number=N_SAMPLES, globals={ + "shared_data": shared_data + }) / N_SAMPLES + time_per_frame_all.append(duration) + print(f"Computation time (ns) for {i} frames: {duration * 1e9}") + +# Plot the result if enough data is available +if len(time_per_frame_all) > 1: + plt.figure() + plt.plot(np.diff(time_per_frame_all) * 1e9) + plt.xlabel("Number of frames") + plt.ylabel("Average computation time per frame (ns)") + plt.show() diff --git a/python/gym_jiminy/examples/reinforcement_learning/rllib/ant_td3.py b/python/gym_jiminy/examples/reinforcement_learning/rllib/ant_td3.py deleted file mode 100644 index df1b507d4..000000000 --- a/python/gym_jiminy/examples/reinforcement_learning/rllib/ant_td3.py +++ /dev/null @@ -1,72 +0,0 @@ -"""Solve the official Open AI Gym Ant-v2 problem simulated in Jiminy using -TD3 algorithm of Ray RLlib reinforcement learning framework. - -It solves it consistently in less than 500000 timesteps in average, and in -about 300000 at best. - -.. warning:: - This script requires pytorch>=1.4 and ray[rllib]==1.2. -""" -# ======================== User parameters ========================= - -GYM_ENV_NAME = "gym_jiminy.envs:ant" -DEBUG = False -SEED = 0 -N_THREADS = 12 -N_GPU = 1 - -# =================== Configure Python workspace =================== - -# GPU device selection must be done at system level to be taken into account -__import__("os").environ["CUDA_VISIBLE_DEVICES"] = \ - ",".join(map(str, range(N_GPU))) - -import logging -import gymnasium as gym -import ray -from ray.tune.registry import register_env -import ray.rllib.agents.ddpg.td3 as td3 - -from gym_jiminy.rllib.utilities import initialize, train, test - -# Register learning environment -register_env("env", lambda env_config: gym.make(GYM_ENV_NAME, **env_config)) - -# ============= Initialize Ray and Tensorboard daemons ============= - -# Initialize Ray backend -logger_creator = initialize( - num_cpus=N_THREADS, num_gpus=N_GPU, debug=DEBUG) - -# ================== Configure learning algorithm ================== - -# General options -config = td3.TD3_DEFAULT_CONFIG.copy() -config["framework"] = "tf" -config["eager_tracing"] = True -config["log_level"] = logging.DEBUG if DEBUG else logging.ERROR -config["seed"] = SEED - -# Environment options -config["horizon"] = 1000 - -# TD3-specific options -config["learning_starts"] = 10000 -config["exploration_config"]["random_timesteps"] = 20000 - -# ====================== Run the optimization ====================== - -train_agent = td3.TD3Trainer(config, "env", logger_creator) -checkpoint_path = train(train_agent, max_timesteps=1000000) - -# ===================== Enjoy a trained agent ====================== - -test_agent = td3.TD3Trainer(config, "env") -test_agent.restore(checkpoint_path) -test(test_agent, explore=False) - -# =================== Terminate the Ray backend ==================== - -train_agent.stop() -test_agent.stop() -ray.shutdown() diff --git a/python/gym_jiminy/examples/reinforcement_learning/rllib/cartpole_ppo.py b/python/gym_jiminy/examples/reinforcement_learning/rllib/cartpole_ppo.py deleted file mode 100644 index 03c5f2a34..000000000 --- a/python/gym_jiminy/examples/reinforcement_learning/rllib/cartpole_ppo.py +++ /dev/null @@ -1,183 +0,0 @@ -"""Solve the official Open AI Gym Cartpole problem simulated in Jiminy using -PPO algorithm of Ray RLlib reinforcement learning framework. - -It solves it consistently in less than 100000 timesteps in average, and in -about 80000 at best. - -.. warning:: - This script requires pytorch>=1.4 and ray[rllib]==1.2. -""" -# flake8: noqa - -# ======================== User parameters ========================= - -GYM_ENV_NAME = "gym_jiminy.envs:cartpole" -GYM_ENV_KWARGS = { - 'continuous': True -} -DEBUG = False -SEED = 0 -N_THREADS = 8 -N_GPU = 1 - -# =================== Configure Python workspace =================== - -# GPU device selection must be done at system level to be taken into account -__import__("os").environ["CUDA_VISIBLE_DEVICES"] = \ - ",".join(map(str, range(N_GPU))) - -import logging -import gymnasium as gym -import ray -from ray.tune.registry import register_env -from ray.rllib.models import ModelCatalog, MODEL_DEFAULTS -from ray.rllib.agents.trainer import COMMON_CONFIG -from ray.rllib.agents.ppo import (PPOTrainer as Trainer, - DEFAULT_CONFIG as AGENT_DEFAULT_CONFIG) - -from gym_jiminy.rllib.utilities import initialize, train, test - -from tools.fcnet import FullyConnectedNetwork - -# Register learning environment -register_env("env", lambda env_config: gym.make(GYM_ENV_NAME, **env_config)) - -# Register the custom model architecture (it implements 'vf_share_layers') -ModelCatalog.register_custom_model("fully_connected_network", FullyConnectedNetwork) - -# ============= Initialize Ray and Tensorboard daemons ============= - -logger_creator = initialize( - num_cpus=N_THREADS, num_gpus=N_GPU, debug=DEBUG) - -# ======================== Configure model ========================= - -# Copy the default model configuration -mdl_cfg = MODEL_DEFAULTS.copy() - -# Fully-connected network settings -mdl_cfg["fcnet_activation"] = "tanh" # Nonlinearity for built-in fully connected net ["tanh", "relu", "linear"] -mdl_cfg["fcnet_hiddens"] = [64, 64] # Number of hidden layers for fully connected net -mdl_cfg["no_final_linear"] = False # Whether to skip the final linear layer used to resize the outputs to `num_outputs` -mdl_cfg["vf_share_layers"] = False # Whether layers should be shared for the value function. - -# Custom model settings -mdl_cfg["custom_model"] = "fully_connected_network" # Name of a custom model to use. (None to disable) -mdl_cfg["custom_model_config"] = {} # Dict of extra options to pass to the custom models, in addition to the usual ones (defined above) - -# ========================= Configure RLlib ======================== - -# Copy the default rllib configuration -rllib_cfg = COMMON_CONFIG.copy() - -# Ressources settings -rllib_cfg["framework"] = "torch" # Use PyTorch ("torch") instead of Tensorflow ("tf" or "tfe" (eager mode)) -rllib_cfg["log_level"] = logging.DEBUG if DEBUG else logging.ERROR -rllib_cfg["num_gpus"] = N_GPU # Number of GPUs to reserve for the trainer process -rllib_cfg["num_workers"] = int(N_THREADS//2) # Number of rollout worker processes for parallel sampling -rllib_cfg["num_envs_per_worker"] = 1 # Number of environments per worker processes -rllib_cfg["remote_worker_envs"] = False # Whether to create the envs per worker in individual remote processes. Note it adds overheads -rllib_cfg["num_cpus_per_worker"] = 1 # Number of CPUs to reserve per worker processes -rllib_cfg["num_cpus_for_driver"] = 0 # Number of CPUs to allocate for trainer process -rllib_cfg["remote_env_batch_wait_ms"] = 0 # Duration worker processes are waiting when polling environments. (0 to disable: as soon as one env is ready) -rllib_cfg["extra_python_environs_for_driver"] = {} # Extra python env vars to set for trainer process -rllib_cfg["extra_python_environs_for_worker"] = { # Extra python env vars to set for worker processes - "OMP_NUM_THREADS": "1" # Disable multi-threaded linear algebra (numpy, torch...etc), ensuring one thread per worker -} - -# Environment settings -rllib_cfg["horizon"] = None # Number of steps after which the episode is forced to terminate -rllib_cfg["soft_horizon"] = False # Calculate rewards but don't reset the environment when the horizon is hit -rllib_cfg["no_done_at_end"] = False # Don't set 'done' at the end of the episode -rllib_cfg["normalize_actions"] = False # Normalize actions to the upper and lower bounds of the action space -rllib_cfg["clip_actions"] = True # Whether to clip actions to the upper and lower bounds of the action space -rllib_cfg["clip_rewards"] = False # Whether to clip rewards prior to experience postprocessing -rllib_cfg["env_config"] = GYM_ENV_KWARGS # Arguments to pass to the env creator - -# Model settings -# rllib_cfg["replay_sequence_length"] = 1 # The number of contiguous environment steps to replay at once -rllib_cfg["model"] = mdl_cfg # Policy model configuration - -# Rollout settings -rllib_cfg["rollout_fragment_length"] = 128 # Sample batches of this size (mult. by `num_envs_per_worker`) are collected from each worker process -rllib_cfg["train_batch_size"] = 512 # Sample batches are concatenated together into batches of this size if sample_async disable, per worker process otherwise -rllib_cfg["batch_mode"] = "truncate_episodes" # Whether to rollout "complete_episodes" or "truncate_episodes" to `rollout_fragment_length` -rllib_cfg["sample_async"] = False # Use a background thread for sampling (slightly off-policy) -rllib_cfg["observation_filter"] = "NoFilter" # Element-wise observation filter ["NoFilter", "MeanStdFilter"] -rllib_cfg["compress_observations"] = False # Whether to LZ4 compress individual observations -rllib_cfg["min_iter_time_s"] = 0 # Minimum time per training iteration. To make sure timing differences do not affect training. -rllib_cfg["seed"] = SEED # sets the random seed of each worker processes (in conjunction with worker_index) - -# Learning settings -rllib_cfg["lr"] = 1.0e-3 # Default learning rate (Not use for actor-critic algorithms such as TD3) -rllib_cfg["gamma"] = 0.99 # Discount factor of the MDP (0.992: <4% after one walk stride) -rllib_cfg["explore"] = True # Disable exploration completely -rllib_cfg["exploration_config"] = { - "type": "StochasticSampling", # The Exploration class to use ["EpsilonGreedy", "Random", ...] -} -rllib_cfg["shuffle_buffer_size"] = 0 # Shuffle input batches via a sliding window buffer of this size (0 = disable) - -# Evaluation settings -rllib_cfg["evaluation_interval"] = None # Evaluate every `evaluation_interval` training iterations (None to disable) -rllib_cfg["evaluation_num_episodes"] = 50 # Number of episodes to run per evaluation -rllib_cfg["evaluation_num_workers"] = 0 # Number of dedicated parallel workers to use for evaluation (0 to disable) - -# Debugging and monitoring settings -rllib_cfg["monitor"] = False # Whether to save episode stats and videos -rllib_cfg["ignore_worker_failures"] = False # Whether to save episode stats and videos -rllib_cfg["log_level"] = "INFO" # Set the ray.rllib.* log level for the agent process and its workers [DEBUG, INFO, WARN, or ERROR] -rllib_cfg["log_sys_usage"] = True # Monitor system resource metrics (requires `psutil` and `gputil`) -rllib_cfg["metrics_smoothing_episodes"] = 100 # Smooth metrics over this many episodes -rllib_cfg["collect_metrics_timeout"] = 180 # Wait for metric batches for this duration. If not in time, collect in the next train iteration -rllib_cfg["timesteps_per_iteration"] = 0 # Minimum env steps to optimize for per train call. It does not affect learning, only monitoring - -# ================== Configure learning algorithm ================== - -# Copy the default learning algorithm configuration, including PPO-specific parameters, -# then overwrite the common parameters that has been updated ONLY. -agent_cfg = AGENT_DEFAULT_CONFIG.copy() -for key, value in rllib_cfg.items(): - if COMMON_CONFIG[key] != value: - agent_cfg[key] = value - -# Estimators settings -agent_cfg["use_gae"] = True # Use the Generalized Advantage Estimator (GAE) with a value function (https://arxiv.org/pdf/1506.02438.pdf) -agent_cfg["use_critic"] = True # Use a critic as a value baseline (otherwise don't use any; required for using GAE). -agent_cfg["lambda"] = 0.95 # The GAE(lambda) parameter. - -# Learning settings -agent_cfg["kl_coeff"] = 0.0 # Initial coefficient for KL divergence. (0.0 for L^CLIP) -agent_cfg["kl_target"] = 0.01 # Target value for KL divergence -agent_cfg["vf_share_layers"] = False # Share layers for value function. If you set this to True, it's important to tune vf_loss_coeff -agent_cfg["vf_loss_coeff"] = 0.5 # Coefficient of the value function loss -agent_cfg["entropy_coeff"] = 0.01 # Coefficient of the entropy regularizer -agent_cfg["entropy_coeff_schedule"] = None # Decay schedule for the entropy regularizer -agent_cfg["clip_param"] = 0.2 # PPO clip parameter -agent_cfg["vf_clip_param"] = float("inf") # Clip param for the value function. Note that this is sensitive to the scale of the rewards (-1 to disable) - -# Optimization settings -agent_cfg["lr_schedule"] = None # Learning rate schedule -agent_cfg["sgd_minibatch_size"] = 128 # Total SGD batch size across all devices for SGD. This defines the minibatch size of each SGD epoch -agent_cfg["num_sgd_iter"] = 8 # Number of SGD epochs to execute per train batch -agent_cfg["shuffle_sequences"] = True # Whether to shuffle sequences in the batch when training -agent_cfg["grad_clip"] = None # Clamp the norm of the gradient during optimization (None to disable) - -# ====================== Run the optimization ====================== - -agent_cfg["lr"] = 1.0e-3 -agent_cfg["lr_schedule"] = None - -train_agent = Trainer(agent_cfg, "env", logger_creator) -checkpoint_path = train(train_agent, max_timesteps=100000) - -# ===================== Enjoy a trained agent ====================== - -test_agent = Trainer(agent_cfg, "env", logger_creator) -test_agent.restore(checkpoint_path) -test(test_agent, explore=False) - -# =================== Terminate the Ray backend ==================== - -train_agent.stop() -test_agent.stop() -ray.shutdown() diff --git a/python/gym_jiminy/examples/reinforcement_learning/rllib/tools/fcnet.py b/python/gym_jiminy/examples/reinforcement_learning/rllib/tools/fcnet.py deleted file mode 100644 index 5facf783f..000000000 --- a/python/gym_jiminy/examples/reinforcement_learning/rllib/tools/fcnet.py +++ /dev/null @@ -1,330 +0,0 @@ -import copy -import logging -import numpy as np -from typing import Tuple, List, Sequence, Dict, Any - -from gym.spaces import Space, flatdim - -from ray.rllib.utils import try_import_torch, try_import_tf -from ray.rllib.utils.typing import ModelConfigDict -from ray.rllib.utils.annotations import override -from ray.rllib.utils.framework import get_activation_fn, TensorType -from ray.rllib.models.tf.misc import normc_initializer_tf -from ray.rllib.models.tf.tf_modelv2 import TFModelV2 -from ray.rllib.models.torch.misc import SlimFC, normc_initializer_torch -from ray.rllib.models.torch.torch_modelv2 import TorchModelV2 -from ray.rllib.policy.sample_batch import SampleBatch -from ray.rllib.policy.view_requirement import ViewRequirement - - -_, tf, _ = try_import_tf() -_, nn = try_import_torch() - -LOGGER = logging.getLogger(__name__) - - -class FullyConnectedNetwork(TorchModelV2, nn.Module): - """Generic fully connected network.""" - - def __init__(self, - obs_space: Space, - action_space: Space, - num_outputs: int, - model_config: ModelConfigDict, - name: str) -> None: - TorchModelV2.__init__(self, obs_space, action_space, num_outputs, - model_config, name) - nn.Module.__init__(self) - - activation = get_activation_fn( - model_config.get("fcnet_activation"), framework="torch") - hiddens = model_config.get("fcnet_hiddens") - no_final_linear = model_config.get("no_final_linear") - vf_share_layers = model_config.get("vf_share_layers") - - LOGGER.debug("Constructing fcnet {} {}".format(hiddens, activation)) - layers = [] - prev_layer_size = int(np.product(obs_space.shape)) - self._logits = None - - # Create layers 0 to second-last. - for size in hiddens[:-1]: - layers.append( - SlimFC(in_size=prev_layer_size, - out_size=size, - initializer=normc_initializer_torch(1.0), - activation_fn=activation)) - prev_layer_size = size - - # The last layer is adjusted to be of size num_outputs, but it's a - # layer with activation. - if no_final_linear and self.num_outputs: - layers.append( - SlimFC(in_size=prev_layer_size, - out_size=self.num_outputs, - initializer=normc_initializer_torch(1.0), - activation_fn=activation)) - prev_layer_size = self.num_outputs - # Finish the layers with the provided sizes (`hiddens`), plus - - # iff num_outputs > 0 - a last linear layer of size num_outputs. - else: - if len(hiddens) > 0: - layers.append( - SlimFC(in_size=prev_layer_size, - out_size=hiddens[-1], - initializer=normc_initializer_torch(1.0), - activation_fn=activation)) - prev_layer_size = hiddens[-1] - if self.num_outputs: - self._logits = SlimFC( - in_size=prev_layer_size, - out_size=self.num_outputs, - initializer=normc_initializer_torch(0.01), - activation_fn=None) - else: - self.num_outputs = ( - [np.product(obs_space.shape)] + hiddens[-1:-1])[-1] - - # Create the feature networks - self._shared_hidden_layers = None - self._hidden_layers = None - self._vf_hidden_layers = None - if vf_share_layers: - self._shared_hidden_layers = nn.Sequential(*layers) - else: - self._hidden_layers = nn.Sequential(*layers) - self._vf_hidden_layers = copy.deepcopy(self._hidden_layers) - - # Non-shared value branch. - self._value_branch = SlimFC(in_size=prev_layer_size, - out_size=1, - initializer=normc_initializer_torch(1.0), - activation_fn=None) - - # Holds the current value output. - self._cur_value = None - - @override(TorchModelV2) - def forward(self, - input_dict: Dict[str, TensorType], - state: Sequence[TensorType], - seq_lens: TensorType - ) -> Tuple[TensorType, Sequence[TensorType]]: - obs = input_dict["obs_flat"].float() - if self._shared_hidden_layers is not None: - features = self._shared_hidden_layers( - obs.reshape(obs.shape[0], -1)) - else: - features = self._hidden_layers(obs.reshape(obs.shape[0], -1)) - vf_features = self._vf_hidden_layers( - obs.reshape(obs.shape[0], -1)) - logits = self._logits(features) if self._logits else features - self._cur_value = self._value_branch(vf_features).squeeze(1) - return logits, state - - @override(TorchModelV2) - def value_function(self) -> TensorType: - assert self._cur_value is not None, "must call forward() first" - return self._cur_value - - -class FrameStackingModel(TFModelV2): - """A simple FC model that takes the last n observations, actions, and - rewards as input. - """ - def __init__(self, - obs_space: Space, - action_space: Space, - num_outputs: int, - model_config: Dict[str, Any], - name: str, - num_frames: int = 1) -> None: - # Call base initializer first - super().__init__(obs_space, action_space, None, model_config, name) - - # Backup some user arguments - self.num_frames = num_frames - self.num_outputs = num_outputs - - # Define some proxies for convenience - sensor_space_start = 0 - for field, space in obs_space.original_space.items(): - if field != "sensors": - sensor_space_start += flatdim(space) - else: - sensor_space_size = flatdim(space) - sensor_space_end = sensor_space_start + sensor_space_size - break - self.sensor_space_range = [sensor_space_start, sensor_space_end] - - # Extract some user arguments - activation = get_activation_fn(model_config.get("fcnet_activation")) - no_final_linear = model_config.get("no_final_linear") - hiddens = model_config.get("fcnet_hiddens", []) - vf_share_layers = model_config.get("vf_share_layers") - - # Specify the inputs - if self.num_frames > 1: - self.view_requirements["prev_n_obs"] = ViewRequirement( - data_col=SampleBatch.OBS, - shift="-{}:-1".format(num_frames), - space=obs_space) - self.view_requirements["prev_n_act"] = ViewRequirement( - data_col=SampleBatch.ACTIONS, - shift="-{}:-1".format(num_frames), - space=action_space) - self.view_requirements["prev_n_rew"] = ViewRequirement( - data_col=SampleBatch.REWARDS, - shift="-{}:-1".format(num_frames)) - - # Buffer to store last computed value - self._last_value = None - - # Define the input layer of the model - stack_size = sensor_space_size + action_space.shape[0] + 1 - obs = tf.keras.layers.Input( - shape=obs_space.shape, name="obs") - if self.num_frames > 1: - stack = tf.keras.layers.Input( - shape=(self.num_frames, stack_size), name="stack") - inputs = [obs, stack] - else: - inputs = obs - - # Build features extraction network - # In: (batch_size, n_features, n_timesteps) - # Out: (batch_size, n_filters, n_timesteps - (kernel_size - 1)) - if self.num_frames >= 16: - conv_1 = tf.keras.layers.Conv1D( - filters=4, - kernel_size=5, - strides=1, - activation="tanh", - padding="valid", - name="conv_1")(stack) - - pool_1 = tf.keras.layers.AveragePooling1D( - pool_size=2, - strides=2, - padding="valid", - name="pool_1")(conv_1) - - conv_2 = tf.keras.layers.Conv1D( - filters=8, - kernel_size=5, - strides=1, - activation="tanh", - padding="valid", - name="conv_2")(pool_1) - - pool_2 = tf.keras.layers.AveragePooling1D( - pool_size=2, - strides=2, - padding="valid", - name="pool_2")(conv_2) - - # Gather observation and extracted features as input - flatten = tf.keras.layers.Flatten(name="flatten")(pool_2) - features = tf.keras.layers.Dense( - units=8, - name="fc_features", - activation=activation, - kernel_initializer=normc_initializer_tf(1.0))(flatten) - concat = tf.keras.layers.Concatenate( - axis=-1, name="concat")([obs, features]) - elif self.num_frames > 1: - # Gather current observation and previous stack as input - features = tf.keras.layers.Flatten(name="flatten")(stack) - concat = tf.keras.layers.Concatenate( - axis=-1, name="concat")([obs, features]) - else: - # Current observation is the only input - concat = obs - - # concat = tf.keras.layers.GaussianNoise(0.1)(concat) - - # Create policy layers 0 to second-last. - i = 1 - last_layer = concat - for size in hiddens[:-1]: - last_layer = tf.keras.layers.Dense( - units=size, - name="fc_{}".format(i), - activation=activation, - kernel_initializer=normc_initializer_tf(1.0))(last_layer) - i += 1 - - # The last layer is adjusted to be of size num_outputs, but it is a - # layer with activation. - if no_final_linear: - logits_out = tf.keras.layers.Dense( - units=num_outputs, - name="fc_out", - activation=activation, - kernel_initializer=normc_initializer_tf(0.01))(last_layer) - # Finish the layers with the provided sizes (`hiddens`), plus a last - # linear layer of size num_outputs. - else: - last_layer = tf.keras.layers.Dense( - units=hiddens[-1], - name="fc_{}".format(i), - activation=activation, - kernel_initializer=normc_initializer_tf(1.0))(last_layer) - logits_out = tf.keras.layers.Dense( - units=num_outputs, - name="fc_out", - activation=None, - kernel_initializer=normc_initializer_tf(0.01))(last_layer) - - last_vf_layer = None - if not vf_share_layers: - # Build a dedicated hidden layers for the value net if requested - i = 1 - last_vf_layer = concat - for size in hiddens: - last_vf_layer = tf.keras.layers.Dense( - units=size, - name="fc_value_{}".format(i), - activation=activation, - kernel_initializer=normc_initializer_tf(1.0) - )(last_vf_layer) - i += 1 - - value_out = tf.keras.layers.Dense( - units=1, - name="value_out", - activation=None, - kernel_initializer=normc_initializer_tf(1.0))( - last_vf_layer or last_layer) - - # Finish definition of the model - self.base_model = tf.keras.Model(inputs, [logits_out, value_out]) - - def forward(self, - input_dict: Dict[str, tf.Tensor], - states: List[Any], - seq_lens: List[int]) -> Tuple[tf.Tensor, List[Any]]: - obs = input_dict["obs_flat"] - if self.num_frames > 1: - stack = tf.concat(( - input_dict["prev_n_obs"][..., slice(*self.sensor_space_range)], - input_dict["prev_n_act"], - tf.expand_dims(input_dict["prev_n_rew"], axis=-1) - ), axis=-1) - inputs = [obs, stack] - else: - inputs = obs - logits, self._last_value = self.base_model(inputs) - mean, std = tf.split(logits, 2, axis=-1) - std = std - 1.5 - logits = tf.concat((mean, std), axis=-1) - return logits, states - - def value_function(self) -> tf.Tensor: - return tf.reshape(self._last_value, [-1]) - - def export_to_h5(self, h5_file: str) -> None: - self.base_model.save_weights(h5_file) - - def import_from_h5(self, h5_file: str) -> None: - self.base_model.load_weights(h5_file) diff --git a/python/gym_jiminy/examples/reinforcement_learning/rllib/acrobot_ppo.py b/python/gym_jiminy/examples/rllib/acrobot_ppo.py similarity index 93% rename from python/gym_jiminy/examples/reinforcement_learning/rllib/acrobot_ppo.py rename to python/gym_jiminy/examples/rllib/acrobot_ppo.py index c99c07972..1e4865e4b 100644 --- a/python/gym_jiminy/examples/reinforcement_learning/rllib/acrobot_ppo.py +++ b/python/gym_jiminy/examples/rllib/acrobot_ppo.py @@ -4,7 +4,7 @@ It solves it consistently in less than 100000 timesteps in average. .. warning:: - This script has been tested for pytorch~=2.0 and ray[rllib]~=2.5.0 + This script has been tested for pytorch~=2.3 and ray[rllib]~=2.9.3 """ # ====================== Configure Python workspace ======================= @@ -29,21 +29,20 @@ evaluate_local_worker, evaluate_algo) +# ============================ User parameters ============================ -if __name__ == "__main__": - # ============================ User parameters ============================ - - GYM_ENV_NAME = "gym_jiminy.envs:acrobot" - GYM_ENV_KWARGS = { - 'continuous': True - } - ENABLE_VIEWER = "JIMINY_VIEWER_DISABLE" in os.environ - SPEED_RATIO = 1.0 - DEBUG = False - SEED = 0 - N_THREADS = 9 - N_GPU = 0 +GYM_ENV_NAME = "gym_jiminy.envs:acrobot" +GYM_ENV_KWARGS = { + 'continuous': True +} +ENABLE_RECORDING = True +ENABLE_VIEWER = "JIMINY_VIEWER_DISABLE" not in os.environ +DEBUG = False +SEED = 0 +N_THREADS = 9 +N_GPU = 0 +if __name__ == "__main__": # ==================== Initialize Ray and Tensorboard ===================== # Start Ray and Tensorboard background processes @@ -52,7 +51,7 @@ # Register the environment register_env("env", lambda env_config: FrameRateLimiter( - gym.make(GYM_ENV_NAME, **env_config), SPEED_RATIO)) + gym.make(GYM_ENV_NAME, **env_config), speed_ratio=1.0)) # ====================== Configure policy's network ======================= @@ -106,9 +105,9 @@ ) # Debugging and monitoring settings - algo_config.rollouts( + algo_config.fault_tolerance( # Whether to attempt to continue training if a worker crashes - ignore_worker_failures=False + recreate_failed_workers=False ) algo_config.debugging( # Set the log level for the whole learning process @@ -205,8 +204,8 @@ custom_evaluation_function=partial( evaluate_algo, print_stats=True, - enable_replay=ENABLE_VIEWER or None, - record_video=ENABLE_VIEWER + enable_replay=ENABLE_VIEWER, + record_video=ENABLE_RECORDING ), # Partially override configuration for evaluation evaluation_config=dict( @@ -292,7 +291,8 @@ algo = algo_config.build() # Train the agent - checkpoint_path = train(algo, max_timesteps=200000, logdir=algo.logdir) + result = train(algo, max_timesteps=200000, logdir=algo.logdir) + checkpoint_path = result.checkpoint.path # ========================= Terminate Ray backend ========================= @@ -303,7 +303,7 @@ # Build a standalone local evaluation worker (not requiring ray backend) register_env("env", lambda env_config: FrameRateLimiter( - gym.make(GYM_ENV_NAME, **env_config), SPEED_RATIO)) + gym.make(GYM_ENV_NAME, **env_config), speed_ratio=1.0)) worker = build_eval_worker_from_checkpoint(checkpoint_path) evaluate_local_worker(worker, evaluation_num=1, diff --git a/python/gym_jiminy/examples/reinforcement_learning/stable_baselines3/acrobot_ppo.py b/python/gym_jiminy/examples/stable_baselines3/acrobot_ppo.py similarity index 98% rename from python/gym_jiminy/examples/reinforcement_learning/stable_baselines3/acrobot_ppo.py rename to python/gym_jiminy/examples/stable_baselines3/acrobot_ppo.py index faafaecc8..0066ab166 100644 --- a/python/gym_jiminy/examples/reinforcement_learning/stable_baselines3/acrobot_ppo.py +++ b/python/gym_jiminy/examples/stable_baselines3/acrobot_ppo.py @@ -1,5 +1,5 @@ """Solve the official Open AI Gym Acrobot problem simulated in Jiminy using PPO -algorithm of Stable_baseline3 reinforcement learning framework. +algorithm of Stable_baselines3 reinforcement learning framework. It solves it consistently in less than 100000 timesteps in average, and in about 40000 at best. diff --git a/python/gym_jiminy/examples/reinforcement_learning/stable_baselines3/cartpole_ppo.py b/python/gym_jiminy/examples/stable_baselines3/cartpole_ppo.py similarity index 97% rename from python/gym_jiminy/examples/reinforcement_learning/stable_baselines3/cartpole_ppo.py rename to python/gym_jiminy/examples/stable_baselines3/cartpole_ppo.py index 0d2fb4162..97321dfbe 100644 --- a/python/gym_jiminy/examples/reinforcement_learning/stable_baselines3/cartpole_ppo.py +++ b/python/gym_jiminy/examples/stable_baselines3/cartpole_ppo.py @@ -1,5 +1,5 @@ """Solve the official Open AI Gym Acrobot problem simulated in Jiminy using PPO -algorithm of Stable_baseline3 reinforcement learning framework. +algorithm of Stable_baselines3 reinforcement learning framework. It solves it consistently in less than 100000 timesteps in average, and in about 40000 at best. diff --git a/python/gym_jiminy/examples/reinforcement_learning/stable_baselines3/tools/utilities.py b/python/gym_jiminy/examples/stable_baselines3/tools/utilities.py similarity index 100% rename from python/gym_jiminy/examples/reinforcement_learning/stable_baselines3/tools/utilities.py rename to python/gym_jiminy/examples/stable_baselines3/tools/utilities.py diff --git a/python/gym_jiminy/examples/reinforcement_learning/tianshou/acrobot_ppo.py b/python/gym_jiminy/examples/tianshou/acrobot_ppo.py similarity index 100% rename from python/gym_jiminy/examples/reinforcement_learning/tianshou/acrobot_ppo.py rename to python/gym_jiminy/examples/tianshou/acrobot_ppo.py diff --git a/python/gym_jiminy/examples/reinforcement_learning/tianshou/cartpole_ppo.py b/python/gym_jiminy/examples/tianshou/cartpole_ppo.py similarity index 100% rename from python/gym_jiminy/examples/reinforcement_learning/tianshou/cartpole_ppo.py rename to python/gym_jiminy/examples/tianshou/cartpole_ppo.py diff --git a/python/gym_jiminy/examples/reinforcement_learning/tianshou/tools/fcnet.py b/python/gym_jiminy/examples/tianshou/tools/fcnet.py similarity index 100% rename from python/gym_jiminy/examples/reinforcement_learning/tianshou/tools/fcnet.py rename to python/gym_jiminy/examples/tianshou/tools/fcnet.py diff --git a/python/gym_jiminy/examples/reinforcement_learning/tianshou/tools/utilities.py b/python/gym_jiminy/examples/tianshou/tools/utilities.py similarity index 100% rename from python/gym_jiminy/examples/reinforcement_learning/tianshou/tools/utilities.py rename to python/gym_jiminy/examples/tianshou/tools/utilities.py diff --git a/python/gym_jiminy/examples/reinforcement_learning/torchrl/acrobot_ppo.py b/python/gym_jiminy/examples/torchrl/acrobot_ppo.py similarity index 100% rename from python/gym_jiminy/examples/reinforcement_learning/torchrl/acrobot_ppo.py rename to python/gym_jiminy/examples/torchrl/acrobot_ppo.py diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py index 1a87e2c8f..2e967ceb0 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py @@ -162,13 +162,12 @@ def _update_flattened_slice(data: torch.Tensor, matrix. """ if len(shape) > 1: - data = data.reshape((-1, *shape)) \ - .swapaxes(1, 0) \ - .reshape((shape[0], -1)) + assert len(shape) == 2, "shape > 2 is not supported for now." + 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 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): @@ -269,7 +268,8 @@ def get_default_config(cls) -> AlgorithmConfig: @classmethod @override(_PPO) - def get_default_policy_class(cls, config: AlgorithmConfig + def get_default_policy_class(cls, + config: AlgorithmConfig ) -> Optional[Type[Policy]]: """Returns a default Policy class to use, given a config. """ @@ -373,6 +373,7 @@ def _get_default_view_requirements(self) -> None: data_col=SampleBatch.OBS, space=self.observation_space, shift=-1, + batch_repeat_value=1, used_for_compute_actions=False, used_for_training=True) return view_requirements diff --git a/python/gym_jiminy/rllib/setup.py b/python/gym_jiminy/rllib/setup.py index 41c028505..b1d511b72 100644 --- a/python/gym_jiminy/rllib/setup.py +++ b/python/gym_jiminy/rllib/setup.py @@ -32,13 +32,10 @@ install_requires=[ f"gym-jiminy~={gym_jiminy_version}", # Highly efficient distributed computation library used for RL - # - <1.6.0: GPU detection must be patched to work - # - 1.11.0: Breaking changes - "ray[default,rllib]~=2.5.0", - # Missing dependency of ray dashboard - "async-timeout", - # Internal dependency of Ray that must be pinned - "pydantic<2.0.0", + # * <1.6.0: GPU detection must be patched to work + # * > 2.5.0: multi-GPU support + # * 1.11.0 / 2.2.0 / 2.5.0 / 2.9.0: Breaking changes + "ray[rllib]~=2.9.0", # Used for monitoring (logging and publishing) learning progress "tensorboardX", # Plot data directly in terminal to monitor stats without X-server diff --git a/python/gym_jiminy/unit_py/data/anymal_pipeline.json b/python/gym_jiminy/unit_py/data/anymal_pipeline.json index 66c72885b..ea19558d5 100644 --- a/python/gym_jiminy/unit_py/data/anymal_pipeline.json +++ b/python/gym_jiminy/unit_py/data/anymal_pipeline.json @@ -11,11 +11,11 @@ "cls": "gym_jiminy.common.blocks.PDController", "kwargs": { "update_ratio": 2, - "order": 1, "kp": [1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0], "kd": [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01], "target_position_margin": 0.0, - "target_velocity_limit": 1000.0 + "target_velocity_limit": 100.0, + "target_acceleration_limit": 10000.0 } }, "wrapper": { @@ -24,6 +24,20 @@ } } }, + { + "block": { + "cls": "gym_jiminy.common.blocks.PDAdapter", + "kwargs": { + "update_ratio": -1, + "order": 1 + } + }, + "wrapper": { + "kwargs": { + "augment_observation": false + } + } + }, { "block": { "cls": "gym_jiminy.common.blocks.MahonyFilter", diff --git a/python/gym_jiminy/unit_py/data/anymal_pipeline.toml b/python/gym_jiminy/unit_py/data/anymal_pipeline.toml index 58866e124..ed567c463 100644 --- a/python/gym_jiminy/unit_py/data/anymal_pipeline.toml +++ b/python/gym_jiminy/unit_py/data/anymal_pipeline.toml @@ -7,14 +7,22 @@ step_dt = 0.04 block.cls = "gym_jiminy.common.blocks.PDController" [layers_config.block.kwargs] update_ratio = 2 -order = 1 kp = [1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0, 1500.0] kd = [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] target_position_margin = 0.0 -target_velocity_limit = 1000.0 +target_velocity_limit = 100.0 +target_acceleration_limit = 10000.0 [layers_config.wrapper.kwargs] augment_observation = true +[[layers_config]] +block.cls = "gym_jiminy.common.blocks.PDAdapter" +[layers_config.block.kwargs] +update_ratio = -1 +order = 1 +[layers_config.wrapper.kwargs] +augment_observation = false + [[layers_config]] block.cls = "gym_jiminy.common.blocks.MahonyFilter" [layers_config.block.kwargs] diff --git a/python/gym_jiminy/unit_py/test_deformation_estimator.py b/python/gym_jiminy/unit_py/test_deformation_estimator.py index ecc18ee90..924e89777 100644 --- a/python/gym_jiminy/unit_py/test_deformation_estimator.py +++ b/python/gym_jiminy/unit_py/test_deformation_estimator.py @@ -10,12 +10,16 @@ from jiminy_py.simulator import Simulator from gym_jiminy.common.envs import BaseJiminyEnv -from gym_jiminy.common.blocks import PDController, MahonyFilter, DeformationEstimator +from gym_jiminy.common.blocks import ( + PDController, PDAdapter, MahonyFilter, DeformationEstimator) from gym_jiminy.common.bases import ObservedJiminyEnv, ControlledJiminyEnv from gym_jiminy.common.utils import matrices_to_quat, build_pipeline from gym_jiminy.envs import AntJiminyEnv +DEBUG = "JIMINY_BUILD_DEBUG" in os.environ + + class DeformationEstimatorBlock(unittest.TestCase): """ TODO: Write documentation """ @@ -34,17 +38,17 @@ def _test_deformation_estimate(self, env, imu_atol, flex_atol): # Check that deformation estimates from DeformationEstimator are valid model_options = env.robot.get_model_options() - flexible_frame_names = [ + flexibility_frame_names = [ flex_options["frameName"] for flex_options in model_options["dynamics"]["flexibilityConfig"] ] est_flex_quats = env.observation['features']['deformation_estimator'] est_flex_quats[:] *= np.sign(est_flex_quats[-1]) for frame_name, joint_index in zip( - flexible_frame_names, env.robot.flexible_joint_indices): + flexibility_frame_names, env.robot.flexibility_joint_indices): idx_q = env.robot.pinocchio_model.joints[joint_index].idx_q true_flex_quat = env.robot_state.q[idx_q:(idx_q + 4)] - flex_index = env.observer.flexible_frame_names.index(frame_name) + flex_index = env.observer.flexibility_frame_names.index(frame_name) est_flex_quat = est_flex_quats[:, flex_index] np.testing.assert_allclose( true_flex_quat, est_flex_quat, atol=flex_atol) @@ -88,7 +92,6 @@ def test_arm(self): 'inertia': np.array([1.0, 1.0, 0.0]) } for i in range(1, 5) ] - model_options['joints']['enablePositionLimit'] = False model_options['joints']['enableVelocityLimit'] = False robot.set_model_options(model_options) @@ -96,13 +99,13 @@ def test_arm(self): simulator = Simulator(robot) # Configure the controller and sensor update periods - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options['stepper']['controllerUpdatePeriod'] = 0.0001 engine_options['stepper']['sensorsUpdatePeriod'] = 0.0001 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Instantiate the environment - env = BaseJiminyEnv(simulator, step_dt=0.01) + env = BaseJiminyEnv(simulator, step_dt=0.01, debug=DEBUG) # Add controller and observer blocks pd_controller = PDController( @@ -110,10 +113,16 @@ def test_arm(self): env, kp=150.0, kd=0.03, - order=1, update_ratio=1) env = ControlledJiminyEnv(env, pd_controller) + pd_adapter = PDAdapter( + "pd_adapter", + env, + order=1, + update_ratio=-1) + env = ControlledJiminyEnv(env, pd_adapter) + mahony_filter = MahonyFilter( "mahony_filter", env, @@ -128,7 +137,7 @@ def test_arm(self): "deformation_estimator", env, imu_frame_names=robot.sensor_names['ImuSensor'], - flex_frame_names=robot.flexible_joint_names, + flex_frame_names=robot.flexibility_joint_names, ignore_twist=True, update_ratio=-1) env = ObservedJiminyEnv(env, deformation_estimator) @@ -180,10 +189,10 @@ def _setup(self) -> None: super()._setup() # Configure the controller and sensor update periods - engine_options = self.engine.get_options() + engine_options = self.simulator.get_options() engine_options['stepper']['controllerUpdatePeriod'] = 0.0001 engine_options['stepper']['sensorsUpdatePeriod'] = 0.0001 - self.engine.set_options(engine_options) + self.simulator.set_options(engine_options) # Add flexibility frames model_options = self.robot.get_model_options() @@ -197,7 +206,7 @@ def _setup(self) -> None: "inertia": np.array([0.01, 0.01, 0.01]), } ) - model_options["dynamics"]["enableFlexibleModel"] = True + model_options["dynamics"]["enableFlexibility"] = True self.robot.set_model_options(model_options) # Create pipeline with Mahony filter and DeformationObserver blocks @@ -212,11 +221,19 @@ def _setup(self) -> None: kwargs=dict( kp=150.0, kd=0.03, - order=1, update_ratio=1, ) ) ), + dict( + block=dict( + cls=PDAdapter, + kwargs=dict( + order=1, + update_ratio=-1, + ) + ) + ), dict( block=dict( cls=MahonyFilter, @@ -243,7 +260,7 @@ def _setup(self) -> None: ) # Instantiate the environment - env = PipelineEnv() + env = PipelineEnv(debug=DEBUG) # Run a simulation env.reset(seed=0) diff --git a/python/gym_jiminy/unit_py/test_pipeline_control.py b/python/gym_jiminy/unit_py/test_pipeline_control.py index 6043a42f0..d2bad793d 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_control.py +++ b/python/gym_jiminy/unit_py/test_pipeline_control.py @@ -2,6 +2,7 @@ """ import os import io +import sys import base64 import logging import unittest @@ -15,11 +16,11 @@ from jiminy_py.core import ImuSensor as imu from jiminy_py.viewer import Viewer -import pinocchio as pin - from gym_jiminy.envs import ( AtlasPDControlJiminyEnv, CassiePDControlJiminyEnv, DigitPDControlJiminyEnv) -from gym_jiminy.common.blocks import PDController, MahonyFilter +from gym_jiminy.common.blocks import PDController, PDAdapter, MahonyFilter +from gym_jiminy.common.blocks.proportional_derivative_controller import ( + integrate_zoh) from gym_jiminy.common.utils import ( quat_to_rpy, matrix_to_rpy, matrix_to_quat, remove_twist_from_quat) @@ -29,6 +30,9 @@ # Small tolerance for numerical equality TOLERANCE = 1.0e-6 +# Skip some tests if Jiminy has been compiled in debug on Mac OS +DEBUG = "JIMINY_BUILD_DEBUG" in os.environ + class PipelineControl(unittest.TestCase): """ TODO: Write documentation @@ -88,19 +92,20 @@ def _test_pid_standing(self): # Check that the joint velocity target is zero time = log_vars["Global.Time"] velocity_target = np.stack([ - log_vars['.'.join(( - 'HighLevelController', self.env.controller.name, name))] + log_vars['.'.join(("controller", self.env.controller.name, name))] for name in self.env.controller.fieldnames], axis=-1) self.assertTrue(np.all( np.abs(velocity_target[time > time[-1] - 1.0]) < 1.0e-9)) # Check that the whole-body robot velocity is close to zero at the end velocity_mes = np.stack([ - log_vars['.'.join(('HighLevelController', name))] - for name in self.env.robot.log_velocity_fieldnames], axis=-1) + log_vars[name] for name in self.env.robot.log_velocity_fieldnames + ], axis=-1) self.assertTrue(np.all( np.abs(velocity_mes[time > time[-1] - 1.0]) < 1.0e-3)) + @unittest.skipIf(DEBUG and sys.platform == "darwin", + "skipping when compiled in debug on Mac OS") def test_pid_standing(self): for backend in ('panda3d-sync', 'meshcat'): for Env in (AtlasPDControlJiminyEnv, @@ -172,36 +177,123 @@ def test_mahony_filter(self): np.testing.assert_allclose(rpy_true, rpy_est, atol=5e-3) - def test_pid_controller(self): + def test_zoh_integrator(self): + """Make sure that low-level state integrator of the PD controller + acceleration is working as expected. + """ + SIZE = 2 + T_END = 100.0 + HLC_DT = 0.01 + HLC_TO_LLC_RATIO = 10 + TOL = 1e-9 + + # np.random.seed(0) + state_min = np.tile(np.array([-0.8, -2.0, -20.0]), (2, 1)).T + state_max = np.tile(np.array([+0.8, +2.0, +20.0]), (2, 1)).T + llc_dt = HLC_DT / HLC_TO_LLC_RATIO + + state = np.zeros((3, SIZE)) + position, velocity, acceleration = state + position_min, _, _ = state_min + position_max, _, acceleration_max = state_max + + for _ in np.arange(T_END // HLC_DT) * HLC_DT: + accel_target = acceleration_max * ( + np.random.randint(2, size=SIZE) - 0.5) + for i in range(HLC_TO_LLC_RATIO): + # Update acceleration + acceleration[:] = accel_target + + # Integration state + integrate_zoh(state, state_min, state_max, llc_dt) + + # The position, velocity and acceleration bounds are satisfied + assert np.all(np.logical_and( + state_min - TOL < state, state < state_max + TOL)) + + # FIXME: The acceleration is maxed out when slowing down + is_position_min = position == position_min + is_position_max = position == position_max + # assert np.all((acceleration > acceleration_max - TOL)[ + # is_position_min & (velocity + TOL < 0.0)]) + # assert np.all((acceleration < acceleration_min + TOL)[ + # is_position_max & (velocity - TOL > 0.0)]) + + # It is still possible to stop without hitting bounds + assert np.all(( + velocity >= - acceleration_max * llc_dt - TOL + )[is_position_min]) + assert np.all(( + velocity <= acceleration_max * llc_dt + TOL + )[is_position_max]) + + # FIXME: The velocity is zero-ed at the velocity limits + # is_velocity_min = np.isclose( + # np.abs(velocity), velocity_min, atol=TOL, rtol=0.0) + # is_velocity_max = np.isclose( + # np.abs(velocity), velocity_max, atol=TOL, rtol=0.0) + # assert np.all(acceleration[is_velocity_min] >= - TOL) + # assert np.all(acceleration[is_velocity_max] <= TOL) + + # FIXME: The acceleration is altered only if necessary + # is_position_limit = is_position_min | is_position_max + # is_velocity_limit = is_velocity_min | is_velocity_max + # assert np.all(np.isclose( + # np.abs(acceleration), 0.5 * acceleration_max, atol=TOL, + # rtol=0.0)[~(is_position_limit | is_velocity_limit)]) + + def test_pd_controller(self): """ TODO: Write documentation. """ # Instantiate the environment and run a simulation with random action env = AtlasPDControlJiminyEnv() + + # Make sure that a PD controller is plugged to the robot + adapter = env.controller + assert isinstance(adapter, PDAdapter) and adapter.order == 1 + controller = env.env.env.controller + assert isinstance(controller, PDController) + + # Disable acceleration limits of PD controller + controller._command_state_lower[2] = float("-inf") + controller._command_state_upper[2] = float("inf") + + # Run a few environment steps env.reset(seed=0) env.unwrapped._height_neutral = float("-inf") while env.stepper_state.t < 2.0: env.step(0.2 * env.action_space.sample()) # Extract the target position and velocity of a single motor - controller = env.env.controller - assert isinstance(controller, PDController) and controller.order == 1 - ctrl_name = controller.name + adapter_name, controller_name = adapter.name, controller.name n_motors = len(controller.fieldnames) - pos = env.log_data["variables"][".".join(( - "HighLevelController", ctrl_name, "state", str(n_motors - 1)))] - vel = env.log_data["variables"][".".join(( - "HighLevelController", ctrl_name, controller.fieldnames[-1]))] + target_pos = env.log_data["variables"][".".join(( + "controller", controller_name, "state", str(n_motors - 1)))] + target_vel = env.log_data["variables"][".".join(( + "controller", controller_name, "state", str(2 * n_motors - 1)))] + target_accel = env.log_data["variables"][".".join(( + "controller", controller_name, controller.fieldnames[-1]))] + command_vel = env.log_data["variables"][".".join(( + "controller", adapter_name, adapter.fieldnames[-1]))] # Make sure that the position and velocity targets are consistent - self.assertTrue(np.allclose( - np.diff(pos) / controller.control_dt, vel[1:], atol=TOLERANCE)) - - # Make sure that the position targets are within bounds. - # No such guarantee exists for higher-order derivatives. + target_vel_diff = np.diff(target_pos) / controller.control_dt + target_accel_diff = np.diff(target_vel) / controller.control_dt + update_ratio = round(adapter.control_dt / controller.control_dt) + np.testing.assert_allclose(target_vel[(update_ratio-1)::update_ratio], + command_vel[(update_ratio-1)::update_ratio], + atol=TOLERANCE) + np.testing.assert_allclose(target_accel_diff, target_accel[1:], atol=TOLERANCE) + np.testing.assert_allclose(target_vel_diff, target_vel[1:], atol=TOLERANCE) + + # Make sure that the position and velocity targets are within bounds robot = env.robot pos_min = robot.position_limit_lower[robot.motor_position_indices[-1]] pos_max = robot.position_limit_upper[robot.motor_position_indices[-1]] - self.assertTrue(np.all(np.logical_and(pos_min < pos, pos < pos_max))) + vel_limit = robot.velocity_limit[robot.motor_velocity_indices[-1]] + self.assertTrue(np.all(np.logical_and( + pos_min <= target_pos, target_pos <= pos_max))) + self.assertTrue(np.all(np.abs(target_vel) <= vel_limit)) def test_repeatability(self): # Instantiate the environment diff --git a/python/gym_jiminy/unit_py/test_pipeline_design.py b/python/gym_jiminy/unit_py/test_pipeline_design.py index 35d7c49e2..211b1692b 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_design.py +++ b/python/gym_jiminy/unit_py/test_pipeline_design.py @@ -10,6 +10,7 @@ from jiminy_py.robot import _gcd from gym_jiminy.common.utils import build_pipeline, load_pipeline from gym_jiminy.common.bases import InterfaceJiminyEnv +from gym_jiminy.common.blocks import PDController TOLERANCE = 1.0e-6 @@ -40,11 +41,11 @@ def setUp(self): cls='gym_jiminy.common.blocks.PDController', kwargs=dict( update_ratio=2, - order=1, kp=self.pid_kp, kd=self.pid_kd, target_position_margin=0.0, - target_velocity_limit=float("inf") + target_velocity_limit=float("inf"), + target_acceleration_limit=float("inf") ) ), wrapper=dict( @@ -52,6 +53,19 @@ def setUp(self): augment_observation=True ) ) + ), dict( + block=dict( + cls='gym_jiminy.common.blocks.PDAdapter', + kwargs=dict( + update_ratio=-1, + order=1, + ) + ), + wrapper=dict( + kwargs=dict( + augment_observation=False + ) + ) ), dict( block=dict( cls='gym_jiminy.common.blocks.MahonyFilter', @@ -167,10 +181,13 @@ def test_step_state(self): # Perform a single step env = self.ANYmalPipelineEnv() env.reset(seed=0) - action = env.env.observation['actions']['pd_controller'].copy() - action += 1.0e-3 + action = env.action + 1.0e-3 obs, *_ = env.step(action) + # Extract PD controller wrapper env + env_ctrl = env.env.env.env + assert isinstance(env_ctrl.controller, PDController) + # Observation stacking is skipping the required number of frames stack_dt = (self.skip_frames_ratio + 1) * env.observe_dt t_obs_last = env.step_dt - env.step_dt % stack_dt @@ -180,7 +197,7 @@ def test_step_state(self): # Initial observation is consistent with internal simulator state controller_target_obs = obs['actions']['pd_controller'] - self.assertTrue(np.all(controller_target_obs[-1] == action)) + self.assertTrue(np.all(controller_target_obs[-1] == env_ctrl.action)) imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] imu_data_obs = obs['measurements']['ImuSensor'][-1] self.assertFalse(np.all(imu_data_ref == imu_data_obs)) @@ -205,19 +222,22 @@ def test_update_periods(self): env = self.ANYmalPipelineEnv() def configure_telemetry() -> InterfaceJiminyEnv: - engine_options = env.simulator.engine.get_options() + engine_options = env.simulator.get_options() engine_options['telemetry']['enableCommand'] = True engine_options['stepper']['logInternalStepperSteps'] = False - env.simulator.engine.set_options(engine_options) + env.simulator.set_options(engine_options) return env env.reset(seed=0, options=dict(reset_hook=configure_telemetry)) env.step(env.action) - # Check that the command is updated 1/2 low-level controller update + controller = env.env.env.env.controller + assert isinstance(controller, PDController) + + # Check that the PD command is updated 1/2 low-level controller update log_vars = env.log_data['variables'] - u_log = log_vars['HighLevelController.currentCommandLF_HAA'] - self.assertEqual(env.control_dt, 2 * env.unwrapped.control_dt) + u_log = log_vars['currentCommandLF_HAA'] + self.assertEqual(controller.control_dt, 2 * env.unwrapped.control_dt) self.assertTrue(np.all(u_log[:2] == 0.0)) self.assertNotEqual(u_log[1], u_log[2]) self.assertEqual(u_log[2], u_log[3]) diff --git a/python/gym_jiminy/unit_py/test_quantities.py b/python/gym_jiminy/unit_py/test_quantities.py new file mode 100644 index 000000000..9ae31c239 --- /dev/null +++ b/python/gym_jiminy/unit_py/test_quantities.py @@ -0,0 +1,137 @@ +""" TODO: Write documentation +""" +import gc +import unittest + +import numpy as np +import gymnasium as gym +import jiminy_py +import pinocchio as pin + +from gym_jiminy.common.quantities import ( + QuantityManager, EulerAnglesFrame, CenterOfMass, ZeroMomentPoint) + + +class Quantities(unittest.TestCase): + """ TODO: Write documentation + """ + def test_shared_cache(self): + env = gym.make("gym_jiminy.envs:atlas") + env.reset() + + quantity_manager = QuantityManager(env) + for name, cls, kwargs in ( + ("acom", CenterOfMass, dict(kinematic_level=pin.ACCELERATION)), + ("com", CenterOfMass, {}), + ("zmp", ZeroMomentPoint, {})): + quantity_manager[name] = (cls, kwargs) + quantities = quantity_manager._quantities + + assert len(quantity_manager) == 3 + assert len(quantities["zmp"].cache.owners) == 1 + assert len(quantities["com"].cache.owners) == 2 + + zmp_0 = quantity_manager.zmp.copy() + assert quantities["com"].cache.has_value + assert not quantities["acom"].cache.has_value + assert not quantities["com"]._is_initialized + assert quantities["zmp"].requirements["com"]._is_initialized + + env.step(env.action) + zmp_1 = quantity_manager["zmp"].copy() + assert np.all(zmp_0 == zmp_1) + quantity_manager.clear() + assert quantities["zmp"].requirements["com"]._is_initialized + assert not quantities["com"].cache.has_value + zmp_1 = quantity_manager.zmp.copy() + assert np.any(zmp_0 != zmp_1) + + env.step(env.action) + quantity_manager.reset() + assert not quantities["zmp"].requirements["com"]._is_initialized + zmp_2 = quantity_manager.zmp.copy() + assert np.any(zmp_1 != zmp_2) + + def test_dynamic_batching(self): + env = gym.make("gym_jiminy.envs:atlas") + env.reset() + env.step(env.action) + + quantity_manager = QuantityManager(env) + for name, cls, kwargs in ( + ("rpy_0", EulerAnglesFrame, dict( + frame_name=env.robot.pinocchio_model.frames[1].name)), + ("rpy_1", EulerAnglesFrame, dict( + frame_name=env.robot.pinocchio_model.frames[1].name)), + ("rpy_2", EulerAnglesFrame, dict( + frame_name=env.robot.pinocchio_model.frames[-1].name))): + quantity_manager[name] = (cls, kwargs) + quantities = quantity_manager._quantities + + rpy_0 = quantity_manager.rpy_0.copy() + assert len(quantities['rpy_0'].requirements['data'].frame_names) == 1 + assert np.all(rpy_0 == quantity_manager.rpy_1) + rpy_2 = quantity_manager.rpy_2.copy() + assert np.any(rpy_0 != rpy_2) + assert len(quantities['rpy_2'].requirements['data'].frame_names) == 2 + + env.step(env.action) + quantity_manager.reset() + rpy_0_next = quantity_manager.rpy_0 + assert np.any(rpy_0 != rpy_0_next) + assert len(quantities['rpy_2'].requirements['data'].frame_names) == 2 + + assert len(quantities['rpy_1'].requirements['data'].cache.owners) == 3 + del quantity_manager['rpy_2'] + gc.collect() + assert len(quantities['rpy_1'].requirements['data'].cache.owners) == 2 + quantity_manager.rpy_1 + assert len(quantities['rpy_1'].requirements['data'].frame_names) == 1 + + quantity_manager.reset(reset_tracking=True) + assert np.all(rpy_0_next == quantity_manager.rpy_0) + assert len(quantities['rpy_0'].requirements['data'].frame_names) == 1 + + def test_discard(self): + env = gym.make("gym_jiminy.envs:atlas") + env.reset() + + quantity_manager = QuantityManager(env) + for name, cls, kwargs in ( + ("rpy_0", EulerAnglesFrame, dict( + frame_name=env.robot.pinocchio_model.frames[1].name)), + ("rpy_1", EulerAnglesFrame, dict( + frame_name=env.robot.pinocchio_model.frames[1].name)), + ("rpy_2", EulerAnglesFrame, dict( + frame_name=env.robot.pinocchio_model.frames[-1].name))): + quantity_manager[name] = (cls, kwargs) + quantities = quantity_manager._quantities + + assert len(quantities['rpy_1'].cache.owners) == 2 + assert len(quantities['rpy_2'].requirements['data'].cache.owners) == 3 + + del quantity_manager['rpy_0'] + gc.collect() + assert len(quantities['rpy_1'].cache.owners) == 1 + assert len(quantities['rpy_2'].requirements['data'].cache.owners) == 2 + + del quantity_manager['rpy_1'] + gc.collect() + assert len(quantities['rpy_2'].requirements['data'].cache.owners) == 1 + + del quantity_manager['rpy_2'] + gc.collect() + for cache in quantity_manager._caches.values(): + assert len(cache.owners) == 0 + + def test_env(self): + env = gym.make("gym_jiminy.envs:atlas") + + env.quantities["com"] = (CenterOfMass, {}) + + env.reset(seed=0) + com_0 = env.quantities["com"].copy() + env.step(env.action) + assert np.all(com_0 != env.quantities["com"]) + env.reset(seed=0) + assert np.all(com_0 == env.quantities["com"]) diff --git a/python/gym_jiminy/unit_py/test_training_toys_models.py b/python/gym_jiminy/unit_py/test_training_toys_models.py index 63ee34fbe..f2a49c3d6 100644 --- a/python/gym_jiminy/unit_py/test_training_toys_models.py +++ b/python/gym_jiminy/unit_py/test_training_toys_models.py @@ -2,6 +2,7 @@ low-level Jiminy engine, to the Gym environment integration. However, it does not assessed that the viewer is working properly. """ +import os import unittest from typing import Optional, Dict, Any @@ -14,9 +15,13 @@ from utilities import train +# Fix the seed and number of threads for CI stability SEED = 0 N_THREADS = 5 +# Skip some tests if Jiminy has been compiled in debug +DEBUG = "JIMINY_BUILD_DEBUG" in os.environ + class ToysModelsStableBaselinesPPO(unittest.TestCase): """Solve several official Open AI Gym toys models problems simulated in @@ -89,6 +94,7 @@ def _ppo_training(cls, # Run the learning process return train(train_agent, test_env, max_timesteps=200000) + @unittest.skipIf(DEBUG, "skipping when compiled in debug") def test_acrobot_stable_baselines(self): """Solve acrobot for both continuous and discrete action spaces. """ @@ -97,6 +103,7 @@ def test_acrobot_stable_baselines(self): self.assertTrue(self._ppo_training( "gym_jiminy.envs:acrobot", {'continuous': False})) + @unittest.skipIf(DEBUG, "skipping when compiled in debug") def test_cartpole_stable_baselines(self): """Solve cartpole for both continuous and discrete action spaces. """ diff --git a/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py b/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py index 6d5ee38dd..84444fe11 100644 --- a/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py +++ b/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py @@ -30,7 +30,7 @@ simulator = Simulator.build(urdf_path, has_freeflyer=True) # Enable constraint contact model - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options['contacts']['model'] = 'constraint' engine_options['contacts']['stabilizationFreq'] = 20.0 engine_options["constraints"]['regularization'] = 0.0 @@ -43,7 +43,7 @@ # Set the ground contact options engine_options['contacts']['friction'] = 1.0 engine_options['contacts']['torsion'] = 0.0 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Generate random ground profile ground_params = list(starmap(random_tile_ground, zip( @@ -51,7 +51,7 @@ TILE_ORIENTATION, TILE_SEED))) engine_options["world"]["groundProfile"] = sum_heightmaps([ ground_params[0], merge_heightmaps(ground_params[1:])]) - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Sample the initial state qpos = pin.neutral(simulator.robot.pinocchio_model) diff --git a/python/jiminy_py/examples/constraint_fixed_frame.py b/python/jiminy_py/examples/constraint_fixed_frame.py index 87cd7e87b..3b7a29343 100644 --- a/python/jiminy_py/examples/constraint_fixed_frame.py +++ b/python/jiminy_py/examples/constraint_fixed_frame.py @@ -21,15 +21,15 @@ robot = simulator.robot # Disable constraint solver regularization - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options["constraints"]["regularization"] = 0.0 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Continuous sensor and controller update - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options["stepper"]["controllerUpdatePeriod"] = 0.0 engine_options["stepper"]["sensorsUpdatePeriod"] = 0.0 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Add fixed frame constraint constraint = jiminy.FrameConstraint( diff --git a/python/jiminy_py/examples/constraint_wheel.py b/python/jiminy_py/examples/constraint_wheel.py index 54c7a17b6..e1c2bec1c 100644 --- a/python/jiminy_py/examples/constraint_wheel.py +++ b/python/jiminy_py/examples/constraint_wheel.py @@ -20,7 +20,7 @@ urdf_path, has_freeflyer=True, hardware_path="") # Disable constraint solver regularization - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options["constraints"]["regularization"] = 0.0 # Continuous sensor and controller update @@ -30,7 +30,7 @@ # Configure integrator engine_options['stepper']['odeSolver'] = 'runge_kutta_dopri5' engine_options['stepper']['dtMax'] = 1.0e-3 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Add fixed frame constraint constraint = jiminy.WheelConstraint( diff --git a/python/jiminy_py/examples/double_pendulum.py b/python/jiminy_py/examples/double_pendulum.py index e080b92e4..788fd03ac 100644 --- a/python/jiminy_py/examples/double_pendulum.py +++ b/python/jiminy_py/examples/double_pendulum.py @@ -102,7 +102,7 @@ def internal_dynamics(self, t, q, v, u_custom): # Plot some data using standard tools only plt.figure() - plt.plot(log_vars['Global.Time'], log_vars['HighLevelController.energy']) + plt.plot(log_vars['Global.Time'], log_vars['energy']) plt.title('Double pendulum energy') plt.grid() plt.show() diff --git a/python/jiminy_py/examples/sphere_rolling.py b/python/jiminy_py/examples/sphere_rolling.py index 85e2645a1..2f51155dc 100644 --- a/python/jiminy_py/examples/sphere_rolling.py +++ b/python/jiminy_py/examples/sphere_rolling.py @@ -18,7 +18,7 @@ simulator = Simulator.build(urdf_path, has_freeflyer=True, hardware_path="") # Disable constraint solver regularization - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options["constraints"]["regularization"] = 0.0 # Continuous sensor and controller update @@ -28,7 +28,7 @@ # Configure integrator engine_options['stepper']['odeSolver'] = 'runge_kutta_dopri5' engine_options['stepper']['dtMax'] = 1.0e-3 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Add fixed frame constraint constraint = jiminy.SphereConstraint("MassBody", 0.5) diff --git a/python/jiminy_py/examples/tutorial.ipynb b/python/jiminy_py/examples/tutorial.ipynb index c224333d1..c29b9ae36 100644 --- a/python/jiminy_py/examples/tutorial.ipynb +++ b/python/jiminy_py/examples/tutorial.ipynb @@ -7,7 +7,7 @@ "# An introduction to Jiminy\n", "\n", "\n", - "Jiminy is a simulation framework built around the `pinocchio` rigidbody dynamics library, targeting the simulation of robotic systems. This document covers the use of jiminy in python for individual robotic simulation: usage in C++ is similar, see C++ API documentation and examples. For information about using jiminy in a learning context with Gym OpenAI, see examples in gym_jiminy. \n", + "Jiminy is a simulation framework built around the `pinocchio` rigid body dynamics library, targeting the simulation of robotic systems. This document covers the use of jiminy in python for individual robotic simulation: usage in C++ is similar, see C++ API documentation and examples. For information about using jiminy in a learning context with Gym OpenAI, see examples in gym_jiminy. \n", "\n", "Jiminy supports the following features:\n", "\n", @@ -154,7 +154,7 @@ "# Let's plot the joint position to see the pendulum fall.\n", "import matplotlib.pyplot as plt\n", "\n", - "plt.plot(log_vars['Global.Time'], log_vars['HighLevelController.currentPositionPendulum'])\n", + "plt.plot(log_vars['Global.Time'], log_vars['currentPositionPendulum'])\n", "plt.title('Pendulum angle (rad)')\n", "plt.grid()\n", "plt.show()" diff --git a/python/jiminy_py/setup.py b/python/jiminy_py/setup.py index 8812c5086..d432b24cc 100644 --- a/python/jiminy_py/setup.py +++ b/python/jiminy_py/setup.py @@ -21,18 +21,26 @@ def finalize_options(self) -> None: self.install_lib = self.install_platlib -# Enforce the right numpy version. It assumes the version currently available -# was used to compile all the C++ extension modules shipping with Jiminy. -# - Numpy API is not backward compatible but is forward compatible -# - For some reason, forward compatibility from 1.19 to 1.20+ seems broken -# - A few versions must be blacklisted because of Boost::Python incompatibility -# - Numpy >= 1.21.0 is required for full typing support +# Determine the supported range of numpy versions, assuming that the version +# currently available was used to compile all C++ extension modules bundled +# with Jiminy. +# * Numpy API as limited backward compatibility range based on some complex +# logics not following a predefined pattern (since 1.25). See documentation: +# https://numpy.org/devdocs/dev/depending_on_numpy.html#build-time-dependency +# * Numpy API is only minor-version forward compatible np_ver = tuple(map(int, version('numpy').split(".", 3)[:2])) -np_req = f"numpy>={np_ver[0]}.{np_ver[1]}.0" -if np_ver < (1, 20): - np_req += ",<1.20.0" -elif np_ver < (1, 22): - np_req += ",!=1.21.0,!=1.21.1,!=1.21.2,!=1.21.3,!=1.21.4" +if np_ver < (1, 25): + np_req = f"numpy>={np_ver[0]}.{np_ver[1]}.0" + if np_ver < (1, 20): + np_req += ",<1.20.0" + elif np_ver < (1, 22): + np_req += ",!=1.21.0,!=1.21.1,!=1.21.2,!=1.21.3,!=1.21.4" +else: + if np_ver < (2, 1): + np_req = "numpy>=1.24" # All version down to 1.19 are supported + else: + raise ImportError("'numpy>2.0' not supported at built-time for now.") + np_req += f",<{np_ver[0]}.{np_ver[1] + 1}" setup( @@ -85,24 +93,22 @@ def finalize_options(self) -> None: "jiminy_replay=jiminy_py.viewer.replay:_play_logs_files_entrypoint" ]}, install_requires=[ - # Display elegant and versatile process bar. - "tqdm", # Standard library for matrix algebra. # - 1.20 breaks ABI # - >=1.21,<1.21.5 is causing segfault with boost::python. # See issue: https://github.com/boostorg/python/issues/376 # - 1.22 breaks API for compiled libs. + # - 1.24 adds `dtype` optional argument to `np.stack` + # - 2.0 is backward compatible up to 1.23, but not forward compatible. + # see: https://numpy.org/devdocs/dev/depending_on_numpy.html np_req, # Parser for Jiminy's hardware description file. "toml", - # Used internally by Robot to replace meshes by associated minimal - # volume bounding box. - "trimesh", # Standalone cross-platform mesh visualizer used as Viewer's backend. # Panda3d is NOT supported by PyPy even if built from source. # - 1.10.12 fixes numerous bugs # - 1.10.13 crashes when generating wheels on MacOS - "panda3d>=1.10.14", + "panda3d>=1.10.13", # Photo-realistic shader for Panda3d to improve rendering of meshes. # - 0.11.X is not backward compatible. "panda3d-simplepbr==0.11.2", @@ -111,7 +117,12 @@ def finalize_options(self) -> None: # Used internally by Viewer to record video programmatically when # Panda3d is used as rendering backend. # - >= 8.0.0 provides cross-platform precompiled binary wheels - "av>=8.0.0" + "av>=8.0.0", + # Used internally by Robot to replace meshes by associated minimal + # volume bounding box. + "trimesh", + # Display elegant and versatile process bar. + "tqdm" ], extras_require={ "plot": [ @@ -132,7 +143,8 @@ def finalize_options(self) -> None: "pillow", # Used internally by Viewer to enable recording video # programmatically with Meshcat as backend. - "playwright" + # - 1.43 is broken + "playwright!=1.43" ], "dev": [ # Generate Python type hints files (aka. stubs) for C extensions. diff --git a/python/jiminy_py/src/jiminy_py/dynamics.py b/python/jiminy_py/src/jiminy_py/dynamics.py index 2df894a69..479510797 100644 --- a/python/jiminy_py/src/jiminy_py/dynamics.py +++ b/python/jiminy_py/src/jiminy_py/dynamics.py @@ -145,11 +145,11 @@ class TrajectoryDataType(TypedDict, total=False): """Basic data structure storing the required information about a trajectory to later replay it using `jiminy_py.viewer.play_trajectories`. """ - # List of State objects of increasing time. + # List of State objects of increasing time evolution_robot: Sequence[State] - # Jiminy robot. None if omitted. + # Optional Jiminy robot robot: Optional[jiminy.Robot] - # Whether to use theoretical or actual model + # Whether to use the theoretical model or the extended simulation model use_theoretical_model: bool @@ -219,7 +219,7 @@ def update_quantities(robot: jiminy.Model, Optional: False by default. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching - the robot's state. + the state of the robot. Optional: True by default. """ if use_theoretical_model: @@ -244,11 +244,11 @@ def update_quantities(robot: jiminy.Model, if update_com: if velocity is None: - kinematic_level = pin.KinematicLevel.POSITION + kinematic_level = pin.POSITION elif acceleration is None: - kinematic_level = pin.KinematicLevel.VELOCITY + kinematic_level = pin.VELOCITY else: - kinematic_level = pin.KinematicLevel.ACCELERATION + kinematic_level = pin.ACCELERATION pin.centerOfMass(model, data, kinematic_level, False) if update_jacobian: @@ -295,8 +295,7 @@ def get_body_world_transform(robot: jiminy.Model, :param body_name: Name of the body. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching - the robot's state. - Optional: True by default. + the state of the robot. :param copy: Whether to return the internal buffers (which could be altered) or copy them. Optional: True by default. It is less efficient but safer. @@ -335,7 +334,7 @@ def get_body_world_velocity(robot: jiminy.Model, :param body_name: Name of the body. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching - the robot's state. + the state of the robot. Optional: True by default. :returns: Spatial velocity. @@ -370,7 +369,7 @@ def get_body_world_acceleration(robot: jiminy.Model, :param body_name: Name of the body. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching - the robot's state. + the state of the robot. Optional: True by default. :returns: Spatial acceleration. @@ -571,8 +570,8 @@ def compute_freeflyer_state_from_fixed_body( :param ground_profile: Ground profile callback. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating - and fetching the robot's state. Must be False if `fixed_body_name` is - not speficied. + and fetching the state of the robot. Must be False if `fixed_body_name` + is not specified. Optional: True by default if `fixed_body_name` is specified, False otherwise. @@ -607,7 +606,7 @@ def compute_freeflyer_state_from_fixed_body( if fixed_body_name is None: if use_theoretical_model: raise RuntimeError( - "Cannot infer contact transform for theoretical model.") + "Cannot infer contact transform for the theoretical model.") w_M_ff = compute_transform_contact(robot, ground_profile) else: ff_M_fixed_body = get_body_world_transform( @@ -657,7 +656,7 @@ def compute_efforts_from_fixed_body( :param fixed_body_name: Name of the body frame. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching - the robot's state. + the state of the robot. Optional: True by default. :returns: articular efforts and external forces. @@ -708,10 +707,10 @@ def compute_inverse_dynamics(robot: jiminy.Model, :param position: Robot configuration vector. :param velocity: Robot velocity vector. :param acceleration: Robot acceleration vector. - :param use_theoretical_model: Whether the position, velocity and - acceleration are associated with the - theoretical model instead of the actual one. - Optional: False by default. + :param use_theoretical_model: + Whether the position, velocity and acceleration are associated with the + theoretical model instead of the extended one. + Optional: False by default. :returns: motor torques. """ @@ -720,10 +719,11 @@ def compute_inverse_dynamics(robot: jiminy.Model, "Robot without active constraints is not supported for now.") # Convert theoretical position, velocity and acceleration if necessary - if use_theoretical_model and robot.is_flexible: - position = robot.get_flexible_position_from_rigid(position) - velocity = robot.get_flexible_velocity_from_rigid(velocity) - acceleration = robot.get_flexible_velocity_from_rigid(acceleration) + if use_theoretical_model and robot.is_flexibility_enabled: + position = robot.get_extended_position_from_theoretical(position) + velocity = robot.get_extended_velocity_from_theoretical(velocity) + acceleration = ( + robot.get_extended_velocity_from_theoretical(acceleration)) # Define some proxies for convenience model = robot.pinocchio_model @@ -731,8 +731,7 @@ def compute_inverse_dynamics(robot: jiminy.Model, motor_velocity_indices = robot.motor_velocity_indices # Updating kinematics quantities - pin.forwardKinematics( - model, data, position, velocity, acceleration) + pin.forwardKinematics(model, data, position, velocity, acceleration) pin.updateFramePlacements(model, data) # Compute constraint jacobian and drift diff --git a/python/jiminy_py/src/jiminy_py/log.py b/python/jiminy_py/src/jiminy_py/log.py index b75f0216a..b4240414f 100644 --- a/python/jiminy_py/src/jiminy_py/log.py +++ b/python/jiminy_py/src/jiminy_py/log.py @@ -3,6 +3,7 @@ reconstructing the robot to reading telemetry variables. """ import os +import re import tempfile from bisect import bisect_right from collections import OrderedDict @@ -24,7 +25,6 @@ from .dynamics import State, TrajectoryDataType -ENGINE_NAMESPACE = "HighLevelController" SENSORS_FIELDS: Dict[ Type[jiminy.AbstractSensor], Union[List[str], Dict[str, List[str]]] ] = { @@ -51,8 +51,8 @@ @overload def extract_variables_from_log(log_vars: Dict[str, np.ndarray], fieldnames: FieldNested, - namespace: Optional[str] = ENGINE_NAMESPACE, *, - as_dict: Literal[False] = False + namespace: str = "", + *, as_dict: Literal[False] = False ) -> List[np.ndarray]: ... @@ -60,24 +60,25 @@ def extract_variables_from_log(log_vars: Dict[str, np.ndarray], @overload def extract_variables_from_log(log_vars: Dict[str, np.ndarray], fieldnames: FieldNested, - namespace: Optional[str] = ENGINE_NAMESPACE, *, - as_dict: Literal[True] + namespace: str = "", + *, as_dict: Literal[True] ) -> Dict[str, np.ndarray]: ... def extract_variables_from_log(log_vars: Dict[str, np.ndarray], fieldnames: FieldNested, - namespace: Optional[str] = ENGINE_NAMESPACE, *, - as_dict: bool = False) -> Union[ + namespace: str = "", + *, as_dict: bool = False + ) -> Union[ List[np.ndarray], Dict[str, np.ndarray]]: """Extract values associated with a set of variables in a specific namespace. :param log_vars: Logged variables as a dictionary. :param fieldnames: Structured fieldnames. - :param namespace: Namespace of the fieldnames. None to disable. - Optional: ENGINE_TELEMETRY_NAMESPACE by default. + :param namespace: Namespace of the fieldnames. Empty string to disable. + Optional: Empty by default. :param keep_structure: Whether to return a dictionary mapping flattened fieldnames to values. Optional: True by default. @@ -92,9 +93,9 @@ def extract_variables_from_log(log_vars: Dict[str, np.ndarray], values: List[np.ndarray] = [] for fieldname_path, fieldname in tree.flatten_with_path(fieldnames): # A key is the concatenation of namespace and full path of fieldname - key = ".".join(map(str, filter( - lambda key: isinstance(key, str), - (namespace, *fieldname_path, fieldname)))) + key = ".".join(filter( + lambda key: isinstance(key, str) and key, # type: ignore[arg-type] + (namespace, *fieldname_path, fieldname))) # Raise an exception if the key does not exists and not fail safe if key not in log_vars: @@ -116,11 +117,17 @@ def build_robot_from_log( log_data: Dict[str, Any], mesh_path_dir: Optional[str] = None, mesh_package_dirs: Sequence[str] = (), + *, robot_name: str = "" ) -> jiminy.Robot: - """Build robot from log. + """Create and initialize a robot from a single- or multi- robot simulation. + + .. warning:: + If the robot to be built is from a multi-robot simulation, then its + name needs to be specified explicitly. Alternatively, one can load all + robots simultaneously in log file using `build_robots_from_log`. .. note:: - model options and `robot.pinocchio_model` will be the same as during + Model options and `robot.pinocchio_model` will be the same as during the simulation until the next call to `reset` method unless the options of the robot that has been restored are overwritten manually. @@ -142,37 +149,43 @@ def build_robot_from_log( directories to the ones provided by log file. It may be necessary to specify it to read log generated on a different environment. + :param robot_name: Name of the robot to build from log. :returns: Reconstructed robot, and parsed log data as returned by `jiminy_py.log.read_log` method. """ # Instantiate empty robot - robot = jiminy.Robot() + robot = jiminy.Robot(robot_name) # Extract log constants log_constants = log_data["constants"] - # Extract common info - pinocchio_model = log_constants[ - ".".join((ENGINE_NAMESPACE, "pinocchio_model"))] + try: + pinocchio_model = log_constants[ + ".".join(filter(None, (robot_name, "pinocchio_model")))] + except KeyError as e: + if robot_name == "": + raise ValueError( + "No robot with empty name has been found. Specify a name or " + "call `build_robots_from_log`.") from e try: # Extract geometry models collision_model = log_constants[ - ".".join((ENGINE_NAMESPACE, "collision_model"))] + ".".join(filter(None, (robot_name, "collision_model")))] visual_model = log_constants[ - ".".join((ENGINE_NAMESPACE, "visual_model"))] + ".".join(filter(None, (robot_name, "visual_model")))] # Initialize the model robot.initialize(pinocchio_model, collision_model, visual_model) except KeyError as e: # Extract initialization arguments urdf_data = log_constants[ - ".".join((ENGINE_NAMESPACE, "urdf_file"))] + ".".join(filter(None, (robot_name, "urdf_file")))] has_freeflyer = int(log_constants[ - ".".join((ENGINE_NAMESPACE, "has_freeflyer"))]) + ".".join(filter(None, (robot_name, "has_freeflyer")))]) mesh_package_dirs = [*mesh_package_dirs, *log_constants.get( - ".".join((ENGINE_NAMESPACE, "mesh_package_dirs")), ())] + ".".join(filter(None, (robot_name, "mesh_package_dirs"))), ())] # Make sure urdf data is available if len(urdf_data) <= 1: @@ -185,7 +198,7 @@ def build_robot_from_log( tempfile.gettempdir(), f"{next(tempfile._get_candidate_names())}.urdf") with open(urdf_path, "xb") as f: - f.write(urdf_data.encode()) + f.write(urdf_data) # Fix the mesh paths in the URDF model if requested if mesh_path_dir is not None: @@ -200,8 +213,8 @@ def build_robot_from_log( os.remove(urdf_path) # Load the options - all_options = log_constants[".".join((ENGINE_NAMESPACE, "options"))] - robot.set_options(all_options["robot"]) + all_options = log_constants["options"] + robot.set_options(all_options[robot_name or "robot"]) # Update model in-place. # Note that `__setstate__` re-allocates memory instead of just calling @@ -218,8 +231,48 @@ def build_robot_from_log( return robot +def build_robots_from_log( + log_data: Dict[str, Any], + mesh_path_dir: Optional[str] = None, + mesh_package_dirs: Sequence[str] = (), + ) -> Sequence[jiminy.Robot]: + """Build all the robots in the log of the simulation. + + .. note:: + This function calls `build_robot_from_log` to build each robot. + Refer to `build_robot_from_log` for more information. + + :param log_data: Logged data (constants and variables) as a dictionary. + :param mesh_path_dir: Overwrite the common root of all absolute mesh paths. + It which may be necessary to read log generated on a + different environment. + :param mesh_package_dirs: Prepend custom mesh package search path + directories to the ones provided by log file. It + may be necessary to specify it to read log + generated on a different environment. + + :returns: Sequence of reconstructed robots. + """ + # Try to infer robot names from log constants + robot_names = [] + for key in log_data["constants"].keys(): + robot_name_match = re.match(r"^(\w*?)\.?has_freeflyer$", key) + if robot_name_match is not None: + robot_names.append(robot_name_match.group(1)) + + # Build all the robots sequentially + robots = [] + for robot_name in robot_names: + robot = build_robot_from_log( + log_data, mesh_path_dir, mesh_package_dirs, robot_name=robot_name) + robots.append(robot) + + return robots + + def extract_trajectory_from_log(log_data: Dict[str, Any], - robot: Optional[jiminy.Model] = None + robot: Optional[jiminy.Model] = None, + *, robot_name: Optional[str] = None ) -> TrajectoryDataType: """Extract the minimal required information from raw log data in order to replay the simulation in a viewer. @@ -234,27 +287,40 @@ def extract_trajectory_from_log(log_data: Dict[str, Any], :param robot: Jiminy robot associated with the logged trajectory. Optional: None by default. If None, then it will be reconstructed from 'log_data' using `build_robot_from_log`. + :param robot_name: Name of the robot to be constructed in the log. If it + is not known, call `build_robot_from_log`. :returns: Trajectory dictionary. The actual trajectory corresponds to the field "evolution_robot" and it is a list of State object. The other fields are additional information. """ + # Prepare robot namespace + if robot is not None: + if robot_name is None: + robot_name = robot.name + elif robot_name != robot.name: + raise ValueError( + "The name specified in `robot_name` and the name of the robot " + "are differents.") + elif robot_name is None: + robot_name = "" + # Handling of default argument(s) if robot is None: - robot = build_robot_from_log(log_data) + robot = build_robot_from_log(log_data, robot_name=robot_name) # Define some proxies for convenience log_vars = log_data["variables"] # Extract the joint positions, velocities and external forces over time positions = np.stack([ - log_vars.get(".".join((ENGINE_NAMESPACE, field)), []) + log_vars.get(".".join(filter(None, (robot_name, field))), []) for field in robot.log_position_fieldnames], axis=-1) velocities = np.stack([ - log_vars.get(".".join((ENGINE_NAMESPACE, field)), []) + log_vars.get(".".join(filter(None, (robot_name, field))), []) for field in robot.log_velocity_fieldnames], axis=-1) forces = np.stack([ - log_vars.get(".".join((ENGINE_NAMESPACE, field)), []) + log_vars.get(".".join(filter(None, (robot_name, field))), []) for field in robot.log_f_external_fieldnames], axis=-1) # Determine which optional data are available @@ -281,6 +347,41 @@ def extract_trajectory_from_log(log_data: Dict[str, Any], "use_theoretical_model": False} +def extract_trajectories_from_log( + log_data: Dict[str, Any], + robots: Optional[Sequence[jiminy.Model]] = None + ) -> Dict[str, TrajectoryDataType]: + """Extract the minimal required information from raw log data in order to + replay the simulation in a viewer. + + .. note:: + This function calls `extract_trajectory_from_log` to extract each + robot's trajectory. Refer to `extract_trajectory_from_log` for more + information. + + :param log_data: Logged data (constants and variables) as a dictionary. + :param robots: Sequend of Jiminy robots associated with the logged + trajectory. + Optional: None by default. If None, then it will be + reconstructed from 'log_data' using `build_robots_from_log`. + + :returns: Dictonary mapping each robot name to its corresponding + trajectory. + """ + trajectories = {} + + # Handling of default argument(s) + if robots is None: + robots = build_robots_from_log(log_data) + + for robot in robots: + trajectory = extract_trajectory_from_log( + log_data, robot, robot_name=robot.name) + + trajectories[robot.name] = trajectory + return trajectories + + def update_sensor_measurements_from_log( log_data: Dict[str, Any], robot: jiminy.Model diff --git a/python/jiminy_py/src/jiminy_py/plot.py b/python/jiminy_py/src/jiminy_py/plot.py index 2d21c58c5..82b83cbd8 100644 --- a/python/jiminy_py/src/jiminy_py/plot.py +++ b/python/jiminy_py/src/jiminy_py/plot.py @@ -606,7 +606,7 @@ def plot_log(log_data: Dict[str, Any], Optional: None by default. If None, then it will be reconstructed from 'log_data' using `build_robot_from_log`. :param enable_flexiblity_data: - Enable display of flexible joints in robot's configuration, + Enable display of flexibility joints in robot's configuration, velocity and acceleration subplots. Optional: False by default. :param block: Whether to wait for the figure to be closed before @@ -634,7 +634,7 @@ def plot_log(log_data: Dict[str, Any], # Get time and robot positions, velocities, and acceleration time = log_vars["Global.Time"] - for fields_type in ["Position", "Velocity", "Acceleration"]: + for fields_type in ("Position", "Velocity", "Acceleration"): fieldnames = getattr(robot, "_".join(( "log", fields_type.lower(), "fieldnames"))) if not enable_flexiblity_data: @@ -642,13 +642,13 @@ def plot_log(log_data: Dict[str, Any], fieldnames = list(filter( lambda field: not any( name in field - for name in robot.flexible_joint_names), + for name in robot.flexibility_joint_names), fieldnames)) try: values = extract_variables_from_log( - log_vars, fieldnames, as_dict=True) + log_vars, fieldnames, namespace=robot.name, as_dict=True) tabs_data[' '.join(("State", fields_type))] = OrderedDict( - (field.split(".", 1)[1][7:].replace(fields_type, ""), elem) + (field[7:].replace(fields_type, ""), elem) for field, elem in values.items()) except ValueError: # Variable has not been recorded and is missing in log file @@ -657,7 +657,7 @@ def plot_log(log_data: Dict[str, Any], # Get motors efforts information try: motors_efforts = extract_variables_from_log( - log_vars, robot.log_motor_effort_fieldnames) + log_vars, robot.log_motor_effort_fieldnames, namespace=robot.name) tabs_data['MotorEffort'] = OrderedDict(zip( robot.motor_names, motors_efforts)) except ValueError: @@ -667,7 +667,7 @@ def plot_log(log_data: Dict[str, Any], # Get command information try: command = extract_variables_from_log( - log_vars, robot.log_command_fieldnames) + log_vars, robot.log_command_fieldnames, namespace=robot.name) tabs_data['Command'] = OrderedDict(zip(robot.motor_names, command)) except ValueError: # Variable has not been recorded and is missing in log file @@ -679,15 +679,14 @@ def plot_log(log_data: Dict[str, Any], sensor_names = robot.sensor_names.get(sensors_type, []) if not sensor_names: continue - namespace = sensors_type if sensors_class.has_prefix else None if isinstance(sensors_fields, dict): for fields_prefix, fieldnames in sensors_fields.items(): try: type_name = ' '.join((sensors_type, fields_prefix)) data_nested = [ - extract_variables_from_log(log_vars, [ - '.'.join((name, fields_prefix + field)) - for name in sensor_names], namespace) + extract_variables_from_log(log_vars, ['.'.join( + (sensors_type, name, fields_prefix + field)) + for name in sensor_names], robot.name) for field in fieldnames] tabs_data[type_name] = OrderedDict( (field, OrderedDict(zip(sensor_names, data))) @@ -700,8 +699,8 @@ def plot_log(log_data: Dict[str, Any], try: type_name = ' '.join((sensors_type, field)) data = extract_variables_from_log(log_vars, [ - '.'.join((name, field)) for name in sensor_names - ], namespace) + '.'.join((sensors_type, name, field)) + for name in sensor_names], robot.name) tabs_data[type_name] = OrderedDict(zip( sensor_names, data)) except ValueError: diff --git a/python/jiminy_py/src/jiminy_py/robot.py b/python/jiminy_py/src/jiminy_py/robot.py index d181c9206..04046457d 100644 --- a/python/jiminy_py/src/jiminy_py/robot.py +++ b/python/jiminy_py/src/jiminy_py/robot.py @@ -16,6 +16,7 @@ import toml import numpy as np import trimesh +import trimesh.parent import hppfcl import pinocchio as pin @@ -679,14 +680,20 @@ def load_hardware_description_file( if os.path.exists(mesh_path): break - # Compute the minimal volume bounding box, then add new frames to - # the robot model at its vertices and register contact points at - # their location. + # Compute the minimal volume bounding box, then try: mesh = trimesh.load(mesh_path) except ValueError: # Mesh file is not available continue + + # Assert(s) for type checker + assert isinstance(mesh, trimesh.parent.Geometry) + + # Extract oriented bounding box box = mesh.bounding_box_oriented + + # Add new frames to the robot model at its vertices and register + # contact points at their respective location. for i in range(8): frame_name = "_".join((mesh_name, "BoundingBox", str(i))) frame_transform_rel = pin.SE3( @@ -696,16 +703,16 @@ def load_hardware_description_file( contact_frame_names.append(frame_name) # Add the collision bodies and contact points. - # Note that it must be done before adding the sensors because - # Contact sensors requires contact points to be defined. - # Mesh collisions is not numerically stable for now, so disabling it. - # Note: Be careful, the order of the contact points is important, it - # changes the computation of the external forces, which is an iterative - # algorithm for impulse model, resulting in different simulation - # results. The order of the element of the set depends of the `hash` - # method of python, whose seed is randomly generated when starting the - # interpreter for security reason. As a result, the set must be sorted - # manually to ensure consistent results. + # * It must be done before adding the sensors because Contact sensors + # requires contact points to be defined. + # * Mesh collisions is not numerically stable for now, so disabling it. + # * Be careful, the order of the contact points is important, it changes + # the computation of the external forces, which is an iterative algorithm + # for impulse model, resulting in different simulation results. The order + # of the element of the set depends of the `hash` method of python, whose + # seed is randomly generated when starting the interpreter for security + # reason. As a result, the set must be sorted manually to ensure consistent + # results. robot.add_collision_bodies( collision_body_names, ignore_meshes=avoid_instable_collisions) robot.add_contact_points(sorted(list(set(contact_frame_names)))) @@ -858,12 +865,15 @@ class BaseJiminyRobot(jiminy.Robot): Hardware description files within the same directory and having the name than the URDF file will be detected automatically without requiring to manually specify its path. + + :param name: Name of the robot to be created. + Optional: Empty string by default. """ - def __init__(self) -> None: + def __init__(self, name: Optional[str] = "") -> None: self.extra_info: Dict[str, Any] = {} self.hardware_path: Optional[str] = None self._urdf_path_orig: Optional[str] = None - super().__init__() + super().__init__(name) def initialize(self, urdf_path: str, diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index 8b374451a..2dfd5a2d6 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/python/jiminy_py/src/jiminy_py/simulator.py @@ -10,19 +10,16 @@ import logging import pathlib import tempfile +import warnings from copy import deepcopy -from itertools import chain from functools import partial -from typing import ( - Any, List, Dict, Optional, Union, Sequence, Iterable, Callable) +from typing import Any, List, Dict, Optional, Union, Sequence, Callable import toml import numpy as np -import pinocchio as pin from . import core as jiminy -from .robot import (generate_default_hardware_description_file, - BaseJiminyRobot) +from .robot import BaseJiminyRobot, generate_default_hardware_description_file from .dynamics import TrajectoryDataType from .log import read_log, build_robot_from_log from .viewer import (CameraPoseType, @@ -54,21 +51,112 @@ ProfileForceFunc = Callable[[float, np.ndarray, np.ndarray, np.ndarray], None] +def _build_robot_from_urdf(name: str, + urdf_path: str, + hardware_path: Optional[str] = None, + mesh_path_dir: Optional[str] = None, + has_freeflyer: bool = True, + avoid_instable_collisions: bool = True, + debug: bool = False) -> jiminy.Robot: + r"""Create and initialize a new robot from scratch, based on configuration + files only. It creates a default hardware file if none is provided. See + `generate_default_hardware_description_file` for details. + + :param name: Name of the robot to build from URDF. + :param urdf_path: Path of the URDF of the robot. + :param hardware_path: Path of Jiminy hardware description toml file. + Optional: Looking for '\*_hardware.toml' file in + the same folder and with the same name. + :param mesh_path_dir: Path to the folder containing all the meshes. + Optional: Env variable 'JIMINY_DATA_PATH' will be + used if available. + :param has_freeflyer: Whether the robot is fixed-based wrt its root + link, or can move freely in the world. + Optional: True by default. + :param avoid_instable_collisions: Prevent numerical instabilities by + replacing collision mesh by vertices + of associated minimal volume bounding + box, and replacing primitive box by + its vertices. + :param debug: Whether the debug mode must be activated. Doing it + enables temporary files automatic deletion. + """ + # Check if robot name is valid + if re.match('[^A-Za-z0-9_]', name): + raise ValueError("The name of the robot should be case-insensitive " + "ASCII alphanumeric characters plus underscore.") + + # Generate a temporary Hardware Description File if necessary + if hardware_path is None: + hardware_path = str(pathlib.Path( + urdf_path).with_suffix('')) + '_hardware.toml' + if not os.path.exists(hardware_path): + # Create a file that will be closed (thus deleted) at exit + urdf_name = os.path.splitext(os.path.basename(urdf_path))[0] + fd, hardware_path = tempfile.mkstemp( + prefix=f"{urdf_name}_", suffix="_hardware.toml") + os.close(fd) + + if not debug: + def remove_file_at_exit(file_path: str) -> None: + try: + os.remove(file_path) + except (PermissionError, FileNotFoundError): + pass + + atexit.register(partial( + remove_file_at_exit, hardware_path)) + + # Generate default Hardware Description File + generate_default_hardware_description_file( + urdf_path, hardware_path, verbose=debug) + + # Build the robot + robot = BaseJiminyRobot(name) + robot.initialize( + urdf_path, hardware_path, mesh_path_dir, (), has_freeflyer, + avoid_instable_collisions, load_visual_meshes=debug, verbose=debug) + + return robot + + class Simulator: - """Single-robot simulation wrapper providing a unified user-API on top of - the low-level jiminy C++ core library and Python-native modules for 3D - rendering and log data visualization. + """Simulation wrapper providing a unified user-API on top of the low-level + jiminy C++ core library and Python-native modules for 3D rendering and log + data visualization. + + The simulation can now be multi-robot but it has been design to remain as + easy of use as possible for single-robot simulation which are just + multi-robot simulations with only one robot. + + * Single-robot simulations: The name of the robot is an empty string by + default but can be specified. It will then appear in the log if specified. + * Multi-robots simulations: The name of the first robot is an empty string + by default but it is advised to specify one. You can add robots to the + simulation with the method `add_robot`, robot names have to be specified. + + Some proxy and methods are not compatible with multi-robot simulations: + + Single-robot simulations | Multi-robot simulations + --------------------------------------------------------------------------- + Simulator.viewer ---> Simulator.viewers + Simulator.robot ---> Simulator.engine.robots + Simulator.robot_state ---> Simulator.engine.robot_states + Simulator.register_profile_force -> Simulator.engine.register_profile_force + Simulator.register_impulse_force -> Simulator.engine.register_impulse_force + + The methods `replay` and `plot` are not supported for multi-robot + simulations at the time being. + + In case of multi-robot simulations, single-robot proxies either return + information associated with the first robot or raise an exception. """ def __init__(self, # pylint: disable=unused-argument robot: jiminy.Robot, - use_theoretical_model: bool = False, viewer_kwargs: Optional[Dict[str, Any]] = None, **kwargs: Any) -> None: """ :param robot: Jiminy robot already initialized. - :param use_theoretical_model: Whether the state corresponds to the - theoretical model when updating and - fetching the robot's state. :param viewer_kwargs: Keyword arguments to override default arguments whenever a viewer must be instantiated, eg when `render` method is first called. Specifically, @@ -78,15 +166,12 @@ def __init__(self, # pylint: disable=unused-argument generation. """ # Backup the user arguments - self.use_theoretical_model = use_theoretical_model self.viewer_kwargs = deepcopy(viewer_kwargs or {}) - # Handling of default argument(s) - if "robot_name" not in self.viewer_kwargs: - model_name = robot.pinocchio_model.name - base_name = re.sub('[^A-Za-z0-9_]', '_', model_name) - robot_name = f"{base_name}_{next(tempfile._get_candidate_names())}" - self.viewer_kwargs["robot_name"] = robot_name + # Check if robot name is valid + if re.match('[^A-Za-z0-9_]', robot.name): + raise ValueError("The name of the robot should be case-insensitive" + " ASCII alphanumeric characters plus underscore.") # Instantiate the low-level Jiminy engine, then initialize it self.engine = jiminy.Engine() @@ -97,7 +182,6 @@ def __init__(self, # pylint: disable=unused-argument self.is_simulation_running = self.engine.is_simulation_running # Viewer management - self.viewer: Optional[Viewer] = None self._viewers: Sequence[Viewer] = [] # Internal buffer for progress bar management @@ -118,8 +202,9 @@ def build(cls, config_path: Optional[str] = None, avoid_instable_collisions: bool = True, debug: bool = False, + *, robot_name: str = "", **kwargs: Any) -> 'Simulator': - r"""Create a new simulator instance from scratch, based on + r"""Create a new single-robot simulator instance from scratch based on configuration files only. :param urdf_path: Path of the urdf model to be used for the simulation. @@ -136,9 +221,11 @@ def build(cls, imported AFTER loading the hardware description file. It can be automatically generated from an instance by calling `export_config_file` method. - Optional: Looking for '\*_options.toml' file in the - same folder and with the same name. If not found, - using default configuration. + One can specify `None` for loading for the file + having the same full path as the URDF file but + suffix '_options.toml' if any. Passing an empty + string "" will force skipping import completely. + Optional: Empty by default :param avoid_instable_collisions: Prevent numerical instabilities by replacing collision mesh by vertices of associated minimal volume bounding @@ -146,38 +233,21 @@ def build(cls, its vertices. :param debug: Whether the debug mode must be activated. Doing it enables temporary files automatic deletion. + :param robot_name: Desired name of the robot. + Optional: Empty string by default. :param kwargs: Keyword arguments to forward to class constructor. """ - # Generate a temporary Hardware Description File if necessary - if hardware_path is None: - hardware_path = str(pathlib.Path( - urdf_path).with_suffix('')) + '_hardware.toml' - if not os.path.exists(hardware_path): - # Create a file that will be closed (thus deleted) at exit - urdf_name = os.path.splitext(os.path.basename(urdf_path))[0] - fd, hardware_path = tempfile.mkstemp( - prefix=f"{urdf_name}_", suffix="_hardware.toml") - os.close(fd) - - if not debug: - def remove_file_at_exit(file_path: str) -> None: - try: - os.remove(file_path) - except (PermissionError, FileNotFoundError): - pass - - atexit.register(partial( - remove_file_at_exit, hardware_path)) - - # Generate default Hardware Description File - generate_default_hardware_description_file( - urdf_path, hardware_path, verbose=debug) + # Handling of default argument(s) + if config_path is None: + config_path = str( + pathlib.Path(urdf_path).with_suffix('')) + '_options.toml' + if not os.path.exists(config_path): + config_path = "" # Instantiate and initialize the robot - robot = BaseJiminyRobot() - robot.initialize( - urdf_path, hardware_path, mesh_path_dir, (), has_freeflyer, - avoid_instable_collisions, load_visual_meshes=debug, verbose=debug) + robot = _build_robot_from_urdf( + robot_name, urdf_path, hardware_path, mesh_path_dir, has_freeflyer, + avoid_instable_collisions, debug) # Instantiate and initialize the engine simulator = Simulator.__new__(cls) @@ -206,6 +276,7 @@ def remove_file_at_exit(file_path: str) -> None: engine_options['contacts']['damping'] = \ robot.extra_info.pop('groundDamping', DEFAULT_GROUND_DAMPING) + # Set engine options simulator.engine.set_options(engine_options) # Override the default options by the one in the configuration file @@ -214,6 +285,61 @@ def remove_file_at_exit(file_path: str) -> None: return simulator + def add_robot(self, + name: str, + urdf_path: str, + hardware_path: Optional[str] = None, + mesh_path_dir: Optional[str] = None, + has_freeflyer: bool = True, + avoid_instable_collisions: bool = True, + debug: bool = False) -> None: + r"""Create a new robot from scratch based on configuration files only + and add it to the simulator. + + :param urdf_path: Path of the urdf model to be used for the simulation. + :param hardware_path: Path of Jiminy hardware description toml file. + Optional: Looking for '\*_hardware.toml' file in + the same folder and with the same name. + :param mesh_path_dir: Path to the folder containing all the meshes. + Optional: Env variable 'JIMINY_DATA_PATH' will be + used if available. + :param has_freeflyer: Whether the robot is fixed-based wrt its root + link, or can move freely in the world. + Optional: True by default. + :param avoid_instable_collisions: Prevent numerical instabilities by + replacing collision mesh by vertices + of associated minimal volume bounding + box, and replacing primitive box by + its vertices. + :param debug: Whether the debug mode must be activated. Doing it + enables temporary files automatic deletion. + """ + # Instantiate the robot + robot = _build_robot_from_urdf( + name, urdf_path, hardware_path, mesh_path_dir, has_freeflyer, + avoid_instable_collisions, debug) + + # Check if some unsupported objects have been specified + unsupported_nested_paths = ( + ('groundStiffness', ('contacts', 'stiffness')), + ('groundDamping', ('contacts', 'damping')), + ('controllerUpdatePeriod', ('stepper', 'controllerUpdatePeriod')), + ('sensorsUpdatePeriod', ('stepper', 'sensorsUpdatePeriod'))) + engine_options = self.engine.get_options() + for extra_info_key, option_nested_path in unsupported_nested_paths: + if extra_info_key in robot.extra_info.keys(): + option = engine_options + for option_path in option_nested_path: + option = option[option_path] + if robot.extra_info[extra_info_key] != option: + warnings.warn( + f"You have specified a different {extra_info_key} " + "than the one of the engine, the simulation will run " + f"with {option}.") + + # Add the new robot to the engine + self.engine.add_robot(robot) + def __del__(self) -> None: """Custom deleter to make sure the close is properly closed at exit. """ @@ -230,64 +356,74 @@ def __getattr__(self, name: str) -> Any: """ return getattr(self.__getattribute__('engine'), name) - def __dir__(self) -> Iterable[str]: + def __dir__(self) -> List[str]: """Attribute lookup. It is mainly used by autocomplete feature of Ipython. It is overloaded to get consistent autocompletion wrt `getattr`. """ - return chain(super().__dir__(), dir(self.engine)) + return [*super().__dir__(), *dir(self.engine)] + + @property + def viewer(self) -> Optional[Viewer]: + """Convenience proxy to get the viewer associated with the robot that + was first added if any. + """ + if self.is_viewer_available: + return self._viewers[0] + return None + + @property + def viewers(self) -> Sequence[Viewer]: + """Convenience proxy to get all the viewers associated with the ongoing + simulation. + """ + return self._viewers[:len(self.engine.robots)] @property def robot(self) -> jiminy.Robot: - """Convenience proxy to get the robot. + """Convenience proxy to get the robot in single-robot simulations. Internally, all it does is returning `self.engine.robots[0]` without any additional processing. + + .. warning:: + Method only supported for single-robot simulations. Call + `self.engine.robots` in multi-robot simulations. """ # A property is used in place of an instance attribute to keep pointing # to the right robot if the latter has been replaced by the user. Doing # so is dodgy and rarely necessary. For this reason, this capability is # not advertised but supported nonetheless. - return self.engine.robots[0] + robot, = self.engine.robots + return robot @property def robot_state(self) -> jiminy.RobotState: - """Convenience proxy to get the state of the robot. + """Convenience proxy to get the state of the robot in single-robot + simulations. Internally, all it does is returning `self.engine.robot_states[0]` without any additional processing. - """ - # property is used in place of attribute for extra safety - return self.engine.robot_states[0] - @property - def pinocchio_model(self) -> pin.Model: - """Get the appropraite pinocchio model, depending on the value of - 'self.use_theoretical_model'. - """ - # property is used in place of attribute for extra safety - if self.use_theoretical_model and self.robot.is_flexible: - return self.robot.pinocchio_model_th - return self.robot.pinocchio_model - - @property - def pinocchio_data(self) -> pin.Data: - """Get the appropriate pinocchio data, depending on the value of - 'self.use_theoretical_model'. + .. warning:: + Method only supported for single-robot simulations. Call + `self.engine.robot_states` in multi-robot simulations. """ # property is used in place of attribute for extra safety - if self.use_theoretical_model and self.robot.is_flexible: - return self.robot.pinocchio_data_th - return self.robot.pinocchio_data + robot_state, = self.engine.robot_states + return robot_state @property def is_viewer_available(self) -> bool: - """Returns whether a viewer instance associated with the robot - is available. + """Returns whether some viewer instances associated with the ongoing + simulation is currently opened. + + .. warning:: + Method only supported for single-robot simulations. """ - return (self.viewer is not None and - self.viewer.is_open()) # type: ignore[misc] + return (len(self._viewers) > 0 and + self._viewers[0].is_open()) # type: ignore[misc] def register_profile_force(self, frame_name: str, @@ -322,6 +458,11 @@ def register_profile_force(self, the whole simulation. There is no issue for C++ bindings such as `jiminy.RandomPerlinProcess`. """ + if len(self.engine.robots) > 1: + raise NotImplementedError( + "To register a force in multirobot simulation, you should use " + "`simulation.engine.register_profile_force` and specify the " + "name of the robot you want the force applied to.") return self.engine.register_profile_force( "", frame_name, force_func, update_period) @@ -344,6 +485,11 @@ def register_impulse_force(self, is aligned with world frame. It is represented as a `np.ndarray` (Fx, Fy, Fz, Mx, My, Mz). """ + if len(self.engine.robots) > 1: + raise NotImplementedError( + "To register a force in multirobot simulation, you should use " + "`simulation.engine.register_profile_force` and specify the " + "name of the robot you want the force applied to.") return self.engine.register_impulse_force("", frame_name, t, dt, force) def seed(self, seed: Union[np.uint32, np.ndarray]) -> None: @@ -384,37 +530,53 @@ def reset(self, remove_all_forces: bool = False) -> None: # Reset the backend engine self.engine.reset(False, remove_all_forces) - def start(self, - q_init: np.ndarray, - v_init: np.ndarray, - a_init: Optional[np.ndarray] = None, - is_state_theoretical: bool = False) -> None: + def start( + self, + q_init: Union[np.ndarray, Dict[str, np.ndarray]], + v_init: Union[np.ndarray, Dict[str, np.ndarray]], + a_init: Optional[Union[np.ndarray, Dict[str, np.ndarray]]] = None, + is_state_theoretical: Optional[bool] = None + ) -> None: """Initialize a simulation, starting from (q_init, v_init) at t=0. - :param q_init: Initial configuration. - :param v_init: Initial velocity. - :param a_init: Initial acceleration. It is only used by acceleration - dependent sensors and controllers, such as IMU and force - sensors. - :param is_state_theoretical: Whether the initial state is associated - with the actual or theoretical model of - the robot. + :param q_init: Initial configuration (by robot if it is a dictionnary). + :param v_init: Initial velocity (by robot if it is a dictionnary). + :param a_init: Initial acceleration (by robot if it is a dictionnary). + It is only used by acceleration dependent sensors and + controllers, such as IMU and force sensors. + :param is_state_theoretical: In single robot simulations, whether the + initial state is associated with the + actual or theoretical model of the robot. """ # Call base implementation - self.engine.start(q_init, v_init, a_init, is_state_theoretical) + # Single-robot simulations with `np.ndarray`, `is_state_theoretical` is + # supported. + if isinstance(q_init, np.ndarray): + if is_state_theoretical is None: + is_state_theoretical = False + self.engine.start(q_init, v_init, a_init, is_state_theoretical) + # Multi-robot simulations or single-robot simulations with + # dictionnaries, `is_state_theoretical` is not supported. + else: + if is_state_theoretical is not None: + raise NotImplementedError( + "Optional argument 'is_state_theoretical' is only " + "supported for single-robot simulations.") + self.engine.start(q_init, v_init, a_init) # Share the external force buffer of the viewer with the engine. # Note that the force vector must be converted to pain list to avoid # copy with external sub-vector. - if self.viewer is not None: - self.viewer.f_external = [*self.robot_state.f_external][1:] + for robot_state, viewer in zip(self.engine.robot_states, self.viewers): + viewer.f_external = [*robot_state.f_external][1:] def simulate(self, t_end: float, - q_init: np.ndarray, - v_init: np.ndarray, - a_init: Optional[np.ndarray] = None, - is_state_theoretical: bool = True, + q_init: Union[np.ndarray, Dict[str, np.ndarray]], + v_init: Union[np.ndarray, Dict[str, np.ndarray]], + a_init: Optional[ + Union[np.ndarray, Dict[str, np.ndarray]]] = None, + is_state_theoretical: Optional[bool] = None, callback: Optional[Callable[[], bool]] = None, log_path: Optional[str] = None, show_progress_bar: bool = True) -> None: @@ -424,12 +586,14 @@ def simulate(self, Optionally, log the result of the simulation. :param t_end: Simulation duration. - :param q_init: Initial configuration. - :param v_init: Initial velocity. - :param a_init: Initial acceleration. - :param is_state_theoretical: Whether the initial state is associated - with the actual or theoretical model of - the robot. + :param q_init: Initial configuration (by robot if it is a dictionnary). + :param v_init: Initial velocity (by robot if it is a dictionnary). + :param a_init: Initial acceleration (by robot if it is a dictionnary). + It is only used by acceleration dependent sensors and + controllers, such as IMU and force sensors. + :param is_state_theoretical: In single robot simulations, whether the + initial state is associated with the + actual or theoretical model of the robot. :param callback: Callable that can be specified to abort simulation. It will be evaluated after every simulation step. Abort if false is returned. @@ -474,8 +638,24 @@ def callback_wrapper(callback: Callable[[], bool]) -> bool: # Run the simulation err = None try: - self.engine.simulate( - t_end, q_init, v_init, a_init, is_state_theoretical, callback) + # Call base implementation + # Single-robot simulations with `np.ndarray`, + # `is_state_theoretical` is supported. + if isinstance(q_init, np.ndarray): + if is_state_theoretical is None: + is_state_theoretical = False + self.engine.simulate( + t_end, q_init, v_init, a_init, is_state_theoretical, + callback) + # Multi-robot simulations or single-robot simulations with + # dictionnaries, `is_state_theoretical` is not supported. + else: + if is_state_theoretical is not None: + raise NotImplementedError( + "Optional argument 'is_state_theoretical' is only " + "supported for single-robot simulations.") + self.engine.simulate(t_end, q_init, v_init, a_init, callback) + except Exception as e: # pylint: disable=broad-exception-caught err = e @@ -508,8 +688,8 @@ def render(self, camera_pose: Optional[CameraPoseType] = None, update_ground_profile: Optional[bool] = None, **kwargs: Any) -> Optional[np.ndarray]: - """Render the current state of the simulation. One can display it - or return an RGB array instead. + """Render the current state of the simulation. One can display it or + return an RGB array instead. :param return_rgb_array: Whether to return the current frame as an rgb array or render it directly. @@ -548,53 +728,59 @@ def render(self, # Instantiate the robot and viewer client if necessary. # A new dedicated scene and window will be created. if not self.is_viewer_available: - # Make sure that the current viewer is properly closed if any - if self.viewer is not None: - self.viewer.close() - self.viewer = None - - # Create new viewer instance - self.viewer = Viewer( - self.robot, - use_theoretical_model=False, - open_gui_if_parent=False, - **viewer_kwargs) - assert self.viewer is not None and self.viewer.backend is not None - - # Share the external force buffer of the viewer with the engine - if self.is_simulation_running: - self.viewer.f_external = [*self.robot_state.f_external][1:] - - if self.viewer.backend.startswith('panda3d'): - # Enable display of COM, DCM and contact markers by default if - # the robot has freeflyer. - if self.robot.has_freeflyer: - if "display_com" not in viewer_kwargs: - self.viewer.display_center_of_mass(True) - if "display_dcm" not in viewer_kwargs: - self.viewer.display_capture_point(True) - if "display_contacts" not in viewer_kwargs: - self.viewer.display_contact_forces(True) - - # Enable display of external forces by default only for - # the joints having an external force registered to it. - if "display_f_external" not in viewer_kwargs: - profile_forces, *_ = self.engine.profile_forces.values() - force_frames = set( - self.robot.pinocchio_model.frames[f.frame_index].parent - for f in profile_forces) - impulse_forces, *_ = self.engine.impulse_forces.values() - force_frames |= set( - self.robot.pinocchio_model.frames[f.frame_index].parent - for f in impulse_forces) - visibility = self.viewer._display_f_external - assert isinstance(visibility, list) - for i in force_frames: - visibility[i - 1] = True - self.viewer.display_external_forces(visibility) + # Make sure that the current viewers are properly closed if any + for viewer in self._viewers: + viewer.close() + self._viewers.clear() + + # Create new viewer instances + for robot, robot_state in zip( + self.engine.robots, self.engine.robot_states): + # Create a single viewer instance + viewer = Viewer( + robot, + robot_name=robot.name, + use_theoretical_model=False, + open_gui_if_parent=False, + **viewer_kwargs) + assert viewer.backend is not None + self._viewers.append(viewer) + + # Share the external force buffer of the viewer with the engine + if self.engine.is_simulation_running: + viewer.f_external = [*robot_state.f_external][1:] + + if viewer.backend.startswith('panda3d'): + # Enable display of COM, DCM and contact markers by default + # if the robot has freeflyer. + if robot.has_freeflyer: + if "display_com" not in viewer_kwargs: + viewer.display_center_of_mass(True) + if "display_dcm" not in viewer_kwargs: + viewer.display_capture_point(True) + if "display_contacts" not in viewer_kwargs: + viewer.display_contact_forces(True) + + # Enable display of external forces by default only for + # the joints having an external force registered to it. + if "display_f_external" not in viewer_kwargs: + profile_forces = self.engine.profile_forces[robot.name] + force_frames = set( + robot.pinocchio_model.frames[f.frame_index].parent + for f in profile_forces) + impulse_forces = self.engine.impulse_forces[robot.name] + force_frames |= set( + robot.pinocchio_model.frames[f.frame_index].parent + for f in impulse_forces) + visibility = viewer._display_f_external + assert isinstance(visibility, list) + for i in force_frames: + visibility[i - 1] = True + viewer.display_external_forces(visibility) # Initialize camera pose - if self.viewer.is_backend_parent and camera_pose is None: + assert self.viewer is not None + if viewer.is_backend_parent and camera_pose is None: camera_pose = viewer_kwargs.get("camera_pose", ( (9.0, 0.0, 2e-5), (np.pi/2, 0.0, np.pi/2), None)) @@ -603,7 +789,8 @@ def render(self, if update_ground_profile: engine_options = self.engine.get_options() ground_profile = engine_options["world"]["groundProfile"] - Viewer.update_floor(ground_profile, show_meshes=False) + Viewer.update_floor( + ground_profile, simplify_mesh=True, show_vertices=False) # Set the camera pose if requested if camera_pose is not None: @@ -614,7 +801,8 @@ def render(self, Viewer.open_gui() # Try refreshing the viewer - self.viewer.refresh() + for viewer in self._viewers: + viewer.refresh() # Compute and return rgb array if needed if return_rgb_array: @@ -629,9 +817,17 @@ def replay(self, **kwargs: Any) -> None: """Replay the current episode until now. + .. warning:: + Method only supported for single-robot simulations. + :param kwargs: Extra keyword arguments for delegation to `replay.play_trajectories` method. """ + # Make sure that the simulation is single-robot + if len(self.engine.robots) > 1: + raise NotImplementedError( + "This method is only supported for single-robot simulations.") + # Close extra viewer instances if any for viewer in self._viewers[1:]: viewer.delete_robot_on_close = True @@ -639,7 +835,7 @@ def replay(self, # Extract log data and robot from extra log files robots = [self.robot] - logs_data = [self.log_data] + logs_data = [self.engine.log_data] for log_file in extra_logs_files: log_data = read_log(log_file) robot = build_robot_from_log( @@ -696,9 +892,10 @@ def replay(self, def close(self) -> None: """Close the connection with the renderer. """ - if hasattr(self, "viewer") and self.viewer is not None: - self.viewer.close() - self.viewer = None + if hasattr(self, "_viewers"): + for viewer in self._viewers: + viewer.close() + self._viewers.clear() if hasattr(self, "figure") and self._figure is not None: self._figure.close() self._figure = None @@ -717,8 +914,11 @@ def plot(self, - Subplots with motors torques - Subplots with raw sensor data (one tab for each type of sensor) + .. warning:: + Method only supported for single-robot simulations. + :param enable_flexiblity_data: - Enable display of flexible joints in robot's configuration, + Enable display of flexibility joints in robot's configuration, velocity and acceleration subplots. Optional: False by default. :param block: Whether to wait for the figure to be closed before @@ -726,6 +926,11 @@ def plot(self, Optional: False in interactive mode, True otherwise. :param kwargs: Extra keyword arguments to forward to `TabbedFigure`. """ + # Make sure that the simulation is single-robot + if len(self.engine.robots) > 1: + raise NotImplementedError( + "This method is only supported for single-robot simulations.") + # Make sure plot submodule is available try: # pylint: disable=import-outside-toplevel @@ -741,53 +946,32 @@ def plot(self, return self._figure - def get_options(self) -> Dict[str, Dict[str, Dict[str, Any]]]: - """Get the options of the engine plus the robot (including controller). - """ - return {'engine': self.engine.get_options(), - 'robot': self.robot.get_options()} - - def set_options(self, - options: Dict[str, Dict[str, Dict[str, Any]]]) -> None: - """Set the options of robot (including controller), and engine. - """ - self.engine.set_options(options['engine']) - self.robot.set_options(options['robot']) - - def export_options(self, - config_path: Optional[Union[str, os.PathLike]] = None - ) -> None: - """Export the full configuration, ie the options of the robot ( - including controller), and the engine. + def export_options(self, config_path: Union[str, os.PathLike]) -> None: + """Export in a single configuration file all the options of the + simulator, ie the engine and all the robots. .. note:: - the configuration can be imported thereafter using `import_options` - method. + The generated configuration file can be imported thereafter using + `import_options` method. + + :param config_path: Full path of the location where to store the + generated file. The extension '.toml' will be + enforced. """ - if config_path is None: - if isinstance(self.robot, BaseJiminyRobot): - urdf_path = self.robot._urdf_path_orig - else: - urdf_path = self.robot.urdf_path - if not urdf_path: - raise ValueError( - "'config_path' must be provided if the robot is not " - "associated with any URDF.") - config_path = str(pathlib.Path( - urdf_path).with_suffix('')) + '_options.toml' + config_path = pathlib.Path(config_path).with_suffix('.toml') with open(config_path, 'w') as f: toml.dump( - self.get_options(), f, encoder=toml.TomlNumpyEncoder()) + self.get_simulation_options(), f, toml.TomlNumpyEncoder()) - def import_options(self, - config_path: Optional[Union[str, os.PathLike]] = None - ) -> None: - """Import the full configuration, ie the options of the robot ( - including controller), and the engine. + def import_options(self, config_path: Union[str, os.PathLike]) -> None: + """Import all the options of the simulator at once, ie the engine + and all the robots. .. note:: - Configuration can be exported beforehand using `export_options` - method. + A full configuration file can be exported beforehand using + `export_options` method. + + :param config_path: Full path of the configuration file to load. """ def deep_update(original: Dict[str, Any], new_dict: Dict[str, Any], @@ -814,19 +998,6 @@ def deep_update(original: Dict[str, Any], original[key] = new_dict[key] return original - if config_path is None: - if isinstance(self.robot, BaseJiminyRobot): - urdf_path = self.robot._urdf_path_orig - else: - urdf_path = self.robot.urdf_path - if not urdf_path: - raise ValueError( - "'config_path' must be provided if the robot is not " - "associated with any URDF.") - config_path = str(pathlib.Path( - urdf_path).with_suffix('')) + '_options.toml' - if not os.path.exists(config_path): - return - - options = deep_update(self.get_options(), toml.load(str(config_path))) - self.set_options(options) + options = deep_update( + self.get_simulation_options(), toml.load(str(config_path))) + self.set_simulation_options(options) diff --git a/python/jiminy_py/src/jiminy_py/viewer/__init__.py b/python/jiminy_py/src/jiminy_py/viewer/__init__.py index be99c26db..9d8a7c096 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/__init__.py +++ b/python/jiminy_py/src/jiminy_py/viewer/__init__.py @@ -1,5 +1,6 @@ # pylint: disable=missing-module-docstring -from .viewer import (CameraPoseType, +from .viewer import (COLORS, + CameraPoseType, ViewerClosedError, Viewer, sleep, @@ -14,6 +15,7 @@ __all__ = [ + 'COLORS', 'CameraPoseType', 'ViewerClosedError', 'sleep', diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py index 340b3c069..8ab567ff3 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py @@ -194,8 +194,8 @@ def update_floor(viewer: meshcat.Visualizer, # Convert provided geometry in Meshcat-specific triangle-based geometry vertices, faces = extract_vertices_and_faces_from_geometry(geom) obj = TriangularMeshGeometry(vertices, faces) - material = meshcat.geometry.MeshLambertMaterial() - material.color = (255 << 16) + (255 << 8) + 255 + material = meshcat.geometry.MeshPhongMaterial() + material.color = (155 << 16) + (155 << 8) + 180 # Disable newly created custom ground profile and hide original flat ground viewer["Ground"].set_object(obj, material) 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 6597e9e82..78e5e8f67 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 @@ -18,15 +18,13 @@ import multiprocessing as mp import xml.etree.ElementTree as ET from functools import wraps -from itertools import chain from datetime import datetime from types import TracebackType from traceback import TracebackException from pathlib import PureWindowsPath from contextlib import AbstractContextManager from typing import ( - Dict, Any, Callable, Optional, Tuple, Union, Sequence, Iterable, Literal, - Type) + Dict, Any, List, Callable, Optional, Tuple, Union, Sequence, Literal, Type) import numpy as np @@ -927,7 +925,7 @@ def _make_axes(self) -> NodePath: def _make_floor(self, geom: Optional[Geom] = None, - show_meshes: bool = False) -> NodePath: + show_vertices: bool = False) -> NodePath: model = GeomNode('floor') node = self.render.attach_new_node(model) @@ -945,7 +943,7 @@ def _make_floor(self, else: model.add_geom(geom) node.set_color((0.75, 0.75, 0.85, 1.0)) - if show_meshes: + if show_vertices: render_attrib = node.get_state().get_attrib_def( RenderModeAttrib.get_class_slot()) node.set_attrib(RenderModeAttrib.make( @@ -984,7 +982,7 @@ def show_floor(self, show: bool) -> None: def update_floor(self, geom: Optional[Geom] = None, - show_meshes: bool = False) -> NodePath: + show_vertices: bool = False) -> NodePath: """Update the floor. :param geom: Ground profile as a generic geometry object. If None, then @@ -996,7 +994,7 @@ def update_floor(self, # Remove existing floor and create a new one self._floor.remove_node() - self._floor = self._make_floor(geom, show_meshes) + self._floor = self._make_floor(geom, show_vertices) # Hide the floor if is was previously hidden if is_hidden: @@ -1145,8 +1143,8 @@ def append_arrow(self, head_geom.add_geom(head) head_node = NodePath(head_geom) head_node.reparent_to(arrow_node.attach_new_node("head")) - head_node.set_scale(1.75, 1.75, 3.5 * radius) - head_node.set_pos(0.0, 0.0, length) + head_node.set_scale(1.75, 1.75, 3.5 * radius * np.sign(length)) + head_node.set_pos(0.0, 0.0, 0.0 if anchor_top else length) body = geometry.make_cylinder() body_geom = GeomNode("body") body_geom.add_geom(body) @@ -1772,13 +1770,13 @@ def __setstate__(self, state: Dict[str, Any]) -> None: """ self.__dict__.update(state) - def __dir__(self) -> Iterable[str]: + def __dir__(self) -> List[str]: """Attribute lookup. It is mainly used by autocomplete feature of Ipython. It is overloaded to get consistent autocompletion wrt `getattr`. """ - return chain(super().__dir__(), dir(Panda3dApp)) + return [*super().__dir__(), *dir(Panda3dApp)] def async_mode(self) -> AbstractContextManager: """Context specifically designed for executing methods asynchronously. @@ -1964,13 +1962,13 @@ def __getattr__(self, name: str) -> Any: """ return getattr(self.__getattribute__('_app'), name) - def __dir__(self) -> Iterable[str]: + def __dir__(self) -> List[str]: """Attribute lookup. It is mainly used by autocomplete feature of Ipython. It is overloaded to get consistent autocompletion wrt `getattr`. """ - return chain(super().__dir__(), dir(self._app)) + return [*super().__dir__(), *dir(self._app)] def convert_bvh_collision_geometry_to_primitive(geom: hppfcl.CollisionGeometry @@ -1987,14 +1985,17 @@ def convert_bvh_collision_geometry_to_primitive(geom: hppfcl.CollisionGeometry if len(faces) == 0: return None - # Define normal to vertices as the average normal of adjacent triangles + # Define normal to vertices as the average normal of adjacent triangles, + # weigthed by their surface area: https://iquilezles.org/articles/normals/ fnormals = np.cross(vertices[faces[:, 2]] - vertices[faces[:, 1]], vertices[faces[:, 0]] - vertices[faces[:, 1]]) - fnormals /= np.linalg.norm(fnormals, axis=0) normals = np.zeros((len(vertices), 3), dtype=np.float32) for i in range(3): - normals[faces[:, i]] += fnormals - normals /= np.linalg.norm(normals, axis=0) + # Must use `np.add.at` which is unbuffered unlike `+=`, otherwise + # accumulation will not work properly as there are repeated indices. + np.add.at(normals, faces[:, i], fnormals) + scale = np.linalg.norm(normals, axis=1) + normals[scale > 0.0] /= scale[scale > 0.0, np.newaxis] # Create primitive triangle geometry vformat = GeomVertexFormat() diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py index 68cd8e9b2..9a2d34b78 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/replay.py +++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py @@ -344,10 +344,11 @@ def play_trajectories( backend = get_default_backend() assert backend is not None - # Always close viewer if gui is open if necessary for efficiency + # Always close viewers if gui is open if necessary for efficiency if (backend.startswith("panda3d") and record_video_path is not None and Viewer.has_gui()): Viewer.close() + viewers = None # Create a temporary video if the backend is 'panda3d-sync', no # 'record_video_path' is provided, and running in interactive mode @@ -678,7 +679,8 @@ def play_trajectories( # of the whole recording process (~75% on discrete gpu). buffer = Viewer.capture_frame( *record_video_size, raw_data=True) - memoryview(frame.planes[0])[:] = buffer + memoryview( + frame.planes[0])[:] = buffer # type: ignore[arg-type] # Write frame for packet in stream.encode(frame): @@ -869,8 +871,8 @@ def play_logs_files(logs_files: Union[str, Sequence[str]], **kwargs: Any) -> Sequence[Viewer]: """Play the content of a logfile in a viewer. - This method reconstruct the exact model used during the simulation - corresponding to the logfile, including random biases or flexible joints. + This method reconstruct the exact extended model used during the simulation + associated with the logfile, including random biases or flexibility joints. :param logs_files: Either a single simulation log files in any format, or a list. diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 3271bb288..615c760de 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -463,11 +463,13 @@ def __init__(self, # pylint: disable=unused-argument **kwargs: Any): """ :param robot: Jiminy.Model to display. - :param use_theoretical_model: Whether to use the theoretical (rigid) - model or the actual (flexible) model of - this robot. Note that using the actual - model is more efficient since update of - the frames placements can be skipped. + :param use_theoretical_model: Whether to use the theoretical rigid or + extended simulation model of this robot. + Note that using the extended model if + possible is more efficient since update + of the frames placements can be skipped + as it is already synchronized with the + simulation state. Optional: Actual model by default. :param robot_color: Color of the robot. It will override the original color of the meshes if not `None`. It supports both @@ -1055,8 +1057,7 @@ def is_alive() -> bool: return Viewer._backend_proc is not None and \ Viewer._backend_proc.is_alive() - def is_open( - self: Optional[Union["Viewer", Type["Viewer"]]] = None) -> bool: + def is_open(self: Optional["Viewer"] = None) -> bool: """Check if a given viewer instance is open, or if the backend server is running if no instance is specified. """ @@ -1066,7 +1067,7 @@ def is_open( return is_open_ @_with_lock - def close(self: Optional[Union["Viewer", Type["Viewer"]]] = None) -> None: + def close(self: Optional["Viewer"] = None) -> None: """Close a given viewer instance, or all of them if no instance is specified. @@ -1079,13 +1080,8 @@ def close(self: Optional[Union["Viewer", Type["Viewer"]]] = None) -> None: than calling this method without specifying any viewer instance. """ # pylint: disable=unsubscriptable-object - if self is None: - self = Viewer # pylint: disable=self-cls-assignment - - # Assert for type checker - assert self is not None - if self is Viewer: + if self is None: # NEVER closing backend automatically if closing instances, # even for the parent. It will be closed at Python exit # automatically. One must call `Viewer.close` to do otherwise. @@ -1159,8 +1155,8 @@ def close(self: Optional[Union["Viewer", Type["Viewer"]]] = None) -> None: except FileNotFoundError: pass - # At this point, consider the viewer has been closed, no matter what - self.__is_open = False + # At this point, consider the viewer has been closed no matter what + self.__is_open = False @staticmethod @_with_lock @@ -1469,8 +1465,7 @@ def get_camera_transform() -> Tuple[Tuple3FType, Tuple3FType]: @_with_lock @_must_be_open - def set_camera_transform(self: Optional[ - Union["Viewer", Type["Viewer"]]] = None, + def set_camera_transform(self: Optional["Viewer"] = None, position: Optional[Tuple3FType] = None, rotation: Optional[Tuple3FType] = None, relative: Optional[Union[str, int]] = None, @@ -1788,8 +1783,8 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunction] = None, x_range: Tuple[float, float] = (-10.0, 10.0), y_range: Tuple[float, float] = (-10.0, 10.0), grid_unit: Tuple[float, float] = (0.04, 0.04), - simplify_meshes: bool = False, - show_meshes: bool = False) -> None: + simplify_mesh: bool = False, + show_vertices: bool = False) -> None: """Display a custom ground profile as a height map or the original tile ground floor. @@ -1797,12 +1792,23 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunction] = None, with the ground profile. It renders a flat tile ground if not specified. Optional: None by default. - :param grid_size: X and Y dimension of the ground profile to render. - Optional: 20m by default. - :param grid_unit: X and Y discretization step of the ground profile. + :param x_range: Tuple gathering min and max limits along x-axis for + which the heightmap mesh associated with the ground + profile will be procedurally generated. + Optional: (-10.0, 10.0) by default. + :param y_range: Tuple gathering min and max limits along y-axis. + Optional: (-10.0, 10.0) by default. + :param grid_unit: Tuple gathering X and Y discretization steps for the + generation of the heightmap mesh. Optional: 4cm by default. - :param show_meshes: Whether to highlight the meshes. - Optional: disabled by default. + :param simplify_mesh: Whether the generated heightmap mesh should be + decimated before final rendering. This option + must be enabled for the ratio between grid size + and unit is very large to avoid a prohibitive + slowdown of the viewer. + Optional: False by default + :param show_vertices: Whether to highlight the mesh vertices. + Optional: disabled by default. """ # pylint: disable=import-outside-toplevel @@ -1815,7 +1821,7 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunction] = None, if ground_profile is not None: geom = discretize_heightmap( ground_profile, *x_range, grid_unit[0], *y_range, grid_unit[1], - must_simplify=simplify_meshes) + must_simplify=simplify_mesh) # Render original flat tile ground if possible. # TODO: Improve this check using LocalAABB box geometry instead. @@ -1830,7 +1836,7 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunction] = None, obj = None if geom is not None: obj = convert_bvh_collision_geometry_to_primitive(geom) - Viewer._backend_obj.gui.update_floor(obj, show_meshes) + Viewer._backend_obj.gui.update_floor(obj, show_vertices) else: from .meshcat.meshcat_visualizer import ( update_floor as meshcat_update_floor) diff --git a/python/jiminy_py/unit_py/data/simple_pendulum.urdf b/python/jiminy_py/unit_py/data/simple_pendulum.urdf index 948a4771f..865854a53 100644 --- a/python/jiminy_py/unit_py/data/simple_pendulum.urdf +++ b/python/jiminy_py/unit_py/data/simple_pendulum.urdf @@ -34,5 +34,14 @@ + + + + + + + + + diff --git a/python/jiminy_py/unit_py/test_dense_pole.py b/python/jiminy_py/unit_py/test_dense_pole.py index 3322b2ce5..0a32d8106 100644 --- a/python/jiminy_py/unit_py/test_dense_pole.py +++ b/python/jiminy_py/unit_py/test_dense_pole.py @@ -41,17 +41,16 @@ def setUp(self): model_options['joints']['positionLimitMin'] = [-self.joint_limit] model_options['joints']['positionLimitMax'] = [self.joint_limit] model_options['joints']['positionLimitFromUrdf'] = False - model_options['joints']['enablePositionLimit'] = True model_options['joints']['enableVelocityLimit'] = False self.robot.set_model_options(model_options) # Configure the integrator - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options['stepper']['tolAbs'] = 1e-9 engine_options['stepper']['tolRel'] = 1e-8 engine_options['constraints']['regularization'] = 0.0 engine_options['contacts']['transitionEps'] = self.transition_eps - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def test_flex_model(self): """Test if the result is the same with true and virtual inertia in @@ -67,9 +66,9 @@ def test_flex_model(self): self.robot.add_constraint("fixed_joint", const) # Configure the engine - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options['stepper']['sensorsUpdatePeriod'] = step_dt - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) # Extract some proxies for convenience pinocchio_model_th = self.robot.pinocchio_model_th @@ -95,7 +94,7 @@ def test_flex_model(self): # Specify flexibility options model_options = self.robot.get_model_options() - model_options['dynamics']['enableFlexibleModel'] = True + model_options['dynamics']['enableFlexibility'] = True flex_options = [{ 'frameName': self.flex_joint_name, 'stiffness': np.full((3,), self.flex_stiffness), @@ -123,22 +122,24 @@ def test_flex_model(self): q_flex = np.stack(extract_variables_from_log(log_vars, [ f"currentPosition{self.flex_joint_name}Quat{e}" for e in ('X', 'Y', 'Z', 'W') - ], 'HighLevelController'), axis=0) + ]), axis=0) twist_flex = 2 * np.arctan2(q_flex[2], q_flex[3]) twist_flex_all.append(twist_flex) assert np.allclose(*twist_flex_all, atol=1e-7) - # Extract parameters of theoretical dynamics equation + # Extract parameters of rigid-body dynamics equation + q_flex_init = self.robot.get_extended_position_from_theoretical(q_init) + v_flex_init = self.robot.get_extended_velocity_from_theoretical(v_init) update_quantities( - self.robot, q_init, v_init, use_theoretical_model=True) + self.robot, q_flex_init, v_flex_init, use_theoretical_model=False) inertia = self.robot.pinocchio_data.Ycrb[2] m = inertia.mass g = - self.robot.pinocchio_model.gravity.linear[2] l = inertia.lever[0] I_equiv = inertia.inertia[2, 2] + m * l ** 2 - # Integrate theoretical model + # Integrate rigid-body model theta_all, dtheta = [0.0,], 0.0 for _ in range(int(np.round(t_end / step_dt))): theta = theta_all[-1] @@ -163,11 +164,11 @@ def test_joint_position_limits(self): theta_all = [] for contact_model in ('constraint', 'spring_damper'): # Configure the engine - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options['stepper']['odeSolver'] = 'euler_explicit' engine_options['stepper']['dtMax'] = step_dt engine_options['contacts']['model'] = contact_model - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) # Start the simulation self.simulator.start(np.array((0.0,)), np.array((1.0,))) @@ -207,8 +208,7 @@ def test_joint_position_limits(self): log_vars = self.simulator.log_data["variables"] (theta,) = extract_variables_from_log( log_vars, - (f"currentPosition{self.robot.pinocchio_model.names[-1]}",), - 'HighLevelController') + (f"currentPosition{self.robot.pinocchio_model.names[-1]}",)) theta_all.append(theta) assert np.allclose(*theta_all, atol=1e-4) diff --git a/python/jiminy_py/unit_py/test_flexible_arm.py b/python/jiminy_py/unit_py/test_flexible_arm.py index 60a730c09..3d5b2964c 100644 --- a/python/jiminy_py/unit_py/test_flexible_arm.py +++ b/python/jiminy_py/unit_py/test_flexible_arm.py @@ -152,13 +152,13 @@ def test_write_replay_standalone_log(self): """Check if reading/writing standalone log file is working. """ # Configure log file to be standalone - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options['telemetry']['isPersistent'] = True - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) # Specify joint flexibility parameters model_options = self.simulator.robot.get_model_options() - model_options['dynamics']['enableFlexibleModel'] = True + model_options['dynamics']['enableFlexibility'] = True model_options['dynamics']['flexibilityConfig'] = [{ 'frameName': f"link{i}_to_link{i+1}", 'stiffness': 10.0 * np.ones(3), @@ -197,7 +197,7 @@ def test_rigid_vs_flex_at_frame(self): for order in (range(N_FLEXIBILITY), range(N_FLEXIBILITY)[::-1]): # Specify joint flexibility parameters model_options = self.simulator.robot.get_model_options() - model_options['dynamics']['enableFlexibleModel'] = True + model_options['dynamics']['enableFlexibility'] = True model_options['dynamics']['flexibilityConfig'] = [{ 'frameName': f"link{i}_to_link{i+1}", 'stiffness': np.zeros(3), @@ -219,7 +219,7 @@ def test_rigid_vs_flex_at_frame(self): # Extract the final configuration q_flex.append( - self.simulator.robot.get_rigid_position_from_flexible( + self.simulator.robot.get_theoretical_position_from_extended( self.simulator.robot_state.q)) # Render the scene diff --git a/python/jiminy_py/unit_py/test_foot_pendulum.py b/python/jiminy_py/unit_py/test_foot_pendulum.py new file mode 100644 index 000000000..cfbdcafdd --- /dev/null +++ b/python/jiminy_py/unit_py/test_foot_pendulum.py @@ -0,0 +1,90 @@ +""" +@brief This file aims at verifying the sanity of the physics and the + integration method of jiminy on simple models. +""" +import os +import unittest + +import numpy as np + + +from jiminy_py.core import ( # pylint: disable=no-name-in-module + ForceSensor as force, ImuSensor as imu, ContactSensor as contact) +from jiminy_py.simulator import Simulator + + +# Small tolerance for numerical equality. +# The integration error is supposed to be bounded. +TOLERANCE = 1.0e-5 + + +class SimulateFootedPendulum(unittest.TestCase): + """Simulate the motion of a pendulum with a square foot having contact + points at each corner. + """ + def test_init_and_consistency(self): + """Verify that the pendulum holds straight without falling when + perfectly initialized at the unstable equilibrium point. + + This test also checks that there is no discontinuity at initialization + and that the various sensors are working properly. + """ + # Create the jiminy robot + current_dir = os.path.dirname(os.path.realpath(__file__)) + data_root_dir = os.path.join(current_dir, "data") + urdf_path = os.path.join(data_root_dir, "foot_pendulum.urdf") + hardware_path = os.path.join(data_root_dir, "foot_pendulum.toml") + simulator = Simulator.build( + urdf_path, hardware_path=hardware_path, has_freeflyer=True) + + # Set options + engine_options = simulator.get_options() + engine_options["telemetry"]["enableConfiguration"] = True + engine_options["stepper"]["odeSolver"] = "runge_kutta_4" + engine_options["stepper"]["dtMax"] = 1.0e-5 + engine_options["stepper"]["sensorsUpdatePeriod"] = 0.0 + engine_options["stepper"]["controllerUpdatePeriod"] = 1.0e-3 + engine_options["stepper"]["logInternalStepperSteps"] = True + engine_options['contacts']['model'] = "constraint" + engine_options['contacts']['stabilizationFreq'] = 0.0 + engine_options['constraints']['regularization'] = 1e-9 + simulator.set_options(engine_options) + + # Initialize the simulation + q0 = np.array([0.0, 0.0, 0.005, 0.0, 0.0, 0.0, 1.0, 0.0]) + v0 = np.zeros((simulator.robot.pinocchio_model.nv,)) + mass = simulator.robot.pinocchio_data.mass[0] + gravity = engine_options["world"]["gravity"] + simulator.start(q0, v0) + + self.assertTrue(np.all(np.abs(simulator.stepper_state.a) < TOLERANCE)) + imu_data = simulator.robot.sensor_measurements[imu.type, 'Foot'] + gyro_data, accel_data = np.split(imu_data, 2) + self.assertTrue(np.allclose(gyro_data, 0.0, atol=TOLERANCE)) + self.assertTrue(np.allclose(accel_data, -gravity[:3], atol=TOLERANCE)) + self.assertTrue(np.allclose( + simulator.robot.sensor_measurements[force.type, 'Foot'], + -gravity * mass, + atol=TOLERANCE)) + for i in range(3): + self.assertTrue(np.allclose( + simulator.robot.sensor_measurements[ + contact.type, f'Foot_CollisionBox_0_{2 * i}'], + simulator.robot.sensor_measurements[ + contact.type, f'Foot_CollisionBox_0_{2 * (i + 1)}'], + atol=TOLERANCE)) + with self.assertRaises(KeyError): + simulator.robot.sensor_measurements[contact.type, 'NA'] + with self.assertRaises(KeyError): + simulator.robot.sensor_measurements['NA', 'Foot_CollisionBox_0_1'] + + # Simulate for a few seconds + simulator.step(1.0) + simulator.stop() + + self.assertTrue(np.all(np.abs(simulator.stepper_state.v) < TOLERANCE)) + self.assertTrue(np.all(np.abs(simulator.stepper_state.a) < TOLERANCE)) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/jiminy_py/unit_py/test_simple_mass.py b/python/jiminy_py/unit_py/test_simple_mass.py index dc4d4beb8..85ce421e1 100644 --- a/python/jiminy_py/unit_py/test_simple_mass.py +++ b/python/jiminy_py/unit_py/test_simple_mass.py @@ -128,7 +128,7 @@ def _test_collision_and_contact_dynamics(self, shape): # Set some extra options of the engine, to avoid assertion failure # because of problem regularization and outliers engine_options = engine.get_options() - engine_options['contacts']['transitionEps'] = 1.0e-6 + engine_options["contacts"]["transitionEps"] = 1.0e-6 engine_options["stepper"]["controllerUpdatePeriod"] = self.dtMax engine.set_options(engine_options) @@ -144,7 +144,7 @@ def _test_collision_and_contact_dynamics(self, shape): # Total energy and derivative log_vars = engine.log_data["variables"] - E_robot = log_vars['HighLevelController.energy'] + E_robot = log_vars["energy"] E_contact = 1 / 2 * self.k_contact * penetration_depth ** 2 E_tot = E_robot + E_contact E_diff_robot = np.concatenate(( @@ -278,8 +278,7 @@ def _test_friction_model(self, shape): # Validate the stiction model: check the transition between dry and # viscous friction because of stiction phenomena. log_vars = engine.log_data["variables"] - acceleration = log_vars[ - 'HighLevelController.currentFreeflyerAccelerationLinX'] + acceleration = log_vars["currentFreeflyerAccelerationLinX"] jerk = np.diff(acceleration) / np.diff(time) snap = np.diff(jerk) / np.diff(time[1:]) snap_rel = np.abs(snap / np.max(snap)) @@ -303,7 +302,7 @@ def _test_friction_model(self, shape): # Check that the energy increases only when the force is applied tolerance_E = 1e-9 - E_robot = log_vars['HighLevelController.energy'] + E_robot = log_vars["energy"] E_diff_robot = np.concatenate(( np.diff(E_robot) / np.diff(time), np.zeros((1,), dtype=E_robot.dtype))) diff --git a/python/jiminy_py/unit_py/test_simple_pendulum.py b/python/jiminy_py/unit_py/test_simple_pendulum.py index 26c3728f1..d74e3b868 100644 --- a/python/jiminy_py/unit_py/test_simple_pendulum.py +++ b/python/jiminy_py/unit_py/test_simple_pendulum.py @@ -83,29 +83,26 @@ def _simulate_and_get_imu_data_evolution( # Extract state evolution over time time = log_vars['Global.Time'] + imu_jiminy = np.stack([ + log_vars['.'.join((jiminy.ImuSensor.type, 'PendulumLink', field))] + for field in jiminy.ImuSensor.fieldnames], axis=-1) + + # Split IMU data if requested if split: - gyro_jiminy = np.stack([ - log_vars['PendulumLink.Gyro' + s] for s in ['x', 'y', 'z'] - ], axis=-1) - accel_jiminy = np.stack([ - log_vars['PendulumLink.Accel' + s] for s in ['x', 'y', 'z'] - ], axis=-1) + gyro_jiminy, accel_jiminy = np.split(imu_jiminy, 2, axis=-1) return time, gyro_jiminy, accel_jiminy - else: - imu_jiminy = np.stack([ - log_vars['PendulumLink.' + f] - for f in jiminy.ImuSensor.fieldnames], axis=-1) - return time, imu_jiminy + + return time, imu_jiminy def test_armature(self): """Verify the dynamics of the system when adding rotor inertia. """ # Configure the robot: set rotor inertia J = 0.1 - motor_options = self.robot.get_motors_options() - motor_options["PendulumJoint"]['enableArmature'] = True - motor_options["PendulumJoint"]['armature'] = J - self.robot.set_motors_options(motor_options) + robot_options = self.robot.get_options() + robot_options["motors"]["PendulumJoint"]['enableArmature'] = True + robot_options["motors"]["PendulumJoint"]['armature'] = J + self.robot.set_options(robot_options) # Dynamics: simulate a spring of stiffness k k_spring = 500.0 @@ -169,6 +166,99 @@ def dynamics(t, x): # using Scipy self.assertTrue(np.allclose(x_jiminy, x_rk_python, atol=TOLERANCE)) + def test_backlash(self): + """Test adding a backlash to a pendulum, and make sure that: + - while 'inside' the backlash, the pendulum behaves like no motor is + present + - once the backlash limit is reached, the pendulum behaves like a + single body, with rotor and body inertia summed up. + """ + # Configure the robot: add rotor inertia and backlash + J = 1.0 + BACKLASH = 1.1 + robot_options = self.robot.get_options() + robot_options["motors"]["PendulumJoint"]['enableArmature'] = True + robot_options["motors"]["PendulumJoint"]['armature'] = J + robot_options["motors"]["PendulumJoint"]["enableBacklash"] = True + robot_options["motors"]["PendulumJoint"]["backlash"] = 2 * BACKLASH + self.robot.set_options(robot_options) + + TAU = 5.0 + def ControllerConstant(t, q, v, sensor_measurements, command): + command[:] = - TAU + + engine = jiminy.Engine() + setup_controller_and_engine( + engine, self.robot, compute_command=ControllerConstant) + + engine_options = engine.get_options() + engine_options["constraints"]["regularization"] = 0.0 + engine.set_options(engine_options) + + # Run simulation and extract log data + x0 = np.array([0.0, 0.1, 0.0, 0.0]) + tf = 5.0 + time, x_jiminy = simulate_and_get_state_evolution( + engine, tf, x0, split=False) + + # Now we compare the two phases of the motion: + # - first phase: before impact: both bodies move independently + # - second phase: after impact: both bodies move as one (a tolerance + # of 0.4s is applied to make sure all rebounds are gone) + t_impact = np.sqrt(BACKLASH / (TAU / J) * 2) + t1, t2 = np.searchsorted(time, [t_impact - 0.02, t_impact + 0.4]) + time_phase_1 = time[:t1] + x_jiminy_phase_1 = x_jiminy[:t1] + def dynamics(t, x): + # The angle of the pendulum mass is actually x[0] + x[1], not x[1] + return np.array([x[2], + x[3], + -TAU / J, + self.g / self.l * np.sin(x[0] + x[1]) + TAU / J]) + x_rk_python = integrate_dynamics(time_phase_1, x0, dynamics) + self.assertTrue(np.allclose( + x_jiminy_phase_1, x_rk_python, atol=TOLERANCE)) + + time_phase_2 = time[t2:] - time[t2] + x_jiminy_phase_2 = x_jiminy[t2:] + + I_total = self.m * self.l ** 2 + J + G = self.m * self.g * self.l / I_total + def dynamics(t, x): + acc = G * np.sin(x[0] + x[1]) - TAU / I_total + return np.array([x[2], x[3], acc, 0.0]) + x_rk_python = integrate_dynamics( + time_phase_2, x_jiminy_phase_2[0], dynamics) + self.assertTrue(np.allclose( + x_jiminy_phase_2, x_rk_python, atol=TOLERANCE)) + + # Simulate with more damping on the constraint to prevent bouncing. + # Note that this needs to be changed after the simulation start. + engine_options["stepper"]["dtMax"] = 0.001 + engine.set_options(engine_options) + engine.start(x0[:self.robot.nq], x0[-self.robot.nv:]) + self.robot.constraints.bounds_joints["PendulumJointBacklash"].kp = 1e5 + self.robot.constraints.bounds_joints["PendulumJointBacklash"].kd = 1e3 + engine.step(t_impact + 1.0) + log_vars = engine.log_data["variables"] + time = log_vars['Global.Time'] + q_jiminy = np.stack([ + log_vars[field] for field in self.robot.log_position_fieldnames + ], axis=-1) + v_jiminy = np.stack([ + log_vars[field] for field in self.robot.log_velocity_fieldnames + ], axis=-1) + x_jiminy = np.concatenate((q_jiminy, v_jiminy), axis=-1) + + # Ignoring some uncertainty at impact time due to the pendulum motion + t2 = np.searchsorted(time, t_impact + 0.20) + time_phase_2 = time[t2:] - time[t2] + x_jiminy_phase_2 = x_jiminy[t2:] + x_rk_python = integrate_dynamics( + time_phase_2, x_jiminy_phase_2[0], dynamics) + self.assertTrue(np.allclose( + x_jiminy_phase_2, x_rk_python, atol=TOLERANCE)) + def test_imu_sensor(self): """Test IMU sensor on pendulum motion. @@ -486,7 +576,7 @@ def test_flexibility_armature(self): # Enable flexibility model_options = self.robot.get_model_options() - model_options["dynamics"]["enableFlexibleModel"] = True + model_options["dynamics"]["enableFlexibility"] = True model_options["dynamics"]["flexibilityConfig"] = [{ 'frameName': "PendulumJoint", 'stiffness': k * np.ones(3), @@ -496,19 +586,19 @@ def test_flexibility_armature(self): self.robot.set_model_options(model_options) # Enable rotor inertia - motor_options = self.robot.get_motors_options() - motor_options["PendulumJoint"]['enableArmature'] = True - motor_options["PendulumJoint"]['armature'] = J - self.robot.set_motors_options(motor_options) + robot_options = self.robot.get_options() + robot_options["motors"]["PendulumJoint"]['enableArmature'] = True + robot_options["motors"]["PendulumJoint"]['armature'] = J + self.robot.set_options(robot_options) # Create an engine: PD controller on motor and no internal dynamics k_control, nu_control = 100.0, 1.0 - def ControllerPD(t, q, v, sensor_measurements, command): + def compute_command(t, q, v, sensor_measurements, command): command[:] = - k_control * q[4] - nu_control * v[3] engine = jiminy.Engine() setup_controller_and_engine( - engine, self.robot, compute_command=ControllerPD) + engine, self.robot, compute_command=compute_command) # Configure the engine engine_options = engine.get_options() @@ -568,10 +658,10 @@ def test_fixed_body_constraint_armature(self): # Enable rotor inertia J = 0.1 - motor_options = robot.get_motors_options() - motor_options["PendulumJoint"]['enableArmature'] = True - motor_options["PendulumJoint"]['armature'] = J - robot.set_motors_options(motor_options) + robot_options = robot.get_options() + robot_options["motors"]["PendulumJoint"]['enableArmature'] = True + robot_options["motors"]["PendulumJoint"]['armature'] = J + robot.set_options(robot_options) # Set fixed body constraint. freeflyer_constraint = jiminy.FrameConstraint("world") @@ -622,5 +712,66 @@ def spring_force(t, q, v, sensor_measurements, u_custom): v_jiminy[:, -1], x_analytical[:, 1], atol=TOLERANCE)) + def test_flexibility_api(self): + """ + @brief Test the addition and disabling of a flexibility in the system. + + @details This function only tests that the flexibility API works, but + performs no validation of the physics behind it. + """ + + # Enable flexibility + model_options = self.robot.get_model_options() + model_options["dynamics"]["enableFlexibility"] = True + model_options["dynamics"]["flexibilityConfig"] = [{ + 'frameName': "PendulumJoint", + 'stiffness': np.ones(3), + 'damping': np.ones(3), + 'inertia': np.ones(3) + }] + self.robot.set_model_options(model_options) + self.assertTrue(self.robot.flexibility_joint_indices == [1]) + + engine = jiminy.Engine() + engine.add_robot(self.robot) + + x0 = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]) + tf = 0.1 + simulate_and_get_state_evolution(engine, tf, x0, split=False) + + self.assertTrue(self.robot.flexibility_joint_indices == [1]) + + # Disable flexibility + model_options = self.robot.get_model_options() + model_options["dynamics"]["enableFlexibility"] = False + self.robot.set_model_options(model_options) + self.assertTrue(self.robot.flexibility_joint_indices == []) + x0 = np.array([0.0, 0.0]) + simulate_and_get_state_evolution(engine, tf, x0, split=False) + self.assertTrue(self.robot.flexibility_joint_indices == []) + + # Re-enable flexibility + model_options = self.robot.get_model_options() + model_options["dynamics"]["enableFlexibility"] = True + self.robot.set_model_options(model_options) + self.assertTrue(self.robot.flexibility_joint_indices == [1]) + + x0 = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]) + simulate_and_get_state_evolution(engine, tf, x0, split=False) + + self.assertTrue(self.robot.flexibility_joint_indices == [1]) + + # Test empty flexibility list + model_options = self.robot.get_model_options() + model_options["dynamics"]["enableFlexibility"] = True + model_options["dynamics"]["flexibilityConfig"] = [] + self.robot.set_model_options(model_options) + self.assertTrue(self.robot.flexibility_joint_indices == []) + + x0 = np.array([0.0, 0.0]) + simulate_and_get_state_evolution(engine, tf, x0, split=False) + + self.assertTrue(self.robot.flexibility_joint_indices == []) + if __name__ == '__main__': unittest.main() diff --git a/python/jiminy_py/unit_py/test_simulator.py b/python/jiminy_py/unit_py/test_simulator.py index 11fa7a3e6..87efed7d9 100644 --- a/python/jiminy_py/unit_py/test_simulator.py +++ b/python/jiminy_py/unit_py/test_simulator.py @@ -10,15 +10,16 @@ from jiminy_py.robot import BaseJiminyRobot from jiminy_py.simulator import Simulator -from jiminy_py.log import read_log, build_robot_from_log +from jiminy_py.log import (read_log, + extract_trajectory_from_log, + extract_trajectories_from_log) from jiminy_py.viewer.replay import play_logs_data class SimulatorTest(unittest.TestCase): def test_single_robot_simulation(self): - ''' - Test simulation with a single robot. - ''' + """Test simulation with a single robot. + """ # Set initial condition and simulation duration np.random.seed(0) q0, v0 = np.random.rand(2), np.random.rand(2) @@ -64,7 +65,14 @@ def test_single_robot_simulation(self): # Test: log reading log_data = read_log(log_path) - robot = build_robot_from_log(log_data) + trajectory = extract_trajectory_from_log(log_data) + + final_log_states = trajectory['evolution_robot'][-1] + + np.testing.assert_array_almost_equal( + simulator.robot_state.q, final_log_states.q, decimal=10) + np.testing.assert_array_almost_equal( + simulator.robot_state.v, final_log_states.v, decimal=10) # Test: replay from log video_path = os.path.join( @@ -74,6 +82,72 @@ def test_single_robot_simulation(self): robot, log_data, record_video_path=video_path, verbose=False) self.assertTrue(os.path.isfile(video_path)) + def test_double_robot_simulation(self): + """Test simulation with two robots. + """ + robot1_name, robot2_name = "robot1", "robot2" + # Set initial condition and simulation duration + np.random.seed(0) + q0 = {robot1_name : np.random.rand(2), robot2_name : np.random.rand(2)} + v0 = {robot1_name : np.random.rand(2), robot2_name : np.random.rand(2)} + t_end = 0.5 + + # Define URDF path + current_dir = os.path.dirname(os.path.realpath(__file__)) + data_root_dir = os.path.join(current_dir, "data") + urdf_path = os.path.join(data_root_dir, "double_pendulum.urdf") + + # Create robot1. + # The existing hardware file should be loaded automatically. + robot1 = BaseJiminyRobot(robot1_name) + robot1.initialize(urdf_path, has_freeflyer=False) + + # Create simulator. + # The existing options file should be loaded automatically. + simulator = Simulator(robot1, viewer_kwargs={"backend": "panda3d-sync"}) + + # Add robot2 to the simulation + simulator.add_robot(robot2_name, urdf_path, has_freeflyer=False) + + # Check synchronous rendering: + # simulation started before display and the other way around. + simulator.start(q0, v0) + simulator.render(return_rgb_array=True) + simulator.stop() + simulator.start(q0, v0) + simulator.stop() + simulator.close() + + # Run the simulation and write log + log_path = os.path.join( + tempfile.gettempdir(), + f"log_{next(tempfile._get_candidate_names())}.hdf5") + simulator.simulate( + t_end, q0, v0, log_path=log_path, show_progress_bar=False) + self.assertTrue(os.path.isfile(log_path)) + + # Test: viewer + with self.assertRaises(NotImplementedError): + simulator.replay(verbose=False) + + # Test: log reading + log_data = read_log(log_path) + trajectories = extract_trajectories_from_log(log_data) + + trajectory_1, trajectory_2 = ( + trajectories[robot.name]['evolution_robot'][-1] + for robot in simulator.robots) + robot_states_1, robot_states_2 = simulator.robot_states + + np.testing.assert_array_almost_equal( + robot_states_1.q, trajectory_1.q, decimal=10) + np.testing.assert_array_almost_equal( + robot_states_1.v, trajectory_1.v, decimal=10) + np.testing.assert_array_almost_equal( + robot_states_2.q, trajectory_2.q, decimal=10) + np.testing.assert_array_almost_equal( + robot_states_2.v, trajectory_2.v, decimal=10) + if __name__ == '__main__': unittest.main() diff --git a/python/jiminy_py/unit_py/test_utils.py b/python/jiminy_py/unit_py/test_utils.py new file mode 100644 index 000000000..6cbd0d6b8 --- /dev/null +++ b/python/jiminy_py/unit_py/test_utils.py @@ -0,0 +1,71 @@ +"""Check that jiminy core helpers are working as expected. +""" +import unittest + +import numpy as np + +import jiminy_py.core as jiminy + + +class UtilitiesTest(unittest.TestCase): + def test_array_copyto(self): + """Test `array_copyto` + """ + for cont in (True, False): + for dtype in (np.float64, np.float32): + # Vector + if cont: + a = np.zeros((3,), dtype=dtype) + else: + a = np.zeros((3, 3), dtype=dtype)[:, 0] + b = 1 + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.array([2], dtype=np.int64) + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.array(3, dtype=np.int64) + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.array([4.0, 5.0, 6.0], dtype=dtype) + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.array([7.0, 8.0, 9.0], dtype=np.int64) + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.zeros((3, 3))[:, 0] + jiminy.array_copyto(a, b) + assert np.all(a == b) + + # Matrix + for transpose in (False, True): + if transpose: + a = np.zeros((3, 5), dtype=dtype) + a = a.T + else: + a = np.zeros((5, 3), dtype=dtype) + b = 1 + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.array([2], dtype=np.int64) + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.array(3, dtype=np.int64) + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.array([4.0, 5.0, 6.0], dtype=dtype) + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.random.rand(*a.shape).astype(dtype=dtype) + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.random.rand(*a.shape[::-1]).astype(dtype=dtype).T + jiminy.array_copyto(a, b) + assert np.all(a == b) + b = np.random.randint(1000, size=a.shape, dtype=np.int64) + jiminy.array_copyto(a, b) + assert np.all(a == b) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/jiminy_py/unit_py/utilities.py b/python/jiminy_py/unit_py/utilities.py index a141113ee..e81d8dca1 100644 --- a/python/jiminy_py/unit_py/utilities.py +++ b/python/jiminy_py/unit_py/utilities.py @@ -49,15 +49,12 @@ def load_urdf_default(urdf_name: str, motor.initialize(joint_name) # Configure the robot - model_options = robot.get_model_options() - motor_options = robot.get_motors_options() - model_options["joints"]["enablePositionLimit"] = False - model_options["joints"]["enableVelocityLimit"] = False - for m in motor_options: - motor_options[m]['enableCommandLimit'] = False - motor_options[m]['enableArmature'] = False - robot.set_model_options(model_options) - robot.set_motors_options(motor_options) + robot_options = robot.get_options() + robot_options["model"]["joints"]["enableVelocityLimit"] = False + for name in robot.motor_names: + robot_options["motors"][name]['enableCommandLimit'] = False + robot_options["motors"][name]['enableArmature'] = False + robot.set_options(robot_options) return robot @@ -194,7 +191,8 @@ def simulate_and_get_state_evolution( """ # Run simulation if isinstance(x0, np.ndarray): - q0, v0 = x0[:engine.robots[0].nq], x0[-engine.robots[0].nv:] + robot, = engine.robots + q0, v0 = x0[:robot.nq], x0[-robot.nv:] else: q0, v0 = {}, {} for robot in engine.robots: @@ -208,29 +206,33 @@ def simulate_and_get_state_evolution( # Extract state evolution over time time = log_vars['Global.Time'] if isinstance(x0, np.ndarray): + robot, = engine.robots + q_jiminy = np.stack([ - log_vars['.'.join(['HighLevelController', s])] - for s in engine.robots[0].log_position_fieldnames], axis=-1) + log_vars[field] for field in robot.log_position_fieldnames + ], axis=-1) v_jiminy = np.stack([ - log_vars['.'.join(['HighLevelController', s])] - for s in engine.robots[0].log_velocity_fieldnames], axis=-1) + log_vars[field] for field in robot.log_velocity_fieldnames + ], axis=-1) + if split: return time, q_jiminy, v_jiminy - else: - x_jiminy = np.concatenate((q_jiminy, v_jiminy), axis=-1) - return time, x_jiminy + + x_jiminy = np.concatenate((q_jiminy, v_jiminy), axis=-1) + return time, x_jiminy else: q_jiminy = [np.stack([ - log_vars['.'.join(['HighLevelController', robot.name, s])] - for s in robot.log_position_fieldnames + log_vars['.'.join((robot.name, field))] + for field in robot.log_position_fieldnames ], axis=-1) for robot in engine.robots] v_jiminy = [np.stack([ - log_vars['.'.join(['HighLevelController', robot.name, s])] - for s in robot.log_velocity_fieldnames + log_vars['.'.join((robot.name, field))] + for field in robot.log_velocity_fieldnames ], axis=-1) for robot in engine.robots] + if split: return time, q_jiminy, v_jiminy - else: - x_jiminy = [np.concatenate((q, v), axis=-1) - for q, v in zip(q_jiminy, v_jiminy)] - return time, x_jiminy + + x_jiminy = [np.concatenate((q, v), axis=-1) + for q, v in zip(q_jiminy, v_jiminy)] + return time, x_jiminy diff --git a/python/jiminy_pywrap/CMakeLists.txt b/python/jiminy_pywrap/CMakeLists.txt index 7014c7b29..44b1588d1 100755 --- a/python/jiminy_pywrap/CMakeLists.txt +++ b/python/jiminy_pywrap/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 3.12.4) # Project name project(${LIBRARY_NAME}_pywrap VERSION ${BUILD_VERSION}) -# Find libraries and headers. +# Find libraries and headers find_package(eigenpy 2.9.0 REQUIRED NO_MODULE NO_CMAKE_SYSTEM_PATH) # >=2.9.0 fully support vectorization # Enable all warnings diff --git a/python/jiminy_pywrap/include/jiminy/python/compatibility.h b/python/jiminy_pywrap/include/jiminy/python/compatibility.h index 062b345af..af0b6f1ec 100644 --- a/python/jiminy_pywrap/include/jiminy/python/compatibility.h +++ b/python/jiminy_pywrap/include/jiminy/python/compatibility.h @@ -6,7 +6,7 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeCompatibility(); + void exposeCompatibility(); } #endif // COMPATIBILITY_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/constraints.h b/python/jiminy_pywrap/include/jiminy/python/constraints.h index cc69375ff..2b39bec1c 100644 --- a/python/jiminy_pywrap/include/jiminy/python/constraints.h +++ b/python/jiminy_pywrap/include/jiminy/python/constraints.h @@ -6,8 +6,8 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeConstraints(); - void JIMINY_DLLAPI exposeConstraintTree(); + void exposeConstraints(); + void exposeConstraintTree(); } #endif // CONSTRAINTS_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/controllers.h b/python/jiminy_pywrap/include/jiminy/python/controllers.h index 674fc7578..3bc7cb93a 100644 --- a/python/jiminy_pywrap/include/jiminy/python/controllers.h +++ b/python/jiminy_pywrap/include/jiminy/python/controllers.h @@ -6,8 +6,8 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeAbstractController(); - void JIMINY_DLLAPI exposeFunctionalController(); + void exposeAbstractController(); + void exposeFunctionalController(); } #endif // CONTROLLERS_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/engine.h b/python/jiminy_pywrap/include/jiminy/python/engine.h index 76699add7..c3b65883e 100644 --- a/python/jiminy_pywrap/include/jiminy/python/engine.h +++ b/python/jiminy_pywrap/include/jiminy/python/engine.h @@ -6,10 +6,10 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeForces(); - void JIMINY_DLLAPI exposeStepperState(); - void JIMINY_DLLAPI exposeRobotState(); - void JIMINY_DLLAPI exposeEngine(); + void exposeForces(); + void exposeStepperState(); + void exposeRobotState(); + void exposeEngine(); } #endif // SIMULATOR_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/generators.h b/python/jiminy_pywrap/include/jiminy/python/generators.h index ef5af4619..b91dfafc2 100644 --- a/python/jiminy_pywrap/include/jiminy/python/generators.h +++ b/python/jiminy_pywrap/include/jiminy/python/generators.h @@ -6,7 +6,7 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeGenerators(); + void exposeGenerators(); } #endif // GENERATORS_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/helpers.h b/python/jiminy_pywrap/include/jiminy/python/helpers.h index c22d60635..32b8dd844 100644 --- a/python/jiminy_pywrap/include/jiminy/python/helpers.h +++ b/python/jiminy_pywrap/include/jiminy/python/helpers.h @@ -6,7 +6,7 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeHelpers(); + void exposeHelpers(); } #endif // HELPERS_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/motors.h b/python/jiminy_pywrap/include/jiminy/python/motors.h index 9457de8eb..25339b3d0 100644 --- a/python/jiminy_pywrap/include/jiminy/python/motors.h +++ b/python/jiminy_pywrap/include/jiminy/python/motors.h @@ -6,8 +6,8 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeAbstractMotor(); - void JIMINY_DLLAPI exposeBasicMotors(); + void exposeAbstractMotor(); + void exposeBasicMotors(); } #endif // MOTORS_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/robot.h b/python/jiminy_pywrap/include/jiminy/python/robot.h index e04db1df0..b94898a39 100644 --- a/python/jiminy_pywrap/include/jiminy/python/robot.h +++ b/python/jiminy_pywrap/include/jiminy/python/robot.h @@ -6,8 +6,8 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeModel(); - void JIMINY_DLLAPI exposeRobot(); + void exposeModel(); + void exposeRobot(); } #endif // ROBOT_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/sensors.h b/python/jiminy_pywrap/include/jiminy/python/sensors.h index 2ab7e87ab..02cc6e122 100644 --- a/python/jiminy_pywrap/include/jiminy/python/sensors.h +++ b/python/jiminy_pywrap/include/jiminy/python/sensors.h @@ -6,9 +6,9 @@ namespace jiminy::python { - void JIMINY_DLLAPI exposeSensorMeasurementTree(); - void JIMINY_DLLAPI exposeAbstractSensor(); - void JIMINY_DLLAPI exposeBasicSensors(); + void exposeSensorMeasurementTree(); + void exposeAbstractSensor(); + void exposeBasicSensors(); } #endif // SENSORS_PYTHON_H diff --git a/python/jiminy_pywrap/include/jiminy/python/utilities.h b/python/jiminy_pywrap/include/jiminy/python/utilities.h index 1930733a9..bd367bb42 100644 --- a/python/jiminy_pywrap/include/jiminy/python/utilities.h +++ b/python/jiminy_pywrap/include/jiminy/python/utilities.h @@ -19,17 +19,14 @@ #include -#define EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(type) \ - namespace boost::python::converter \ - { \ - template<> \ - struct expected_pytype_for_arg \ - { \ - static const PyTypeObject * get_pytype() \ - { \ - return &PyArray_Type; \ - } \ - }; \ +#define EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(type) \ + namespace boost::python::converter \ + { \ + template<> \ + struct expected_pytype_for_arg \ + { \ + static const PyTypeObject * get_pytype() { return &PyArray_Type; } \ + }; \ } EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(numpy::ndarray) @@ -238,7 +235,7 @@ namespace jiminy::python doc, std::pair{"fget", getMemberFuncPtr}, std::pair{"fset", setMemberFuncPtr}); } - // clang-format off +// clang-format off #define DEF_READONLY3(namePy, memberFuncPtr, doc) \ def_readonly(namePy, \ memberFuncPtr, \ @@ -335,7 +332,7 @@ namespace jiminy::python [[noreturn]] static bool contains(Container & /* container */, const typename Container::value_type & /* key */) { - THROW_ERROR(not_implemented_error, "Contains method not supported."); + JIMINY_THROW(not_implemented_error, "Contains method not supported."); } }; @@ -562,25 +559,29 @@ namespace jiminy::python // Check array dtype if (PyArray_EquivTypenums(PyArray_TYPE(arrayPy), getPyType()) == NPY_FALSE) { - THROW_ERROR(std::invalid_argument, - "'values' input array has dtype '", - PyArray_TYPE(arrayPy), - "' but '", - getPyType(), - "' was expected."); + JIMINY_THROW(std::invalid_argument, + "'values' input array has dtype '", + PyArray_TYPE(arrayPy), + "' but '", + getPyType(), + "' was expected."); } - // Check array number of dimensions + // Pick the right mapping depending on dimensionality and memory layout + int32_t flags = PyArray_FLAGS(arrayPy); T * dataPtr = static_cast(PyArray_DATA(arrayPy)); switch (PyArray_NDIM(arrayPy)) { case 0: return {dataPtr, 1, 1, {1, 1}}; case 1: - return {dataPtr, PyArray_SIZE(arrayPy), 1, {PyArray_SIZE(arrayPy), 1}}; + if (flags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS)) + { + return {dataPtr, PyArray_SIZE(arrayPy), 1, {PyArray_SIZE(arrayPy), 1}}; + } + JIMINY_THROW(std::invalid_argument, "Numpy array must be contiguous."); case 2: { - int32_t flags = PyArray_FLAGS(arrayPy); npy_intp * arrayPyShape = PyArray_SHAPE(arrayPy); if (flags & NPY_ARRAY_C_CONTIGUOUS) { @@ -590,11 +591,11 @@ namespace jiminy::python { return {dataPtr, arrayPyShape[0], arrayPyShape[1], {arrayPyShape[0], 1}}; } - THROW_ERROR(std::invalid_argument, - "Numpy arrays must be either row or column contiguous."); + JIMINY_THROW(std::invalid_argument, + "Numpy array must be either row or column contiguous."); } default: - THROW_ERROR(not_implemented_error, "Only 1D and 2D 'np.ndarray' are supported."); + JIMINY_THROW(not_implemented_error, "Only 1D and 2D 'np.ndarray' are supported."); } } @@ -607,7 +608,7 @@ namespace jiminy::python // Check if raw Python object pointer is actually a numpy array if (!PyArray_Check(dataPy)) { - THROW_ERROR(std::invalid_argument, "'values' must have type 'np.ndarray'."); + JIMINY_THROW(std::invalid_argument, "'values' must have type 'np.ndarray'."); } /* Cast raw Python object pointer to numpy array. @@ -623,8 +624,8 @@ namespace jiminy::python { return getEigenReferenceImpl(arrayPy); } - THROW_ERROR(not_implemented_error, - "'values' input array must have dtype 'np.float64' or 'np.int64'."); + JIMINY_THROW(not_implemented_error, + "'values' input array must have dtype 'np.float64' or 'np.int64'."); } /// Convert most C++ objects into Python objects by value @@ -634,7 +635,7 @@ namespace jiminy::python !std::is_same_v, GenericConfig> && !std::is_same_v, SensorMeasurementTree::value_type> && !std::is_same_v, std::string_view> && - !std::is_same_v, FlexibleJointData>, + !std::is_same_v, FlexibilityJointConfig>, bp::object> convertToPython(T && data, const bool & copy = true) { @@ -691,22 +692,22 @@ namespace jiminy::python } template - std::enable_if_t, FlexibleJointData>, bp::object> - convertToPython(T && flexibleJointData, const bool & copy) + std::enable_if_t, FlexibilityJointConfig>, bp::object> + convertToPython(T && flexibilityJointConfig, const bool & copy) { if (!copy) { - THROW_ERROR( - not_implemented_error, - "Passing 'FlexibleJointData' object to python by reference is not supported."); + JIMINY_THROW(not_implemented_error, + "Passing 'FlexibilityJointConfig' object to python by reference is not " + "supported."); } - bp::dict flexibilityJointDataPy; - flexibilityJointDataPy["frameName"] = flexibleJointData.frameName; - flexibilityJointDataPy["stiffness"] = flexibleJointData.stiffness; - flexibilityJointDataPy["damping"] = flexibleJointData.damping; - flexibilityJointDataPy["inertia"] = flexibleJointData.inertia; + bp::dict flexibilityJointConfigPy; + flexibilityJointConfigPy["frameName"] = flexibilityJointConfig.frameName; + flexibilityJointConfigPy["stiffness"] = flexibilityJointConfig.stiffness; + flexibilityJointConfigPy["damping"] = flexibilityJointConfig.damping; + flexibilityJointConfigPy["inertia"] = flexibilityJointConfig.inertia; // FIXME: Remove explicit and redundant move when moving to C++20 - return std::move(flexibilityJointDataPy); + return std::move(flexibilityJointConfigPy); } template @@ -778,7 +779,7 @@ namespace jiminy::python return &PyList_Type; } else if constexpr (std::is_same_v, GenericConfig> || - std::is_same_v, FlexibleJointData>) + std::is_same_v, FlexibilityJointConfig>) { return &PyDict_Type; } @@ -929,29 +930,38 @@ namespace jiminy::python np::ndarray dataNumpy = bp::extract(dataPy); if (dataNumpy.get_dtype() != np::dtype::get_builtin()) { - THROW_ERROR(std::invalid_argument, - "Scalar type of eigen object does not match dtype of numpy object."); + JIMINY_THROW(std::invalid_argument, + "Scalar type of eigen object does not match dtype of numpy object."); } return getEigenReferenceImpl(reinterpret_cast(dataPy.ptr())); } catch (const bp::error_already_set &) { - PyErr_Clear(); - if constexpr (is_eigen_vector_v) + if constexpr (is_eigen_plain_v) { - return listPyToEigenVector(bp::extract(dataPy)); + PyErr_Clear(); + if constexpr (is_eigen_vector_v) + { + return listPyToEigenVector(bp::extract(dataPy)); + } + else + { + return listPyToEigenMatrix(bp::extract(dataPy)); + } } else { - return listPyToEigenMatrix(bp::extract(dataPy)); + bp::throw_error_already_set(); + throw; } } } template<> - inline FlexibleJointData convertFromPython(const bp::object & dataPy) + inline FlexibilityJointConfig + convertFromPython(const bp::object & dataPy) { - FlexibleJointData flexData; + FlexibilityJointConfig flexData; const bp::dict flexDataPy = bp::extract(dataPy); flexData.frameName = convertFromPython(flexDataPy["frameName"]); flexData.stiffness = convertFromPython(flexDataPy["stiffness"]); @@ -1006,7 +1016,7 @@ namespace jiminy::python const bp::list listPy = bp::extract(dataPy); if (bp::len(listPy) != N) { - THROW_ERROR(std::invalid_argument, "Consistent number of elements"); + JIMINY_THROW(std::invalid_argument, "Consistent number of elements"); } return internal::BuildArrayFromCallable( std::make_index_sequence{}, diff --git a/python/jiminy_pywrap/src/controllers.cc b/python/jiminy_pywrap/src/controllers.cc index aa9d6f6e3..e611a7d4c 100644 --- a/python/jiminy_pywrap/src/controllers.cc +++ b/python/jiminy_pywrap/src/controllers.cc @@ -89,35 +89,35 @@ namespace jiminy::python } static void registerVariable( - AbstractController & self, const std::string & fieldname, PyObject * dataPy) + AbstractController & self, const std::string & name, PyObject * dataPy) { // Note that const qualifier is not supported by PyArray_DATA if (!PyArray_Check(dataPy)) { - THROW_ERROR(std::invalid_argument, - "'value' input must have type 'numpy.ndarray'."); + JIMINY_THROW(std::invalid_argument, + "'value' input must have type 'numpy.ndarray'."); } PyArrayObject * dataPyArray = reinterpret_cast(dataPy); if (PyArray_SIZE(dataPyArray) > 1U) { - THROW_ERROR(std::invalid_argument, - "'value' input array must have a single element."); + JIMINY_THROW(std::invalid_argument, + "'value' input array must have a single element."); } if (PyArray_TYPE(dataPyArray) == NPY_FLOAT64) { auto data = static_cast(PyArray_DATA(dataPyArray)); - return self.registerVariable(fieldname, *data); + return self.registerVariable(name, *data); } if (PyArray_TYPE(dataPyArray) == NPY_INT64) { auto data = static_cast(PyArray_DATA(dataPyArray)); - return self.registerVariable(fieldname, *data); + return self.registerVariable(name, *data); } - THROW_ERROR(not_implemented_error, - "'value' input array must have dtype 'np.float64' or 'np.int64'."); + JIMINY_THROW(not_implemented_error, + "'value' input array must have dtype 'np.float64' or 'np.int64'."); } template @@ -135,8 +135,8 @@ namespace jiminy::python // Check fieldnames and array have same length if (static_cast(data.size()) != fieldnames.size()) { - THROW_ERROR(std::invalid_argument, - "'values' input array must have same length than 'fieldnames'."); + JIMINY_THROW(std::invalid_argument, + "'values' input array must have same length than 'fieldnames'."); } // Register all variables at once @@ -161,8 +161,8 @@ namespace jiminy::python } if (!are_fieldnames_valid) { - THROW_ERROR(std::invalid_argument, - "'fieldnames' must be nested list with same shape than 'value'."); + JIMINY_THROW(std::invalid_argument, + "'fieldnames' must be nested list with same shape than 'value'."); } // Register rows sequentially @@ -182,31 +182,17 @@ namespace jiminy::python } static void registerConstant( - AbstractController & self, const std::string & fieldname, PyObject * dataPy) + AbstractController & self, const std::string & name, PyObject * dataPy) { - if (PyArray_Check(dataPy)) - { - return std::visit([&](auto && arg) - { return self.registerConstant(fieldname, arg); }, - getEigenReference(dataPy)); - } - if (PyFloat_Check(dataPy)) - { - return self.registerConstant(fieldname, PyFloat_AsDouble(dataPy)); - } - if (PyLong_Check(dataPy)) - { - return self.registerConstant(fieldname, PyLong_AsLong(dataPy)); - } if (PyBytes_Check(dataPy)) { - return self.registerConstant(fieldname, PyBytes_AsString(dataPy)); + return self.registerConstant(name, PyBytes_AsString(dataPy)); } - if (PyUnicode_Check(dataPy)) + if (PyBytes_Check(dataPy)) { - return self.registerConstant(fieldname, PyUnicode_AsUTF8(dataPy)); + return self.registerConstant(name, PyBytes_AsString(dataPy)); } - THROW_ERROR(not_implemented_error, "'value' type is unsupported."); + JIMINY_THROW(not_implemented_error, "'value' must have type 'bytes' or 'str'."); } static void setOptions(AbstractController & self, const bp::dict & configPy) @@ -242,21 +228,23 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("is_initialized", &AbstractController::getIsInitialized, bp::return_value_policy()) + .def("register_constant", + &internal::abstract_controller::registerConstant, + (bp::arg("self"), "name", "value")) .def("register_variable", &internal::abstract_controller::registerVariable, - (bp::arg("self"), "fieldname", "value"), + (bp::arg("self"), "name", "value"), "@copydoc AbstractController::registerVariable") .def("register_variables", &internal::abstract_controller::registerVariableArray, (bp::arg("self"), "fieldnames", "values")) - .def("register_constants", - &internal::abstract_controller::registerConstant, - (bp::arg("self"), "fieldnames", "values")) .def("remove_entries", &AbstractController::removeEntries) .def("set_options", &internal::abstract_controller::setOptions, (bp::arg("self"), "options")) - .def("get_options", &AbstractController::getOptions) + .def("get_options", + &AbstractController::getOptions, + bp::return_value_policy()) .ADD_PROPERTY_GET("robot", &internal::abstract_controller::getRobot) .DEF_READONLY("sensor_measurements", &AbstractController::sensorMeasurements_); diff --git a/python/jiminy_pywrap/src/engine.cc b/python/jiminy_pywrap/src/engine.cc index e07a9973c..25d476c75 100644 --- a/python/jiminy_pywrap/src/engine.cc +++ b/python/jiminy_pywrap/src/engine.cc @@ -6,7 +6,7 @@ #include "jiminy/core/control/abstract_controller.h" #include "jiminy/core/robot/robot.h" #include "jiminy/core/engine/engine.h" -#include "jiminy/core/engine/engine.h" +#include "jiminy/core/stepper/abstract_stepper.h" #include "pinocchio/bindings/python/fwd.hpp" #include @@ -277,16 +277,24 @@ namespace jiminy::python { aInit.emplace(convertFromPython>(aInitPy)); } - std::optional callback; + AbortSimulationFunction callback; if (!callbackPy.is_none()) { - callback.emplace(callbackPy); + callback = callbackPy; + } + else + { + callback = []() + { + return true; + }; } return self.simulate( endTime, convertFromPython>(qInitPy), convertFromPython>(vInitPy), - aInit); + aInit, + callback); } static void simulate(Engine & self, @@ -372,7 +380,7 @@ namespace jiminy::python // Get constants for (const auto & [key, value] : logData.constants) { - if (endsWith(key, ".options")) + if (endsWith(key, "options")) { std::vector jsonStringVec(value.begin(), value.end()); std::shared_ptr device = @@ -381,7 +389,7 @@ namespace jiminy::python jsonLoad(robotOptions, device); constants[key] = robotOptions; } - else if (endsWith(key, ".pinocchio_model")) + else if (key.find("pinocchio_model") != std::string::npos) { try { @@ -391,12 +399,12 @@ namespace jiminy::python } catch (const std::exception & e) { - THROW_ERROR(std::ios_base::failure, - "Failed to load pinocchio model from log: ", - e.what()); + JIMINY_THROW(std::ios_base::failure, + "Failed to load pinocchio model from log: ", + e.what()); } } - else if (endsWith(key, ".visual_model") || endsWith(key, ".collision_model")) + else if (endsWith(key, "visual_model") || endsWith(key, "collision_model")) { try { @@ -406,12 +414,12 @@ namespace jiminy::python } catch (const std::exception & e) { - THROW_ERROR(std::ios_base::failure, - "Failed to load collision and/or visual model from log: ", - e.what()); + JIMINY_THROW(std::ios_base::failure, + "Failed to load collision and/or visual model from log: ", + e.what()); } } - else if (endsWith(key, ".mesh_package_dirs")) + else if (endsWith(key, "mesh_package_dirs")) { bp::list meshPackageDirs; std::stringstream ss(value); @@ -432,7 +440,8 @@ namespace jiminy::python } else { - constants[key] = value; // convertToPython(value, false); + constants[key] = bp::object( + bp::handle<>(PyBytes_FromStringAndSize(value.c_str(), value.size()))); } } @@ -573,9 +582,9 @@ namespace jiminy::python } else { - THROW_ERROR(std::runtime_error, - "Impossible to determine the file format " - "automatically. Please specify it manually."); + JIMINY_THROW(std::runtime_error, + "Impossible to determine the file format " + "automatically. Please specify it manually."); } } const LogData logData = Engine::readLog(filename, format); @@ -588,6 +597,13 @@ namespace jiminy::python convertFromPython(configPy, config); return self.setOptions(config); } + + static void setSimulationOptions(Engine & self, const bp::dict & configPy) + { + GenericConfig config = self.getSimulationOptions(); + convertFromPython(configPy, config); + return self.setSimulationOptions(config); + } } void exposeEngine() @@ -767,10 +783,19 @@ namespace jiminy::python .def("remove_all_forces", &Engine::removeAllForces) .def("set_options", &internal::engine::setOptions) - .def("get_options", &Engine::getOptions) + .def( + "get_options", &Engine::getOptions, bp::return_value_policy()) + .def("set_simulation_options", &internal::engine::setSimulationOptions) + .def("get_simulation_options", &Engine::getSimulationOptions) .DEF_READONLY_WITH_POLICY( "robots", &Engine::robots_, bp::return_value_policy>()) + .def("get_robot", &Engine::getRobot, (bp::arg("self"), "robot_name")) + .def("get_robot_index", &Engine::getRobotIndex, (bp::arg("self"), "robot_name")) + .def("get_robot_state", + &Engine::getRobotState, + (bp::arg("self"), "robot_name"), + bp::return_value_policy>()) .ADD_PROPERTY_GET("robot_states", &internal::engine::getRobotStates) .ADD_PROPERTY_GET_WITH_POLICY("stepper_state", diff --git a/python/jiminy_pywrap/src/functors.cc b/python/jiminy_pywrap/src/functors.cc index a63dae53d..dbc52cf50 100644 --- a/python/jiminy_pywrap/src/functors.cc +++ b/python/jiminy_pywrap/src/functors.cc @@ -24,6 +24,24 @@ namespace jiminy::python // *********************************** HeightmapFunction *********************************** // + void queryHeightMap(HeightmapFunction & heightmap, + np::ndarray positionsPy, + np::ndarray heightsPy) + { + auto const positions = convertFromPython< + Eigen::Map> + >(positionsPy); + auto heights = convertFromPython< + Eigen::Map> + >(heightsPy).col(0); + + for (Eigen::Index i = 0; i < positions.cols() ; ++i) + { + Eigen::Vector3d normal; + heightmap(positions.col(i), heights[i], normal); + } + } + namespace internal::heightmap_function { static bp::tuple eval(HeightmapFunction & self, const Eigen::Vector2d & position) @@ -64,5 +82,9 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("py_function", &internal::heightmap_function::getPyFun, bp::return_value_policy()); + + bp::def("query_heightmap", + &queryHeightMap, + (bp::args("heightmap"), "positions", "heights")); } } diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc index 22d0adc60..f2c00bce5 100644 --- a/python/jiminy_pywrap/src/generators.cc +++ b/python/jiminy_pywrap/src/generators.cc @@ -53,8 +53,8 @@ namespace jiminy::python { \ return convertFromPython>>(array).cast(); \ } \ - THROW_ERROR(std::invalid_argument, \ - "Matrix arguments must have dtype 'np.float32' or 'np.float64'."); \ + JIMINY_THROW(std::invalid_argument, \ + "Matrix arguments must have dtype 'np.float32' or 'np.float64'."); \ }; \ return dist(generator, cast(arg1), cast(arg2)).cast(); \ } \ @@ -83,7 +83,7 @@ namespace jiminy::python case 0: \ break; \ default: \ - THROW_ERROR(std::invalid_argument, "'size' must have at most 2 dimensions."); \ + JIMINY_THROW(std::invalid_argument, "'size' must have at most 2 dimensions."); \ } \ return convertToPython(dist(nrows, ncols, generator, arg1, arg2).cast(), true); \ } diff --git a/python/jiminy_pywrap/src/helpers.cc b/python/jiminy_pywrap/src/helpers.cc index 59d02bb21..820b320cb 100644 --- a/python/jiminy_pywrap/src/helpers.cc +++ b/python/jiminy_pywrap/src/helpers.cc @@ -90,188 +90,209 @@ namespace jiminy::python return bp::extract(objPy); } + template + void EigenMapTransposeAssign(char * dstData, char * srcData, npy_intp rows, npy_intp cols) + { + using EigenMapType = Eigen::Map>; + EigenMapType dst(reinterpret_cast(dstData), rows, cols); + EigenMapType src(reinterpret_cast(srcData), cols, rows); + dst = src.transpose(); + } + void arrayCopyTo(PyObject * dstPy, PyObject * srcPy) { - // Making sure that 'dst' is a valid array and is writable, raises an exception otherwise + // Organize code path to run as fast as possible for the most common use-cases + // 1. 1D or 2D arrays for which neither type casting nor broadcasting is necessary + // 2. Assigning a scalar value to a N-D array, incl. type casting if necessary + // 3. Discontinuous 1D or N-D array assignment without type casting nor broadcasting + // 4. Deal with all other cases by calling generic Numpy routine `PyArray_CopyInto` + + // Re-interpret source and destination as a Numpy array, not just a plain Python object + PyArrayObject * dstPyArray = reinterpret_cast(dstPy); + PyArrayObject * srcPyArray = reinterpret_cast(srcPy); + + // Make sure that 'dst' is a valid array and is writable, raises an exception otherwise if (!PyArray_Check(dstPy)) { - THROW_ERROR(std::invalid_argument, "'dst' must have type 'np.ndarray'."); + JIMINY_THROW(std::invalid_argument, "'dst' must have type 'np.ndarray'."); } - PyArrayObject * dstPyArray = reinterpret_cast(dstPy); + + // Make sure that 'dst' is writable const int dstPyFlags = PyArray_FLAGS(dstPyArray); if (!(dstPyFlags & NPY_ARRAY_WRITEABLE)) { - THROW_ERROR(std::invalid_argument, "'dst' must be writable."); + JIMINY_THROW(std::invalid_argument, "'dst' must be writable."); } - // Dedicated path to fill with scalar - const npy_intp itemsize = PyArray_ITEMSIZE(dstPyArray); - char * dstPyData = PyArray_BYTES(dstPyArray); - if (!PyArray_Check(srcPy) || PyArray_IsScalar(srcPy, Generic) || - (PyArray_SIZE(reinterpret_cast(srcPy)) == 1)) + // Return early if destination is empty + if (PyArray_SIZE(dstPyArray) < 1) { - /* Eigen does a much better job than element-wise copy assignment in this scenario. - Note that only the width of the scalar type matters here, not the actual type. - Ensure copy and casting are both slow as they allocate new array, so avoiding - using them entirely if possible and falling back to default routine otherwise. */ - if ((itemsize == 8) && (dstPyFlags & NPY_ARRAY_ALIGNED) && - (dstPyFlags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS))) + return; + } + + // Check if the source is a numpy array + const bool isSrcArray = PyArray_Check(srcPy); + + // Source and destination are both numpy arrays + if (isSrcArray) + { + // Get shape of source and destination arrays + const int dstNdim = PyArray_NDIM(dstPyArray); + const int srcNdim = PyArray_NDIM(srcPyArray); + const npy_intp * const dstShape = PyArray_SHAPE(dstPyArray); + const npy_intp * const srcShape = PyArray_SHAPE(srcPyArray); + + // N-Dim arrays but no broadcasting nor casting required. Easy enough to handle. + if (dstNdim == srcNdim && PyArray_CompareLists(dstShape, srcShape, dstNdim) && + PyArray_EquivArrTypes(dstPyArray, srcPyArray)) { - // Convert src scalar data to raw bytes with dst dtype, casting if necessary - bool isSuccess = false; - double srcPyScalar; - if (PyArray_Check(srcPy)) + // Get memory layout of source and destination arrays + char * dstPyData = PyArray_BYTES(dstPyArray); + char * srcPyData = PyArray_BYTES(srcPyArray); + const npy_intp itemsize = PyArray_ITEMSIZE(dstPyArray); + const int srcPyFlags = PyArray_FLAGS(srcPyArray); + const int commonPyFlags = dstPyFlags & srcPyFlags; + + // Same memory layout. Straightforward regardless the dimensionality. + if ((commonPyFlags & NPY_ARRAY_ALIGNED) && + (commonPyFlags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS))) { - PyArrayObject * srcPyArray = reinterpret_cast(srcPy); - if (PyArray_EquivArrTypes(dstPyArray, srcPyArray)) + Eigen::Map> dst(dstPyData, itemsize, PyArray_SIZE(dstPyArray)); + Eigen::Map> src(srcPyData, itemsize, PyArray_SIZE(srcPyArray)); + dst = src; + return; + } + + /* Different memory layout in 2D. + TODO: Extend to support any number of dims by operating on flattened view. */ + if ((dstNdim == 2) && + (dstPyFlags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS)) && + (srcPyFlags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS))) + { + /* Using Eigen once again to avoid slow element-wise copy assignment. + Note that only the width of the scalar type matters, not the actual type. */ + npy_intp rows, cols; + if (dstPyFlags & NPY_ARRAY_C_CONTIGUOUS) { - srcPyScalar = *reinterpret_cast(PyArray_DATA(srcPyArray)); - isSuccess = true; + rows = dstShape[1]; + cols = dstShape[0]; } else { - int dstPyTypeNum = PyArray_TYPE(dstPyArray); - PyArray_Descr * srcPyDtype = PyArray_DESCR(srcPyArray); - if (!PyTypeNum_ISEXTENDED(dstPyTypeNum) && - !PyTypeNum_ISEXTENDED(srcPyDtype->type_num)) - { - auto srcToDstCastFunc = PyArray_GetCastFunc(srcPyDtype, dstPyTypeNum); - srcToDstCastFunc( - PyArray_DATA(srcPyArray), &srcPyScalar, 1, NULL, NULL); - isSuccess = true; - } + rows = dstShape[0]; + cols = dstShape[1]; + } + switch (itemsize) + { + case 8: + EigenMapTransposeAssign(dstPyData, srcPyData, rows, cols); + return; + case 4: + EigenMapTransposeAssign(dstPyData, srcPyData, rows, cols); + return; + case 2: + EigenMapTransposeAssign(dstPyData, srcPyData, rows, cols); + return; + case 1: + EigenMapTransposeAssign(dstPyData, srcPyData, rows, cols); + return; + default: + break; } } - else if (PyArray_IsScalar(srcPy, Generic)) - { - PyArray_CastScalarToCtype(srcPy, &srcPyScalar, PyArray_DESCR(dstPyArray)); - isSuccess = true; - } - else if (PyFloat_Check(srcPy) || PyLong_Check(srcPy)) + + // Falling back to slow element-wise strided ND-array copy assignment + int i = 0; + npy_intp coord[NPY_MAXDIMS]; + const npy_intp * const dstStrides = PyArray_STRIDES(dstPyArray); + const npy_intp * const srcStrides = PyArray_STRIDES(srcPyArray); + memset(coord, 0, dstNdim * sizeof(npy_intp)); + while (i < dstNdim) { - int dstPyTypeNum = PyArray_TYPE(dstPyArray); - PyArray_Descr * srcPyDtype = PyArray_DescrFromObject(srcPy, NULL); - if (!PyTypeNum_ISEXTENDED(dstPyTypeNum)) + char * _dstPyData = dstPyData; + char * _srcPyData = srcPyData; + for (int j = 0; j < dstShape[0]; ++j) { - auto srcToDstCastFunc = PyArray_GetCastFunc(srcPyDtype, dstPyTypeNum); - if (PyFloat_Check(srcPy)) + memcpy(_dstPyData, _srcPyData, itemsize); + _dstPyData += dstStrides[0]; + _srcPyData += srcStrides[0]; + } + for (i = 1; i < dstNdim; ++i) + { + if (++coord[i] == dstShape[i]) { - srcPyScalar = PyFloat_AsDouble(srcPy); + coord[i] = 0; + dstPyData -= (dstShape[i] - 1) * dstStrides[i]; + srcPyData -= (dstShape[i] - 1) * srcStrides[i]; } else { - auto srcPyBuiltin = std::make_unique(PyLong_AsLong(srcPy)); - srcPyScalar = *reinterpret_cast(srcPyBuiltin.get()); - } - if (srcToDstCastFunc != NULL) - { - srcToDstCastFunc(&srcPyScalar, &srcPyScalar, 1, NULL, NULL); - isSuccess = true; + dstPyData += dstStrides[i]; + srcPyData += srcStrides[i]; + break; } } - Py_DECREF(srcPyDtype); } + return; + } + } - // Copy scalar bytes to destination if available - if (isSuccess) - { - Eigen::Map> dst(reinterpret_cast(dstPyData), - PyArray_SIZE(dstPyArray)); - dst.setConstant(srcPyScalar); - return; - } + // Specialization to fill with scalar + if (!isSrcArray || PyArray_IsScalar(srcPy, Generic) || (PyArray_SIZE(srcPyArray) == 1)) + { + // Extract built-in scalar value to promote setitem fast path + if (isSrcArray) + { + srcPy = PyArray_GETITEM(srcPyArray, PyArray_BYTES(srcPyArray)); } - // Too complicated to deal with it manually. Falling back to default routine. - if (PyArray_FillWithScalar(dstPyArray, srcPy) < 0) + + /* Eigen does a much better job than element-wise copy assignment in this scenario. + Ensuring copy and casting are both slow as they allocate new array, so avoiding + using them entirely if possible and falling back to default routine otherwise. */ + if ((dstPyFlags & NPY_ARRAY_ALIGNED) && + (dstPyFlags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS))) { - THROW_ERROR(std::runtime_error, "Impossible to copy from 'src' to 'dst'."); + // Convert src scalar data to raw bytes with dst dtype, casting if necessary + const npy_intp itemsize = PyArray_ITEMSIZE(dstPyArray); + char * dstPyData = PyArray_BYTES(dstPyArray); + PyArray_SETITEM(dstPyArray, dstPyData, srcPy); + Eigen::Map> dst(dstPyData, itemsize, PyArray_SIZE(dstPyArray)); + dst.rightCols(dst.cols() - 1).colwise() = dst.col(0); + return; } - return; - } - // Check if too complicated to deal with it manually. Falling back to default routine. - PyArrayObject * srcPyArray = reinterpret_cast(srcPy); - const int dstNdim = PyArray_NDIM(dstPyArray); - const int srcNdim = PyArray_NDIM(srcPyArray); - const npy_intp * const dstShape = PyArray_SHAPE(dstPyArray); - const npy_intp * const srcShape = PyArray_SHAPE(srcPyArray); - const int srcPyFlags = PyArray_FLAGS(srcPyArray); - const int commonPyFlags = dstPyFlags & srcPyFlags; - if (dstNdim != srcNdim || !PyArray_CompareLists(dstShape, srcShape, dstNdim) || - !(commonPyFlags & NPY_ARRAY_ALIGNED) || !PyArray_EquivArrTypes(dstPyArray, srcPyArray)) - { - if (PyArray_CopyInto(dstPyArray, srcPyArray) < 0) + // Too complicated to deal with it manually. Falling back to default routine. + if (PyArray_FillWithScalar(dstPyArray, srcPy) < 0) { - THROW_ERROR(std::runtime_error, "Impossible to copy from 'src' to 'dst'."); + JIMINY_THROW(std::runtime_error, "Impossible to copy from 'src' to 'dst'."); } return; } - // Multi-dimensional array but no broadcasting nor casting required. Easy enough to handle. - char * srcPyData = PyArray_BYTES(srcPyArray); - if (commonPyFlags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS)) + // Falling back to default routine if too complicated to deal with it manually + if (PyArray_CopyInto(dstPyArray, srcPyArray) < 0) { - /* Fast specialization if both dst and src are jointly C- or F-contiguous. - Note that converting arrays to Eigen matrices would leverage SIMD-vectorized - assignment, which is faster than `memcpy`. Yet, instantiating the mapping is tricky - because of memory alignment issues and dtype handling, so let's keep it simple. The - slowdown should be marginal for small-size arrays (size < 50). */ - memcpy(dstPyData, srcPyData, PyArray_NBYTES(dstPyArray)); + JIMINY_THROW(std::runtime_error, "Impossible to copy from 'src' to 'dst'."); } - else if ((dstNdim == 2) && (itemsize == 8) && - (dstPyFlags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS)) && - (srcPyFlags & (NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_F_CONTIGUOUS))) + return; + } + + void multiArrayCopyTo(PyObject * dstPy, PyObject * srcPy) + { + PyObject * dstSeqPy = PySequence_Fast(dstPy, "Only tuple or list is supported for 'dst'."); + PyObject * srcSeqPy = PySequence_Fast(srcPy, "Only tuple or list is supported for 'src'."); + const Py_ssize_t dstSize = PySequence_Fast_GET_SIZE(dstSeqPy); + const Py_ssize_t srcSize = PySequence_Fast_GET_SIZE(srcSeqPy); + if (dstSize != srcSize) { - /* Using Eigen once again to avoid slow element-wise copy assignment. - TODO: Extend to support any number of dims by working on flattened view. */ - using EigenMapType = Eigen::Map; - if (dstPyFlags & NPY_ARRAY_C_CONTIGUOUS) - { - EigenMapType dst(reinterpret_cast(dstPyData), dstShape[1], dstShape[0]); - EigenMapType src(reinterpret_cast(srcPyData), dstShape[0], dstShape[1]); - dst = src.transpose(); - } - else - { - EigenMapType dst(reinterpret_cast(dstPyData), dstShape[0], dstShape[1]); - EigenMapType src(reinterpret_cast(srcPyData), dstShape[1], dstShape[0]); - dst = src.transpose(); - } + JIMINY_THROW(std::runtime_error, "Length mismatch between 'src' and 'dst'."); } - else + + PyObject ** dstItemsPy = PySequence_Fast_ITEMS(dstPy); + PyObject ** srcItemsPy = PySequence_Fast_ITEMS(srcPy); + for (uint32_t i = 0; i < dstSize; ++i) { - // Falling back to slow element-wise strided ND-array copy assignment - int i = 0; - npy_intp coord[NPY_MAXDIMS]; - const npy_intp * const dstStrides = PyArray_STRIDES(dstPyArray); - const npy_intp * const srcStrides = PyArray_STRIDES(srcPyArray); - memset(coord, 0, dstNdim * sizeof(npy_intp)); - while (i < dstNdim) - { - char * _dstPyData = dstPyData; - char * _srcPyData = srcPyData; - for (int j = 0; j < dstShape[0]; ++j) - { - memcpy(_dstPyData, _srcPyData, itemsize); - _dstPyData += dstStrides[0]; - _srcPyData += srcStrides[0]; - } - for (i = 1; i < dstNdim; ++i) - { - if (++coord[i] == dstShape[i]) - { - coord[i] = 0; - dstPyData -= (dstShape[i] - 1) * dstStrides[i]; - srcPyData -= (dstShape[i] - 1) * srcStrides[i]; - } - else - { - dstPyData += dstStrides[i]; - srcPyData += srcStrides[i]; - break; - } - } - } + arrayCopyTo(dstItemsPy[i], srcItemsPy[i]); } } @@ -307,6 +328,7 @@ namespace jiminy::python (bp::arg("pinocchio_model"), "joint_names")); bp::def("array_copyto", &arrayCopyTo, (bp::arg("dst"), "src")); + bp::def("multi_array_copyto", &multiArrayCopyTo, (bp::arg("dst"), "src")); // Do not use EigenPy To-Python converter because it considers matrices with 1 column as vectors bp::def("interpolate_positions", &interpolatePositions, diff --git a/python/jiminy_pywrap/src/motors.cc b/python/jiminy_pywrap/src/motors.cc index 13a203156..1c9cacf66 100644 --- a/python/jiminy_pywrap/src/motors.cc +++ b/python/jiminy_pywrap/src/motors.cc @@ -56,9 +56,14 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("armature", &AbstractMotorBase::getArmature, bp::return_value_policy()) + .ADD_PROPERTY_GET_WITH_POLICY("backlash", + &AbstractMotorBase::getBacklash, + bp::return_value_policy()) .def("set_options", &internal::abstract_motor::setOptions) - .def("get_options", &AbstractMotorBase::getOptions); + .def("get_options", + &AbstractMotorBase::getOptions, + bp::return_value_policy()); } // ************************************** BasicMotors ************************************** // diff --git a/python/jiminy_pywrap/src/robot.cc b/python/jiminy_pywrap/src/robot.cc index db960ac74..ba9dba8d5 100644 --- a/python/jiminy_pywrap/src/robot.cc +++ b/python/jiminy_pywrap/src/robot.cc @@ -59,6 +59,12 @@ namespace jiminy::python namespace internal::model { + static void removeFrames(Model & self, const bp::object & frameNamesPy) + { + auto frameNames = convertFromPython>(frameNamesPy); + return self.removeFrames(frameNames); + } + static void addCollisionBodies( Model & self, const bp::object & linkNamesPy, bool ignoreMeshes) { @@ -119,41 +125,41 @@ namespace jiminy::python return bp::make_tuple(J, gamma); } - static Eigen::VectorXd getFlexiblePositionFromRigid(Model & self, - const Eigen::VectorXd & qRigid) + static Eigen::VectorXd getExtendedPositionFromTheoretical( + Model & self, const Eigen::VectorXd & qTheoretical) { - Eigen::VectorXd qFlexible; - self.getFlexiblePositionFromRigid(qRigid, qFlexible); - return qFlexible; + Eigen::VectorXd qExtended; + self.getExtendedPositionFromTheoretical(qTheoretical, qExtended); + return qExtended; } - static Eigen::VectorXd getFlexibleVelocityFromRigid(Model & self, - const Eigen::VectorXd & vRigid) + static Eigen::VectorXd getExtendedVelocityFromTheoretical( + Model & self, const Eigen::VectorXd & vTheoretical) { - Eigen::VectorXd vFlexible; - self.getFlexibleVelocityFromRigid(vRigid, vFlexible); - return vFlexible; + Eigen::VectorXd vExtended; + self.getExtendedVelocityFromTheoretical(vTheoretical, vExtended); + return vExtended; } - static Eigen::VectorXd getRigidPositionFromFlexible(Model & self, - const Eigen::VectorXd & qFlexible) + static Eigen::VectorXd getTheoreticalPositionFromExtended( + Model & self, const Eigen::VectorXd & qExtended) { - Eigen::VectorXd qRigid; - self.getRigidPositionFromFlexible(qFlexible, qRigid); - return qRigid; + Eigen::VectorXd qTheoretical; + self.getTheoreticalPositionFromExtended(qExtended, qTheoretical); + return qTheoretical; } - static Eigen::VectorXd getRigidVelocityFromFlexible(Model & self, - const Eigen::VectorXd & vFlexible) + static Eigen::VectorXd getTheoreticalVelocityFromExtended( + Model & self, const Eigen::VectorXd & vExtended) { - Eigen::VectorXd vRigid; - self.getRigidVelocityFromFlexible(vFlexible, vRigid); - return vRigid; + Eigen::VectorXd vTheoretical; + self.getTheoreticalVelocityFromExtended(vExtended, vTheoretical); + return vTheoretical; } - static bool isFlexibleModelEnabled(Model & self) + static bool isFlexibilityEnabled(Model & self) { - return self.modelOptions_->dynamics.enableFlexibleModel; + return self.modelOptions_->dynamics.enableFlexibility; } } @@ -180,14 +186,15 @@ namespace jiminy::python (bp::arg("self"), "generator"))) .def("set_options", &setOptions, (bp::arg("self"), "options")) - .def("get_options", &Model::getOptions) + .def("get_options", &Model::getOptions, bp::return_value_policy()) .def("add_frame", static_cast( &Model::addFrame), (bp::arg("self"), "frame_name", "parent_body_name", "frame_placement")) - .def("remove_frame", &Model::removeFrame, (bp::arg("self"), "frame_name")) + .def("remove_frames", &internal::model::removeFrames, (bp::arg("self"), "frame_names")) + .def("add_collision_bodies", &internal::model::addCollisionBodies, (bp::arg("self"), @@ -224,34 +231,34 @@ namespace jiminy::python &internal::model::getConstraintsJacobianAndDrift) .def("compute_constraints", &Model::computeConstraints, (bp::arg("self"), "q", "v")) - .def("get_flexible_position_from_rigid", - &internal::model::getFlexiblePositionFromRigid, - (bp::arg("self"), "rigid_position")) - .def("get_flexible_velocity_from_rigid", - &internal::model::getFlexibleVelocityFromRigid, - (bp::arg("self"), "rigid_velocity")) - .def("get_rigid_position_from_flexible", - &internal::model::getRigidPositionFromFlexible, - (bp::arg("self"), "flexible_position")) - .def("get_rigid_velocity_from_flexible", - &internal::model::getRigidVelocityFromFlexible, - (bp::arg("self"), "flexible_velocity")) + .def("get_extended_position_from_theoretical", + &internal::model::getExtendedPositionFromTheoretical, + (bp::arg("self"), "mechanical_position")) + .def("get_extended_velocity_from_theoretical", + &internal::model::getExtendedVelocityFromTheoretical, + (bp::arg("self"), "mechanical_velocity")) + .def("get_theoretical_position_from_extended", + &internal::model::getTheoreticalPositionFromExtended, + (bp::arg("self"), "flexibility_position")) + .def("get_theoretical_velocity_from_extended", + &internal::model::getTheoreticalVelocityFromExtended, + (bp::arg("self"), "flexibility_velocity")) // FIXME: Disable automatic typing because typename returned by 'py_type_str' is // missing module prefix, which makes it impossible to distinguish 'pinocchio.Model' // from 'jiminy.Model' classes. .def_readonly("pinocchio_model_th", - &Model::pinocchioModelOrig_, + &Model::pinocchioModelTh_, "fget( (Model)self) -> pinocchio.Model") .def_readonly("pinocchio_model", &Model::pinocchioModel_, "fget( (Model)self) -> pinocchio.Model") - .DEF_READONLY("collision_model_th", &Model::collisionModelOrig_) + .DEF_READONLY("collision_model_th", &Model::collisionModelTh_) .DEF_READONLY("collision_model", &Model::collisionModel_) - .DEF_READONLY("visual_model_th", &Model::visualModelOrig_) + .DEF_READONLY("visual_model_th", &Model::visualModelTh_) .DEF_READONLY("visual_model", &Model::visualModel_) .DEF_READONLY("visual_data", &Model::visualData_) - .DEF_READONLY("pinocchio_data_th", &Model::pinocchioDataOrig_) + .DEF_READONLY("pinocchio_data_th", &Model::pinocchioDataTh_) .DEF_READONLY("pinocchio_data", &Model::pinocchioData_) .DEF_READONLY("collision_data", &Model::collisionData_) @@ -268,7 +275,7 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("has_freeflyer", &Model::getHasFreeflyer, bp::return_value_policy()) - .ADD_PROPERTY_GET("is_flexible", &internal::model::isFlexibleModelEnabled) + .ADD_PROPERTY_GET("is_flexibility_enabled", &internal::model::isFlexibilityEnabled) .ADD_PROPERTY_GET_WITH_POLICY( "nq", &Model::nq, bp::return_value_policy()) .ADD_PROPERTY_GET_WITH_POLICY( @@ -291,23 +298,29 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("contact_frame_indices", &Model::getContactFrameIndices, bp::return_value_policy>()) - .ADD_PROPERTY_GET_WITH_POLICY("rigid_joint_names", - &Model::getRigidJointNames, + .ADD_PROPERTY_GET_WITH_POLICY("mechanical_joint_names", + &Model::getMechanicalJointNames, + bp::return_value_policy>()) + .ADD_PROPERTY_GET_WITH_POLICY("mechanical_joint_indices", + &Model::getMechanicalJointIndices, bp::return_value_policy>()) - .ADD_PROPERTY_GET_WITH_POLICY("rigid_joint_indices", - &Model::getRigidJointIndices, + .ADD_PROPERTY_GET_WITH_POLICY("mechanical_joint_position_indices", + &Model::getMechanicalJointPositionIndices, bp::return_value_policy>()) - .ADD_PROPERTY_GET_WITH_POLICY("rigid_joint_position_indices", - &Model::getRigidJointPositionIndices, + .ADD_PROPERTY_GET_WITH_POLICY("mechanical_joint_velocity_indices", + &Model::getMechanicalJointVelocityIndices, bp::return_value_policy>()) - .ADD_PROPERTY_GET_WITH_POLICY("rigid_joint_velocity_indices", - &Model::getRigidJointVelocityIndices, + .ADD_PROPERTY_GET_WITH_POLICY("flexibility_joint_names", + &Model::getFlexibilityJointNames, bp::return_value_policy>()) - .ADD_PROPERTY_GET_WITH_POLICY("flexible_joint_names", - &Model::getFlexibleJointNames, + .ADD_PROPERTY_GET_WITH_POLICY("flexibility_joint_indices", + &Model::getFlexibilityJointIndices, bp::return_value_policy>()) - .ADD_PROPERTY_GET_WITH_POLICY("flexible_joint_indices", - &Model::getFlexibleJointIndices, + .ADD_PROPERTY_GET_WITH_POLICY("backlash_joint_names", + &Model::getBacklashJointNames, + bp::return_value_policy>()) + .ADD_PROPERTY_GET_WITH_POLICY("backlash_joint_indices", + &Model::getBacklashJointIndices, bp::return_value_policy>()) .ADD_PROPERTY_GET_WITH_POLICY("position_limit_lower", @@ -366,34 +379,6 @@ namespace jiminy::python convertFromPython(configPy, config); return self.setModelOptions(config); } - - static void setMotorsOptions(Robot & self, const bp::dict & configPy) - { - GenericConfig config = self.getMotorsOptions(); - convertFromPython(configPy, config); - return self.setMotorsOptions(config); - } - - static void setSensorsOptions(Robot & self, const bp::dict & configPy) - { - GenericConfig config = self.getSensorsOptions(); - convertFromPython(configPy, config); - return self.setSensorsOptions(config); - } - - static void setControllerOptions(Robot & self, const bp::dict & configPy) - { - GenericConfig config = self.getControllerOptions(); - convertFromPython(configPy, config); - return self.setControllerOptions(config); - } - - static void setTelemetryOptions(Robot & self, const bp::dict & configPy) - { - GenericConfig config = self.getTelemetryOptions(); - convertFromPython(configPy, config); - return self.setTelemetryOptions(config); - } } void exposeRobot() @@ -420,15 +405,14 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY( "name", &Robot::getName, bp::return_value_policy()) - .def("dump_options", &Robot::dumpOptions, (bp::arg("self"), "json_filename")) - .def("load_options", &Robot::loadOptions, (bp::arg("self"), "json_filename")) - .def("attach_motor", &Robot::attachMotor, (bp::arg("self"), "motor")) .def("get_motor", static_cast (Robot::*)(const std::string &)>( &Robot::getMotor), (bp::arg("self"), "motor_name")) - .def("detach_motor", &Robot::detachMotor, (bp::arg("self"), "joint_name")) + .def("detach_motor", + static_cast(&Robot::detachMotor), + (bp::arg("self"), "joint_name")) .def("detach_motors", &internal::robot::detachMotors, (bp::arg("self"), bp::arg("joints_names") = bp::list())) @@ -455,27 +439,13 @@ namespace jiminy::python .ADD_PROPERTY_GET("sensor_measurements", &internal::robot::getSensorMeasurements) .def("set_options", &setOptions, (bp::arg("self"), "robot_options")) - .def("get_options", &Robot::getOptions) + .def("get_options", &Robot::getOptions, bp::return_value_policy()) .def("set_model_options", &internal::robot::setModelOptions, (bp::arg("self"), "model_options")) - .def("get_model_options", &Robot::getModelOptions) - .def("set_motors_options", - &internal::robot::setMotorsOptions, - (bp::arg("self"), "motors_options")) - .def("get_motors_options", &Robot::getMotorsOptions) - .def("set_sensors_options", - &internal::robot::setSensorsOptions, - (bp::arg("self"), "sensors_options")) - .def("get_sensors_options", &Robot::getSensorsOptions) - .def("set_controller_options", - &internal::robot::setControllerOptions, - (bp::arg("self"), "controller_options")) - .def("get_controller_options", &Robot::getControllerOptions) - .def("set_telemetry_options", - &internal::robot::setTelemetryOptions, - (bp::arg("self"), "telemetry_options")) - .def("get_telemetry_options", &Robot::getTelemetryOptions) + .def("get_model_options", + &Robot::getModelOptions, + bp::return_value_policy()) .ADD_PROPERTY_GET("nmotors", &Robot::nmotors) .ADD_PROPERTY_GET_WITH_POLICY("motor_names", diff --git a/python/jiminy_pywrap/src/sensors.cc b/python/jiminy_pywrap/src/sensors.cc index 55aa5309f..1577414fb 100644 --- a/python/jiminy_pywrap/src/sensors.cc +++ b/python/jiminy_pywrap/src/sensors.cc @@ -32,7 +32,7 @@ namespace jiminy::python auto sensorDataIt = SensorMeasurementTreeByName.find(sensorName); if (sensorDataIt == SensorMeasurementTreeByName.end()) { - THROW_ERROR(std::runtime_error, ""); + JIMINY_THROW(std::runtime_error, ""); } return sensorDataIt->value; } @@ -273,7 +273,9 @@ namespace jiminy::python const Eigen::MatrixBase &)>(&AbstractSensorBase::set)) .def("set_options", &internal::abstract_sensor::setOptions) - .def("get_options", &AbstractSensorBase::getOptions) + .def("get_options", + &AbstractSensorBase::getOptions, + bp::return_value_policy()) .def("__repr__", &internal::abstract_sensor::repr); } @@ -291,7 +293,6 @@ namespace jiminy::python // clang-format off cl .def_readonly("type", &DerivedSensor::type_) - .def_readonly("has_prefix", &DerivedSensor::areFieldnamesGrouped_) .add_static_property("fieldnames", bp::make_getter(&DerivedSensor::fieldnames_, bp::return_value_policy>())) ;