From eb8e39eda74525f966f1dbec645fb34ce607c23c Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Sun, 31 Dec 2023 15:35:51 +0100 Subject: [PATCH] [core] Move member initialization in header when possible. (#683) * [core] Move member initialization in header when possible. * [core] Use attributes 'explicit', 'noexcept'. * [core/python] More robust and generic from/to python converters. * [misc] More robust build script on unix systems. * [python/viewer] Do not use 'atexit' anymore to fix memory leaks. --- .github/workflows/macos.yml | 8 +- build_tools/build_install_deps_unix.sh | 48 +-- build_tools/cmake/base.cmake | 2 +- core/CMakeLists.txt | 10 +- .../double_pendulum/double_pendulum.cc | 3 +- .../external_project/double_pendulum.cc | 3 +- .../core/constraints/abstract_constraint.h | 18 +- .../core/constraints/distance_constraint.h | 22 +- .../core/constraints/frame_constraint.h | 32 +- .../core/constraints/joint_constraint.h | 20 +- .../core/constraints/sphere_constraint.h | 18 +- .../core/constraints/wheel_constraint.h | 20 +- .../jiminy/core/control/abstract_controller.h | 27 +- .../jiminy/core/control/controller_functor.h | 4 +- .../core/control/controller_functor.hxx | 10 +- core/include/jiminy/core/engine/engine.h | 4 +- .../jiminy/core/engine/engine_multi_robot.h | 68 ++-- core/include/jiminy/core/engine/system.h | 173 +++++---- core/include/jiminy/core/fwd.h | 11 +- .../jiminy/core/hardware/abstract_motor.h | 41 ++- .../jiminy/core/hardware/abstract_sensor.h | 117 +----- .../jiminy/core/hardware/abstract_sensor.hxx | 10 +- .../jiminy/core/hardware/basic_motors.h | 16 +- .../jiminy/core/hardware/basic_sensors.h | 42 +-- core/include/jiminy/core/hardware/fwd.h | 104 ++++++ .../jiminy/core/io/abstract_io_device.h | 106 +++--- .../jiminy/core/io/abstract_io_device.hxx | 53 +-- core/include/jiminy/core/io/file_device.h | 21 +- core/include/jiminy/core/io/json_loader.h | 7 +- core/include/jiminy/core/io/json_writer.h | 6 +- core/include/jiminy/core/io/memory_device.h | 34 +- core/include/jiminy/core/robot/model.h | 108 +++--- .../robot/pinocchio_overload_algorithms.h | 7 +- core/include/jiminy/core/robot/robot.h | 33 +- .../jiminy/core/solver/constraint_solvers.h | 57 ++- .../stepper/abstract_runge_kutta_stepper.h | 12 +- .../jiminy/core/stepper/abstract_stepper.h | 3 +- .../core/stepper/euler_explicit_stepper.h | 4 +- core/include/jiminy/core/stepper/lie_group.h | 26 +- .../core/stepper/runge_kutta4_stepper.h | 3 +- .../core/stepper/runge_kutta_dopri_stepper.h | 8 +- core/include/jiminy/core/telemetry/fwd.h | 36 ++ .../jiminy/core/telemetry/telemetry_data.h | 28 +- .../core/telemetry/telemetry_recorder.h | 38 +- .../jiminy/core/telemetry/telemetry_sender.h | 13 +- .../core/telemetry/telemetry_sender.hxx | 9 +- core/include/jiminy/core/traits.h | 73 +++- core/include/jiminy/core/utilities/helpers.h | 24 +- .../include/jiminy/core/utilities/helpers.hxx | 12 - core/include/jiminy/core/utilities/json.hxx | 4 +- .../include/jiminy/core/utilities/pinocchio.h | 33 +- core/include/jiminy/core/utilities/random.h | 176 +++++---- core/src/constraints/distance_constraint.cc | 95 +++-- core/src/constraints/frame_constraint.cc | 38 +- core/src/constraints/joint_constraint.cc | 35 +- core/src/constraints/sphere_constraint.cc | 25 +- core/src/constraints/wheel_constraint.cc | 23 +- core/src/control/abstract_controller.cc | 25 +- core/src/engine/engine_multi_robot.cc | 240 ++++++------- core/src/engine/system.cc | 105 ++---- core/src/hardware/abstract_motor.cc | 27 +- core/src/hardware/abstract_sensor.cc | 25 +- core/src/hardware/basic_motors.cc | 5 +- core/src/hardware/basic_sensors.cc | 61 +--- core/src/io/abstract_io_device.cc | 114 +----- core/src/io/file_device.cc | 67 ++-- core/src/io/json_loader.cc | 10 +- core/src/io/json_writer.cc | 4 +- core/src/io/memory_device.cc | 88 ++--- core/src/robot/model.cc | 98 ++--- core/src/robot/robot.cc | 45 +-- core/src/solver/constraint_solvers.cc | 20 +- .../stepper/abstract_runge_kutta_stepper.cc | 19 +- core/src/stepper/abstract_stepper.cc | 6 +- core/src/stepper/euler_explicit_stepper.cc | 6 - core/src/stepper/runge_kutta4_stepper.cc | 2 +- core/src/stepper/runge_kutta_dopri_stepper.cc | 6 +- core/src/telemetry/telemetry_data.cc | 12 +- core/src/telemetry/telemetry_recorder.cc | 41 ++- core/src/telemetry/telemetry_sender.cc | 7 - core/src/utilities/helpers.cc | 22 +- core/src/utilities/json.cc | 13 +- core/src/utilities/pinocchio.cc | 44 ++- core/src/utilities/random.cc | 340 +++++------------- core/unit/engine_sanity_check.cc | 2 + .../src/jiminy_py/viewer/meshcat/wrapper.py | 4 - .../jiminy_py/src/jiminy_py/viewer/viewer.py | 38 +- .../include/jiminy/python/functors.h | 34 +- .../include/jiminy/python/utilities.h | 162 +++++---- python/jiminy_pywrap/src/constraints.cc | 2 +- python/jiminy_pywrap/src/controllers.cc | 4 +- python/jiminy_pywrap/src/engine.cc | 10 +- python/jiminy_pywrap/src/functors.cc | 2 - python/jiminy_pywrap/src/helpers.cc | 8 +- python/jiminy_pywrap/src/module.cc | 30 +- python/jiminy_pywrap/src/robot.cc | 2 + python/jiminy_pywrap/src/sensors.cc | 6 +- soup/gtest/CMakeLists.txt | 4 +- soup/hdf5/CMakeLists.txt | 8 +- soup/jsoncpp/CMakeLists.txt | 6 +- 100 files changed, 1689 insertions(+), 2066 deletions(-) create mode 100644 core/include/jiminy/core/hardware/fwd.h create mode 100644 core/include/jiminy/core/telemetry/fwd.h diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index e6dca85b9..7f0f6eae7 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -31,7 +31,7 @@ jobs: CMAKE_C_COMPILER: "/usr/bin/clang" CMAKE_CXX_COMPILER: "/usr/bin/clang++" CMAKE_CXX_FLAGS: "-DEIGEN_MPL2_ONLY -Wno-deprecated-declarations" - MACOSX_DEPLOYMENT_TARGET: "10.15" + OSX_DEPLOYMENT_TARGET: "10.15" OSX_ARCHITECTURES: "x86_64;arm64" WHEEL_ARCH: "universal2" @@ -64,7 +64,7 @@ jobs: "${PYTHON_EXECUTABLE}" -m pip install delocate twine - name: Build project dependencies run: | - MACOSX_DEPLOYMENT_TARGET=${MACOSX_DEPLOYMENT_TARGET} OSX_ARCHITECTURES=${OSX_ARCHITECTURES} \ + OSX_DEPLOYMENT_TARGET=${OSX_DEPLOYMENT_TARGET} OSX_ARCHITECTURES=${OSX_ARCHITECTURES} \ BUILD_TYPE="${{ matrix.BUILD_TYPE }}" ./build_tools/build_install_deps_unix.sh "${PYTHON_EXECUTABLE}" -m pip install "torch==${TORCH_VERSION}" -f https://download.pytorch.org/whl/cpu "${PYTHON_EXECUTABLE}" -m pip install "gymnasium>=0.26,<0.29" "importlib-metadata>=3.3.0" @@ -82,7 +82,7 @@ jobs: cmake "${RootDir}" -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="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON \ -DBOOST_ROOT="${InstallDir}" -DBoost_INCLUDE_DIR="${InstallDir}/include" \ -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \ @@ -126,7 +126,7 @@ jobs: delocate-wheel -vvv --require-archs "${WHEEL_ARCH}" \ -w "${RootDir}/build/wheelhouse" "${RootDir}/build/pypi/dist/jiminy_py/"*.whl "${PYTHON_EXECUTABLE}" "${RootDir}/build_tools/wheel_addplat_macos.py" -vvv --rm-orig --clobber \ - -p "macosx_${MACOSX_DEPLOYMENT_TARGET//./_}_${WHEEL_ARCH}" "${RootDir}/build/wheelhouse/"*.whl + -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' diff --git a/build_tools/build_install_deps_unix.sh b/build_tools/build_install_deps_unix.sh index abfab96ee..b89f3cf27 100755 --- a/build_tools/build_install_deps_unix.sh +++ b/build_tools/build_install_deps_unix.sh @@ -10,14 +10,14 @@ if [ -z ${BUILD_TYPE} ]; then fi ### Set the macos sdk version min if undefined -if [ -z ${MACOSX_DEPLOYMENT_TARGET} ]; then - MACOSX_DEPLOYMENT_TARGET="10.15" - echo "MACOSX_DEPLOYMENT_TARGET is unset. Defaulting to '${MACOSX_DEPLOYMENT_TARGET}'." +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" + OSX_ARCHITECTURES="arm64" echo "OSX_ARCHITECTURES is unset. Defaulting to '${OSX_ARCHITECTURES}'." fi @@ -32,6 +32,8 @@ if [ -z ${CMAKE_CXX_COMPILER} ]; then fi ### Set common CMAKE_C/CXX_FLAGS +# '_LIBCPP_ENABLE_CXX17_REMOVED_UNARY_BINARY_FUNCTION' flag is required to compile `boost::container_hash::hash` +# with C++17 enabled from "old" releases of Boost. CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -D_LIBCPP_ENABLE_CXX17_REMOVED_UNARY_BINARY_FUNCTION -Wno-deprecated-declarations" if [ "${BUILD_TYPE}" == "Release" ]; then CMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -DNDEBUG -O3" @@ -43,8 +45,8 @@ fi echo "CMAKE_CXX_FLAGS: ${CMAKE_CXX_FLAGS}" ### Get the fullpath of Jiminy project -ScriptDir="$(cd "$(dirname "$0")" >/dev/null 2>&1 && pwd)" -RootDir="$(dirname $ScriptDir)" +ScriptDir="$(cd "$(dirname -- $0)" >/dev/null 2>&1 && pwd)" +RootDir="$(dirname -- ${ScriptDir})" ### Set the fullpath of the install directory, then creates it InstallDir="${RootDir}/install" @@ -79,7 +81,7 @@ unset Boost_ROOT # - Boost.Python == 1.75 fixes support of PyPy # - Boost.Python == 1.76 fixes error handling at import # - Boost >= 1.75 is required to compile ouf-of-the-box on MacOS for intel and Apple Silicon -# - Boost < 1.77 causes compilation failure with gcc-12. +# - Boost < 1.77 causes compilation failure with gcc-12 if not patched # - Boost >= 1.77 affects the memory layout to improve alignment, breaking retro-compatibility if [ ! -d "${RootDir}/boost" ]; then git clone --depth 1 https://github.com/boostorg/boost.git "${RootDir}/boost" @@ -206,6 +208,9 @@ git apply --reject --whitespace=fix "${RootDir}/build_tools/patch_deps_unix/pino # * Set the cmake cache variable CMAKE_PREFIX_PATH # * Set the environment variable Boost_DIR +# Must add toolset to search path for boost build system to find it +PATH="${PATH}:$(dirname -- ${CMAKE_C_COMPILER})" + ### Build and install the build tool b2 (build-ception !) cd "${RootDir}/boost" ./bootstrap.sh --prefix="${InstallDir}" --with-python="${PYTHON_EXECUTABLE}" @@ -234,7 +239,7 @@ else fi CMAKE_CXX_FLAGS_B2="-fPIC -std=c++11" if [ "${OSTYPE//[0-9.]/}" == "darwin" ]; then - CMAKE_CXX_FLAGS_B2="${CMAKE_CXX_FLAGS_B2} -mmacosx-version-min=${MACOSX_DEPLOYMENT_TARGET}" + CMAKE_CXX_FLAGS_B2="${CMAKE_CXX_FLAGS_B2} -mmacosx-version-min=${OSX_DEPLOYMENT_TARGET}" fi if grep -q ";" <<< "${OSX_ARCHITECTURES}" ; then CMAKE_CXX_FLAGS_B2="${CMAKE_CXX_FLAGS_B2} $(echo "-arch ${OSX_ARCHITECTURES}" | sed "s/;/ -arch /g")" @@ -249,7 +254,7 @@ mkdir -p "${RootDir}/boost/build" architecture= address-model=64 $DebugOptionsB2 \ threading=single link=static runtime-link=static \ cxxflags="${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_B2}" \ - linkflags="${CMAKE_CXX_FLAGS_B2}" \ + linkflags="${CMAKE_CXX_FLAGS_B2}" toolset="$(basename -- ${CMAKE_C_COMPILER})" \ variant="$BuildTypeB2" install -q -d0 -j2 ./b2 --prefix="${InstallDir}" --build-dir="${RootDir}/boost/build" \ @@ -258,7 +263,7 @@ mkdir -p "${RootDir}/boost/build" architecture= address-model=64 $DebugOptionsB2 \ threading=single link=shared runtime-link=shared \ cxxflags="${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_B2}" \ - linkflags="${CMAKE_CXX_FLAGS_B2}" \ + linkflags="${CMAKE_CXX_FLAGS_B2}" toolset="$(basename -- ${CMAKE_C_COMPILER})" \ variant="$BuildTypeB2" install -q -d0 -j2 #################################### Build and install eigen3 ########################################## @@ -277,7 +282,7 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" -DCMAKE_PREFIX_PATH="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DBUILD_SHARED_LIBS=ON -DBUILD_STATIC_LIBS=OFF \ -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" -DPYTHON_STANDARD_LAYOUT=ON \ @@ -295,12 +300,11 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON \ -DCMAKE_CXX_FLAGS_RELEASE_INIT="" -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -DTIXML_USE_STL" \ -DCMAKE_BUILD_TYPE="$BUILD_TYPE" - make install -j2 ############################## Build and install console_bridge ######################################## @@ -310,7 +314,7 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON \ -DCMAKE_CXX_FLAGS_RELEASE_INIT="" -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS}" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" @@ -330,7 +334,7 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" -DCMAKE_PREFIX_PATH="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON -DBUILD_TESTING=OFF \ -DCMAKE_CXX_FLAGS_RELEASE_INIT="" -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS}" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" @@ -343,7 +347,7 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DCMAKE_CXX_FLAGS_RELEASE_INIT="" -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS}" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 @@ -356,7 +360,7 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DGOOGLETEST_GIT=ON \ -DCMAKE_CXX_FLAGS_RELEASE_INIT="" -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS}" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" @@ -371,14 +375,14 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON \ -DASSIMP_BUILD_ASSIMP_TOOLS=OFF -DASSIMP_BUILD_ZLIB=ON -DASSIMP_BUILD_TESTS=OFF \ -DASSIMP_BUILD_SAMPLES=OFF -DBUILD_DOCS=OFF -DBUILD_TESTING=OFF \ -DCMAKE_C_FLAGS="${CMAKE_CXX_FLAGS} -DHAVE_HIDDEN" -DCMAKE_CXX_FLAGS_RELEASE_INIT="" \ -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -Wno-strict-overflow -Wno-tautological-compare -Wno-array-compare $( - ) -Wno-alloc-size-larger-than -Wno-unknown-warning-option -Wno-unknown-warning" \ + ) -Wno-alloc-size-larger-than -Wno-unknown-warning-option -Wno-unknown-warning -Wno-error=array-bounds" \ -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 @@ -389,7 +393,7 @@ cd "${RootDir}/hpp-fcl/third-parties/qhull/build" cmake "${RootDir}/hpp-fcl/third-parties/qhull" -Wno-dev -DCMAKE_CXX_STANDARD=17 \ -DCMAKE_C_COMPILER="${CMAKE_C_COMPILER}" -DCMAKE_CXX_COMPILER="${CMAKE_CXX_COMPILER}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON \ -DCMAKE_C_FLAGS="${CMAKE_CXX_FLAGS}" -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -Wno-conversion" \ @@ -401,7 +405,7 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" -DCMAKE_PREFIX_PATH="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DBUILD_SHARED_LIBS=ON -DBUILD_STATIC_LIBS=OFF \ -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" -DPYTHON_STANDARD_LAYOUT=ON \ @@ -423,7 +427,7 @@ 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}" \ -DCMAKE_INSTALL_PREFIX="${InstallDir}" -DCMAKE_PREFIX_PATH="${InstallDir}" \ - -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${MACOSX_DEPLOYMENT_TARGET}" \ + -DCMAKE_OSX_ARCHITECTURES="${OSX_ARCHITECTURES}" -DCMAKE_OSX_DEPLOYMENT_TARGET="${OSX_DEPLOYMENT_TARGET}" \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ -DBUILD_SHARED_LIBS=ON -DBUILD_STATIC_LIBS=OFF \ -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" -DPYTHON_STANDARD_LAYOUT=ON \ diff --git a/build_tools/cmake/base.cmake b/build_tools/cmake/base.cmake index 0e7a7e0d2..124ea2b31 100644 --- a/build_tools/cmake/base.cmake +++ b/build_tools/cmake/base.cmake @@ -55,7 +55,7 @@ else() -Wno-unknown-warning -Wno-undefined-var-template \ -Wno-long-long -Wno-error=maybe-uninitialized \ -Wno-error=uninitialized -Wno-error=deprecated \ - -Wno-error=array-bounds") + -Wno-error=array-bounds -Wno-error=redundant-move") endif() # Shared libraries need PIC diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index f963b1735..776bdbdd3 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -114,7 +114,7 @@ target_include_directories(${PROJECT_NAME}-object PUBLIC ) # Configure jsoncpp and hdf5 dependencies. -# Note that defining only BUILD_INTERFACE will prevent the header files to be exported, +# Note that defining only BUILD_INTERFACE will prevent the header files from being exported, # thus not making them available by a side-project depending on Jiminy. However, since # jsoncpp is installed on the system as an external project, those side-projects must # simply call find_package(jsoncpp) and it will be included automatically. @@ -124,7 +124,7 @@ target_include_directories(${PROJECT_NAME}-object SYSTEM PUBLIC $ ) -# Linking with other libraries (in such a way to avoid any warnings compiling them) +# Linking with other libraries (as a way to avoid any warnings compiling them) target_link_libraries_system(${PROJECT_NAME}-object pinocchio::pinocchio hpp-fcl::hpp-fcl Eigen3::Eigen) if(NOT "${urdfdom_LIBRARIES}" MATCHES ".*tinyxml.*") list(APPEND urdfdom_LIBRARIES "tinyxml") @@ -133,7 +133,7 @@ target_link_libraries(${PROJECT_NAME}-object ${urdfdom_LIBRARIES}) target_link_libraries(${PROJECT_NAME}-object jsoncpp::jsoncpp hdf5::hdf5_cpp hdf5::hdf5 hdf5::zlib) # Beware the order is critical ! if(WIN32) # Adding this dependency on Windows is necessary since HDF5 >= 1.14.3. - # Hopefully this broken transitive dependency when build static libary + # Hopefully this broken transitive dependency when building static libary # will be fixed. See: https://github.com/HDFGroup/hdf5/issues/3663 target_link_libraries(${PROJECT_NAME}-object shlwapi) endif() @@ -143,7 +143,7 @@ if(UNIX AND NOT APPLE) target_link_libraries(${PROJECT_NAME}-object ${CMAKE_DL_LIBS} -lrt) endif() -# Create both static and shared libraries from intermediary object +# Create both static and shared libraries from the intermediary object add_library(${PROJECT_NAME} STATIC $) target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}-object) add_library(${LIBRARY_NAME} SHARED $) @@ -171,7 +171,7 @@ if(BUILD_EXAMPLES) add_subdirectory(examples) endif() -# Specialize jiminy core configuration file and install it +# Specialize Jiminy core configuration file and install it set(JIMINY_CONFIG_IN "${CMAKE_SOURCE_DIR}/build_tools/cmake/jiminyConfig.cmake.in") set(JIMINY_CONFIG_OUT "${CMAKE_BINARY_DIR}/cmake/jiminyConfig.cmake") configure_file(${JIMINY_CONFIG_IN} ${JIMINY_CONFIG_OUT} @ONLY) diff --git a/core/examples/double_pendulum/double_pendulum.cc b/core/examples/double_pendulum/double_pendulum.cc index 897e7f79d..378cff4b3 100644 --- a/core/examples/double_pendulum/double_pendulum.cc +++ b/core/examples/double_pendulum/double_pendulum.cc @@ -7,9 +7,10 @@ #include #include "jiminy/core/fwd.h" +#include "jiminy/core/hardware/fwd.h" +#include "jiminy/core/telemetry/fwd.h" #include "jiminy/core/utilities/helpers.h" #include "jiminy/core/io/file_device.h" -#include "jiminy/core/telemetry/telemetry_recorder.h" #include "jiminy/core/hardware/abstract_sensor.h" #include "jiminy/core/hardware/basic_motors.h" #include "jiminy/core/control/controller_functor.h" diff --git a/core/examples/external_project/double_pendulum.cc b/core/examples/external_project/double_pendulum.cc index 78ffee9d6..776fb51f5 100644 --- a/core/examples/external_project/double_pendulum.cc +++ b/core/examples/external_project/double_pendulum.cc @@ -7,9 +7,10 @@ #include #include "jiminy/core/fwd.h" +#include "jiminy/core/hardware/fwd.h" +#include "jiminy/core/telemetry/fwd.h" #include "jiminy/core/utilities/helpers.h" #include "jiminy/core/io/file_device.h" -#include "jiminy/core/telemetry/telemetry_recorder.h" #include "jiminy/core/hardware/abstract_sensor.h" #include "jiminy/core/hardware/basic_motors.h" #include "jiminy/core/control/controller_functor.h" diff --git a/core/include/jiminy/core/constraints/abstract_constraint.h b/core/include/jiminy/core/constraints/abstract_constraint.h index 2f2ccd0e2..ae5af5817 100644 --- a/core/include/jiminy/core/constraints/abstract_constraint.h +++ b/core/include/jiminy/core/constraints/abstract_constraint.h @@ -22,7 +22,7 @@ namespace jiminy DISABLE_COPY(AbstractConstraintBase) public: - AbstractConstraintBase() = default; + explicit AbstractConstraintBase() = default; virtual ~AbstractConstraintBase(); /// \brief Refresh the internal buffers and proxies. @@ -77,25 +77,25 @@ namespace jiminy public: /// \brief Lambda multipliers. - Eigen::VectorXd lambda_; + Eigen::VectorXd lambda_{}; protected: /// \brief Model on which the constraint operates. - std::weak_ptr model_; + std::weak_ptr model_{}; /// \brief Flag to indicate whether the constraint has been attached to a model. - bool isAttached_; + bool isAttached_{false}; /// \brief Flag to indicate whether the constraint is enabled. /// /// \remarks Handling of this flag is done at Robot level. - bool isEnabled_; + bool isEnabled_{false}; /// \brief Position-related baumgarte stabilization gain. - double kp_; + double kp_{0.0}; /// \brief Velocity-related baumgarte stabilization gain. - double kd_; + double kd_{0.0}; /// \brief Jacobian of the constraint. - Eigen::MatrixXd jacobian_; + Eigen::MatrixXd jacobian_{}; /// \brief Drift of the constraint. - Eigen::VectorXd drift_; + Eigen::VectorXd drift_{}; }; template diff --git a/core/include/jiminy/core/constraints/distance_constraint.h b/core/include/jiminy/core/constraints/distance_constraint.h index 3d0b0cccc..d612a17fe 100644 --- a/core/include/jiminy/core/constraints/distance_constraint.h +++ b/core/include/jiminy/core/constraints/distance_constraint.h @@ -20,15 +20,15 @@ namespace jiminy auto shared_from_this() { return shared_from(this); } public: - DistanceConstraint(const std::string & firstFrameName, - const std::string & secondFrameName); + explicit DistanceConstraint(const std::string & firstFrameName, + const std::string & secondFrameName) noexcept; virtual ~DistanceConstraint() = default; - const std::vector & getFramesNames() const; - const std::vector & getFramesIdx() const; + const std::array & getFramesNames() const noexcept; + const std::array & getFramesIdx() const noexcept; hresult_t setReferenceDistance(double distanceRef); - double getReferenceDistance() const; + double getReferenceDistance() const noexcept; virtual hresult_t reset(const Eigen::VectorXd & q, const Eigen::VectorXd & v) override final; @@ -38,15 +38,13 @@ namespace jiminy private: /// \brief Names of the frames on which the constraint operates. - std::vector framesNames_; + std::array frameNames_; /// \brief Corresponding frames indices. - std::vector framesIdx_; + std::array frameIndices_{{0, 0}}; /// \brief Reference Distance between the frames - double distanceRef_; - /// \brief Stores first frame jacobian in world. - Matrix6Xd firstFrameJacobian_; - /// \brief Stores second frame jacobian in world. - Matrix6Xd secondFrameJacobian_; + double distanceRef_{0.0}; + /// \brief Stores frame jacobians in world. + std::array frameJacobians_{}; }; } diff --git a/core/include/jiminy/core/constraints/frame_constraint.h b/core/include/jiminy/core/constraints/frame_constraint.h index 21fcc7824..a2e2ad8b1 100644 --- a/core/include/jiminy/core/constraints/frame_constraint.h +++ b/core/include/jiminy/core/constraints/frame_constraint.h @@ -23,21 +23,21 @@ namespace jiminy public: /// \param[in] frameName Name of the frame on which the constraint is to be /// applied. - FrameConstraint( - const std::string & frameName, - const std::array & maskDoFs = {{true, true, true, true, true, true}}); + explicit FrameConstraint(const std::string & frameName, + const std::array & maskDoFs = { + {true, true, true, true, true, true}}) noexcept; virtual ~FrameConstraint() = default; - const std::string & getFrameName() const; - pinocchio::FrameIndex getFrameIdx() const; + const std::string & getFrameName() const noexcept; + pinocchio::FrameIndex getFrameIdx() const noexcept; - const std::vector & getDofsFixed() const; + const std::vector & getDofsFixed() const noexcept; - void setReferenceTransform(const pinocchio::SE3 & transformRef); - const pinocchio::SE3 & getReferenceTransform() const; + void setReferenceTransform(const pinocchio::SE3 & transformRef) noexcept; + const pinocchio::SE3 & getReferenceTransform() const noexcept; - void setNormal(const Eigen::Vector3d & normal); - const Eigen::Matrix3d & getLocalFrame() const; + void setNormal(const Eigen::Vector3d & normal) noexcept; + const Eigen::Matrix3d & getLocalFrame() const noexcept; virtual hresult_t reset(const Eigen::VectorXd & q, const Eigen::VectorXd & v) override final; @@ -49,19 +49,19 @@ namespace jiminy /// \brief Name of the frame on which the constraint operates. const std::string frameName_; /// \brief Corresponding frame index. - pinocchio::FrameIndex frameIdx_; + pinocchio::FrameIndex frameIdx_{0}; /// \brief Degrees of freedom to fix. std::vector dofsFixed_; /// \brief Reference pose of the frame to enforce. - pinocchio::SE3 transformRef_; + pinocchio::SE3 transformRef_{}; /// \brief Normal direction locally at the interface. - Eigen::Vector3d normal_; + Eigen::Vector3d normal_{}; /// \brief Rotation matrix of the local frame in which to apply masking - Eigen::Matrix3d rotationLocal_; + Eigen::Matrix3d rotationLocal_{Eigen::Matrix3d::Identity()}; /// \brief Stores full frame jacobian in reference frame. - Matrix6Xd frameJacobian_; + Matrix6Xd frameJacobian_{}; /// \brief Stores full frame drift in reference frame. - pinocchio::Motion frameDrift_; + pinocchio::Motion frameDrift_{}; }; } // namespace jiminy diff --git a/core/include/jiminy/core/constraints/joint_constraint.h b/core/include/jiminy/core/constraints/joint_constraint.h index 28a05f5d8..24a635e11 100644 --- a/core/include/jiminy/core/constraints/joint_constraint.h +++ b/core/include/jiminy/core/constraints/joint_constraint.h @@ -21,17 +21,17 @@ namespace jiminy public: /// \param[in] jointName Name of the joint. - JointConstraint(const std::string & jointName); + explicit JointConstraint(const std::string & jointName) noexcept; virtual ~JointConstraint() = default; - const std::string & getJointName() const; - pinocchio::JointIndex getJointIdx() const; + const std::string & getJointName() const noexcept; + pinocchio::JointIndex getJointModelIdx() const noexcept; - void setReferenceConfiguration(const Eigen::VectorXd & configurationRef); - const Eigen::VectorXd & getReferenceConfiguration() const; + void setReferenceConfiguration(const Eigen::VectorXd & configurationRef) noexcept; + const Eigen::VectorXd & getReferenceConfiguration() const noexcept; - void setRotationDir(bool isReversed); - bool getRotationDir(); + void setRotationDir(bool isReversed) noexcept; + bool getRotationDir() noexcept; virtual hresult_t reset(const Eigen::VectorXd & q, const Eigen::VectorXd & v) override final; @@ -43,11 +43,11 @@ namespace jiminy /// \brief Name of the joint on which the constraint operates. std::string jointName_; /// \brief Corresponding joint index. - pinocchio::JointIndex jointIdx_; + pinocchio::JointIndex jointModelIdx_{0}; /// \brief Reference position of the joint to enforce. - Eigen::VectorXd configurationRef_; + Eigen::VectorXd configurationRef_{}; /// \brief Whether to reverse the sign of the constraint. - bool isReversed_; + bool isReversed_{false}; }; } diff --git a/core/include/jiminy/core/constraints/sphere_constraint.h b/core/include/jiminy/core/constraints/sphere_constraint.h index 54f1ac954..881bb504f 100644 --- a/core/include/jiminy/core/constraints/sphere_constraint.h +++ b/core/include/jiminy/core/constraints/sphere_constraint.h @@ -29,14 +29,14 @@ namespace jiminy /// \param[in] groundNormal Normal to the ground in the world as a unit vector. SphereConstraint(const std::string & frameName, double sphereRadius, - const Eigen::Vector3d & groundNormal = Eigen::Vector3d::UnitZ()); + const Eigen::Vector3d & groundNormal = Eigen::Vector3d::UnitZ()) noexcept; virtual ~SphereConstraint() = default; - const std::string & getFrameName() const; - pinocchio::FrameIndex getFrameIdx() const; + const std::string & getFrameName() const noexcept; + pinocchio::FrameIndex getFrameIdx() const noexcept; - void setReferenceTransform(const pinocchio::SE3 & transformRef); - const pinocchio::SE3 & getReferenceTransform() const; + void setReferenceTransform(const pinocchio::SE3 & transformRef) noexcept; + const pinocchio::SE3 & getReferenceTransform() const noexcept; virtual hresult_t reset(const Eigen::VectorXd & /* q */, const Eigen::VectorXd & /* v */) override final; @@ -48,17 +48,17 @@ namespace jiminy /// \brief Name of the frame on which the constraint operates. std::string frameName_; /// \brief Corresponding frame index. - pinocchio::FrameIndex frameIdx_; + pinocchio::FrameIndex frameIdx_{0}; /// \brief Sphere radius. double radius_; /// \brief Ground normal, world frame. Eigen::Vector3d normal_; /// \brief Skew of ground normal, in world frame, scaled by radius. - Eigen::Matrix3d skewRadius_; + Eigen::Matrix3d skewRadius_{pinocchio::alphaSkew(radius_, normal_)}; /// \brief Reference pose of the frame to enforce. - pinocchio::SE3 transformRef_; + pinocchio::SE3 transformRef_{}; /// \brief Stores full frame jacobian in world. - Matrix6Xd frameJacobian_; + Matrix6Xd frameJacobian_{}; }; } diff --git a/core/include/jiminy/core/constraints/wheel_constraint.h b/core/include/jiminy/core/constraints/wheel_constraint.h index 91ebe2321..d2ddc348b 100644 --- a/core/include/jiminy/core/constraints/wheel_constraint.h +++ b/core/include/jiminy/core/constraints/wheel_constraint.h @@ -31,14 +31,14 @@ namespace jiminy WheelConstraint(const std::string & frameName, double wheelRadius, const Eigen::Vector3d & groundNormal, - const Eigen::Vector3d & wheelAxis); + const Eigen::Vector3d & wheelAxis) noexcept; virtual ~WheelConstraint() = default; - const std::string & getFrameName() const; - pinocchio::FrameIndex getFrameIdx() const; + const std::string & getFrameName() const noexcept; + pinocchio::FrameIndex getFrameIdx() const noexcept; - void setReferenceTransform(const pinocchio::SE3 & transformRef); - const pinocchio::SE3 & getReferenceTransform() const; + void setReferenceTransform(const pinocchio::SE3 & transformRef) noexcept; + const pinocchio::SE3 & getReferenceTransform() const noexcept; virtual hresult_t reset(const Eigen::VectorXd & /* q */, const Eigen::VectorXd & /* v */) override final; @@ -50,7 +50,7 @@ namespace jiminy /// \brief Name of the frame on which the constraint operates. std::string frameName_; /// \brief Corresponding frame index. - pinocchio::FrameIndex frameIdx_; + pinocchio::FrameIndex frameIdx_{0}; /// \brief Wheel radius. double radius_; /// \brief Ground normal, world frame. @@ -58,13 +58,13 @@ namespace jiminy /// \brief Wheel axis, local frame. Eigen::Vector3d axis_; /// \brief Skew matrix of wheel axis, in world frame, scaled by radius. - Eigen::Matrix3d skewRadius_; + Eigen::Matrix3d skewRadius_{}; /// \brief Derivative of skew matrix of wheel axis, in world frame, scaled by radius. - Eigen::Matrix3d dskewRadius_; + Eigen::Matrix3d dskewRadius_{}; /// \brief Reference pose of the frame to enforce. - pinocchio::SE3 transformRef_; + pinocchio::SE3 transformRef_{}; /// \brief Stores full frame jacobian in world. - Matrix6Xd frameJacobian_; + Matrix6Xd frameJacobian_{}; }; } diff --git a/core/include/jiminy/core/control/abstract_controller.h b/core/include/jiminy/core/control/abstract_controller.h index 4eb6a9c11..b9c80ae38 100644 --- a/core/include/jiminy/core/control/abstract_controller.h +++ b/core/include/jiminy/core/control/abstract_controller.h @@ -4,7 +4,7 @@ #include #include "jiminy/core/fwd.h" -#include "jiminy/core/telemetry/telemetry_sender.h" +#include "jiminy/core/hardware/fwd.h" #include "jiminy/core/hardware/abstract_sensor.h" @@ -13,6 +13,7 @@ namespace jiminy /// \brief Namespace of the telemetry object. inline constexpr std::string_view CONTROLLER_TELEMETRY_NAMESPACE{"HighLevelController"}; + class TelemetrySender; class TelemetryData; class Robot; class Engine; @@ -49,8 +50,8 @@ namespace jiminy DISABLE_COPY(AbstractController) public: - AbstractController(); - virtual ~AbstractController() = default; + explicit AbstractController() noexcept; + virtual ~AbstractController(); /// \brief Set the parameters of the controller. /// @@ -134,7 +135,7 @@ namespace jiminy Eigen::VectorXd & uCustom) = 0; /// \brief Dictionary with the parameters of the controller. - GenericConfig getOptions() const; + GenericConfig getOptions() const noexcept; /// \brief Set the configuration options of the controller. /// @@ -198,27 +199,27 @@ namespace jiminy public: /// \brief Structure with the parameters of the controller. - std::unique_ptr baseControllerOptions_; + std::unique_ptr baseControllerOptions_{nullptr}; /// \brief Robot for which to compute the command and internal dynamics must be computed. - std::weak_ptr robot_; - SensorsDataMap sensorsData_; + std::weak_ptr robot_{}; + SensorsDataMap sensorsData_{}; protected: /// \brief Flag to determine whether the controller has been initialized or not. - bool isInitialized_; + bool isInitialized_{false}; /// \brief Flag to determine whether the telemetry of the controller has been initialized. - bool isTelemetryConfigured_; + bool isTelemetryConfigured_{false}; /// \brief Dictionary with the parameters of the controller. - GenericConfig ctrlOptionsHolder_; + GenericConfig ctrlOptionsHolder_{}; /// \brief Telemetry sender used to register and update telemetry variables. - TelemetrySender telemetrySender_; + std::unique_ptr telemetrySender_; private: /// \brief Vector of dynamically registered telemetry variables. static_map_t> - registeredVariables_; + registeredVariables_{}; /// \brief Vector of dynamically registered telemetry constants. - static_map_t registeredConstants_; + static_map_t registeredConstants_{}; }; } diff --git a/core/include/jiminy/core/control/controller_functor.h b/core/include/jiminy/core/control/controller_functor.h index ce838e12d..59ebf169c 100644 --- a/core/include/jiminy/core/control/controller_functor.h +++ b/core/include/jiminy/core/control/controller_functor.h @@ -32,8 +32,8 @@ namespace jiminy /// /// \param[in] commandFct 'Callable' computing the command. /// \param[in] internalDynamicsFct 'Callable' computing the internal dynamics. - ControllerFunctor(F1 & commandFct, F2 & internalDynamicsFct); - ControllerFunctor(F1 && commandFct, F2 && internalDynamicsFct); + explicit ControllerFunctor(F1 & commandFct, F2 & internalDynamicsFct) noexcept; + explicit ControllerFunctor(F1 && commandFct, F2 && internalDynamicsFct) noexcept; virtual ~ControllerFunctor() = default; diff --git a/core/include/jiminy/core/control/controller_functor.hxx b/core/include/jiminy/core/control/controller_functor.hxx index 0b2cf33da..ef26dd8c0 100644 --- a/core/include/jiminy/core/control/controller_functor.hxx +++ b/core/include/jiminy/core/control/controller_functor.hxx @@ -6,15 +6,17 @@ namespace jiminy { template - ControllerFunctor::ControllerFunctor(F1 & commandFct, F2 & internalDynamicsFct) : + ControllerFunctor::ControllerFunctor(F1 & commandFct, + F2 & internalDynamicsFct) noexcept : AbstractController(), - commandFct_(commandFct), - internalDynamicsFct_(internalDynamicsFct) + commandFct_{commandFct}, + internalDynamicsFct_{internalDynamicsFct} { } template - ControllerFunctor::ControllerFunctor(F1 && commandFct, F2 && internalDynamicsFct) : + ControllerFunctor::ControllerFunctor(F1 && commandFct, + F2 && internalDynamicsFct) noexcept : AbstractController(), commandFct_(std::move(commandFct)), internalDynamicsFct_(std::move(internalDynamicsFct)) diff --git a/core/include/jiminy/core/engine/engine.h b/core/include/jiminy/core/engine/engine.h index d772361d4..2621e1956 100644 --- a/core/include/jiminy/core/engine/engine.h +++ b/core/include/jiminy/core/engine/engine.h @@ -15,8 +15,8 @@ namespace jiminy DISABLE_COPY(Engine) public: - Engine() = default; - virtual ~Engine() = default; + explicit Engine() = default; + ~Engine() = default; hresult_t initialize(std::shared_ptr robot, std::shared_ptr controller, diff --git a/core/include/jiminy/core/engine/engine_multi_robot.h b/core/include/jiminy/core/engine/engine_multi_robot.h index 0b48258a1..56a44286e 100644 --- a/core/include/jiminy/core/engine/engine_multi_robot.h +++ b/core/include/jiminy/core/engine/engine_multi_robot.h @@ -5,7 +5,6 @@ #include #include "jiminy/core/fwd.h" -#include "jiminy/core/telemetry/telemetry_sender.h" #include "jiminy/core/engine/system.h" @@ -43,6 +42,7 @@ namespace jiminy class AbstractStepper; class TelemetryData; class TelemetryRecorder; + class TelemetrySender; struct LogData; using ForceCouplingRegister = std::vector; @@ -69,18 +69,18 @@ namespace jiminy } public: - uint32_t iter; - uint32_t iterFailed; - double t; - double tPrev; + uint32_t iter{0U}; + uint32_t iterFailed{0U}; + double t{0.0}; + double tPrev{0.0}; /// \brief Internal buffer used for Kahan algorithm storing the residual sum of errors. - double tError; - double dt; - double dtLargest; - double dtLargestPrev; - std::vector qSplit; - std::vector vSplit; - std::vector aSplit; + double tError{0.0}; + double dt{0.0}; + double dtLargest{0.0}; + double dtLargestPrev{0.0}; + std::vector qSplit{}; + std::vector vSplit{}; + std::vector aSplit{}; }; class JIMINY_DLLAPI EngineMultiRobot @@ -327,8 +327,8 @@ namespace jiminy DISABLE_COPY(EngineMultiRobot) public: - EngineMultiRobot(); - virtual ~EngineMultiRobot(); + explicit EngineMultiRobot() noexcept; + ~EngineMultiRobot(); hresult_t addSystem(const std::string & systemName, std::shared_ptr robot, @@ -488,11 +488,11 @@ namespace jiminy hresult_t getForcesProfile(const std::string & systemName, const ForceProfileRegister *& forcesProfilePtr) const; - GenericConfig getOptions() const; + GenericConfig getOptions() const noexcept; hresult_t setOptions(const GenericConfig & engineOptions); bool getIsTelemetryConfigured() const; std::vector getSystemsNames() const; - hresult_t getSystemIdx(const std::string & systemName, int32_t & systemIdx) const; + hresult_t getSystemIdx(const std::string & systemName, std::ptrdiff_t & systemIdx) const; hresult_t getSystem(const std::string & systemName, systemHolder_t *& system); hresult_t getSystemState(const std::string & systemName, const systemState_t *& systemState) const; @@ -654,30 +654,30 @@ namespace jiminy const vector_aligned_t & fext); public: - std::unique_ptr engineOptions_; - std::vector systems_; + std::unique_ptr engineOptions_{nullptr}; + std::vector systems_{}; protected: - bool isTelemetryConfigured_; - bool isSimulationRunning_; - GenericConfig engineOptionsHolder_; + bool isTelemetryConfigured_{false}; + bool isSimulationRunning_{false}; + GenericConfig engineOptionsHolder_{}; private: - std::unique_ptr timer_; - contactModel_t contactModel_; - TelemetrySender telemetrySender_; + std::unique_ptr timer_{std::make_unique()}; + contactModel_t contactModel_{contactModel_t::UNSUPPORTED}; + std::unique_ptr telemetrySender_; std::shared_ptr telemetryData_; std::unique_ptr telemetryRecorder_; - std::unique_ptr stepper_; - double stepperUpdatePeriod_; - StepperState stepperState_; - vector_aligned_t systemsDataHolder_; - ForceCouplingRegister forcesCoupling_; - vector_aligned_t contactForcesPrev_; - vector_aligned_t fPrev_; - vector_aligned_t aPrev_; - std::vector energy_; - std::shared_ptr logData_; + std::unique_ptr stepper_{nullptr}; + double stepperUpdatePeriod_{INF}; + StepperState stepperState_{}; + vector_aligned_t systemsDataHolder_{}; + ForceCouplingRegister forcesCoupling_{}; + vector_aligned_t contactForcesPrev_{}; + vector_aligned_t fPrev_{}; + vector_aligned_t aPrev_{}; + std::vector energy_{}; + std::shared_ptr logData_{nullptr}; }; } diff --git a/core/include/jiminy/core/engine/system.h b/core/include/jiminy/core/engine/system.h index 0032cd70d..a3d8cc0f0 100644 --- a/core/include/jiminy/core/engine/system.h +++ b/core/include/jiminy/core/engine/system.h @@ -14,7 +14,6 @@ namespace jiminy { class Robot; class AbstractConstraintSolver; - class AbstractConstraintBase; class AbstractController; class LockGuardLocal; @@ -25,36 +24,37 @@ namespace jiminy struct JIMINY_DLLAPI ForceProfile { public: - ForceProfile() = default; - ForceProfile(const std::string & frameNameIn, - pinocchio::FrameIndex frameIdxIn, - double updatePeriodIn, - const ForceProfileFunctor & forceFctIn); + explicit ForceProfile() = default; + explicit ForceProfile(const std::string & frameNameIn, + pinocchio::FrameIndex frameIdxIn, + double updatePeriodIn, + const ForceProfileFunctor & forceFctIn) noexcept; public: - std::string frameName; - pinocchio::FrameIndex frameIdx; - double updatePeriod; - pinocchio::Force forcePrev; - ForceProfileFunctor forceFct; + std::string frameName{}; + pinocchio::FrameIndex frameIdx{0}; + double updatePeriod{0.0}; + pinocchio::Force forcePrev{pinocchio::Force::Zero()}; + ForceProfileFunctor forceFct{}; }; struct JIMINY_DLLAPI ForceImpulse { public: - ForceImpulse() = default; - ForceImpulse(const std::string & frameNameIn, - pinocchio::FrameIndex frameIdxIn, - double tIn, - double dtIn, - const pinocchio::Force & FIn); + explicit ForceImpulse() = default; + explicit ForceImpulse(const std::string & frameNameIn, + pinocchio::FrameIndex frameIdxIn, + double tIn, + double dtIn, + const pinocchio::Force & FIn) noexcept; + public: - std::string frameName; - pinocchio::FrameIndex frameIdx; - double t; - double dt; - pinocchio::Force F; + std::string frameName{}; + pinocchio::FrameIndex frameIdx{0}; + double t{0.0}; + double dt{0.0}; + pinocchio::Force F{}; }; using ForceCouplingFunctor = std::function; @@ -98,49 +98,38 @@ namespace jiminy struct JIMINY_DLLAPI systemHolder_t { - public: - systemHolder_t(); - systemHolder_t(const std::string & systemNameIn, - std::shared_ptr robotIn, - std::shared_ptr controllerIn, - CallbackFunctor callbackFctIn); - systemHolder_t(const systemHolder_t & other) = default; - systemHolder_t(systemHolder_t && other) = default; - systemHolder_t & operator=(const systemHolder_t & other) = default; - systemHolder_t & operator=(systemHolder_t && other) = default; - ~systemHolder_t() = default; - - public: - std::string name; - std::shared_ptr robot; - std::shared_ptr controller; - CallbackFunctor callbackFct; + std::string name{}; + std::shared_ptr robot{nullptr}; + std::shared_ptr controller{nullptr}; + CallbackFunctor callbackFct{+[](double /* t */, + const Eigen::VectorXd & /* q */, + const Eigen::VectorXd & /* v */) -> bool + { + return false; + }}; }; struct JIMINY_DLLAPI systemState_t { public: - // Non-default constructor to be considered initialized even if not - systemState_t(); - hresult_t initialize(const Robot & robot); bool getIsInitialized() const; void clear(); public: - Eigen::VectorXd q; - Eigen::VectorXd v; - Eigen::VectorXd a; - Eigen::VectorXd command; - Eigen::VectorXd u; - Eigen::VectorXd uMotor; - Eigen::VectorXd uInternal; - Eigen::VectorXd uCustom; - ForceVector fExternal; + Eigen::VectorXd q{}; + Eigen::VectorXd v{}; + Eigen::VectorXd a{}; + Eigen::VectorXd command{}; + Eigen::VectorXd u{}; + Eigen::VectorXd uMotor{}; + Eigen::VectorXd uInternal{}; + Eigen::VectorXd uCustom{}; + ForceVector fExternal{}; private: - bool isInitialized_; + bool isInitialized_{false}; }; struct JIMINY_DLLAPI systemDataHolder_t @@ -159,43 +148,43 @@ namespace jiminy ~systemDataHolder_t(); public: - std::unique_ptr robotLock; + std::unique_ptr robotLock{nullptr}; - ForceProfileRegister forcesProfile; - ForceImpulseRegister forcesImpulse; + ForceProfileRegister forcesProfile{}; + ForceImpulseRegister forcesImpulse{}; /// \brief Ordered list without repetitions of all the start/end times of the forces. - std::set forcesImpulseBreaks; + std::set forcesImpulseBreaks{}; /// \brief Time of the next breakpoint associated with the impulse forces. - std::set::const_iterator forcesImpulseBreakNextIt; + std::set::const_iterator forcesImpulseBreakNextIt{}; /// \brief Set of flags tracking whether each force is active. /// /// \details This flag is used to handle t-, t+ properly. Without it, it is impossible to /// determine at time t if the force is active or not. - std::vector forcesImpulseActive; + std::vector forcesImpulseActive{}; - uint32_t successiveSolveFailed; - std::unique_ptr constraintSolver; + uint32_t successiveSolveFailed{0}; + std::unique_ptr constraintSolver{nullptr}; /// \brief Store copy of constraints register for fast access. - constraintsHolder_t constraintsHolder; + constraintsHolder_t constraintsHolder{}; /// \brief Contact forces for each contact frames in local frame. - ForceVector contactFramesForces; + ForceVector contactFramesForces{}; /// \brief Contact forces for each geometries of each collision bodies in local frame. - vector_aligned_t collisionBodiesForces; + vector_aligned_t collisionBodiesForces{}; /// \brief Jacobian of the joints in local frame. Used for computing `data.u`. - std::vector jointsJacobians; + std::vector jointsJacobians{}; - std::vector logFieldnamesPosition; - std::vector logFieldnamesVelocity; - std::vector logFieldnamesAcceleration; - std::vector logFieldnamesForceExternal; - std::vector logFieldnamesCommand; - std::vector logFieldnamesMotorEffort; - std::string logFieldnameEnergy; + std::vector logFieldnamesPosition{}; + std::vector logFieldnamesVelocity{}; + std::vector logFieldnamesAcceleration{}; + std::vector logFieldnamesForceExternal{}; + std::vector logFieldnamesCommand{}; + std::vector logFieldnamesMotorEffort{}; + std::string logFieldnameEnergy{}; /// \brief Internal buffer with the state for the integration loop. - systemState_t state; + systemState_t state{}; /// \brief Internal state for the integration loop at the end of the previous iteration. - systemState_t statePrev; + systemState_t statePrev{}; }; } diff --git a/core/include/jiminy/core/fwd.h b/core/include/jiminy/core/fwd.h index 6b9ef2ded..d3781e2b4 100644 --- a/core/include/jiminy/core/fwd.h +++ b/core/include/jiminy/core/fwd.h @@ -137,10 +137,10 @@ namespace jiminy this->damping == other.damping && this->inertia == other.inertia); }; - std::string frameName; - Eigen::Vector3d stiffness; - Eigen::Vector3d damping; - Eigen::Vector3d inertia; + std::string frameName{}; + Eigen::Vector3d stiffness{}; + Eigen::Vector3d damping{}; + Eigen::Vector3d inertia{}; }; using FlexibilityConfig = std::vector; @@ -162,9 +162,6 @@ namespace jiminy FlexibilityConfig, std::unordered_map>::type>; - struct SensorDataTypeMap; - using SensorsDataMap = std::unordered_map; - // Generic utilities used everywhere template std::string toString(Args &&... args) diff --git a/core/include/jiminy/core/hardware/abstract_motor.h b/core/include/jiminy/core/hardware/abstract_motor.h index 0a698e765..7f0d1303f 100644 --- a/core/include/jiminy/core/hardware/abstract_motor.h +++ b/core/include/jiminy/core/hardware/abstract_motor.h @@ -2,7 +2,7 @@ /// /// \details Any motor must inherit from this base class and implement its virtual methods. /// -/// \remark Each motor added to a Jiminy Robot is downcasted as an instance of AbstractMotorBase +/// \remark Each motor added to a Jiminy Robot is down-casted as an instance of AbstractMotorBase /// and polymorphism is used to call the actual implementations. #ifndef JIMINY_ABSTRACT_MOTOR_H @@ -77,9 +77,8 @@ namespace jiminy DISABLE_COPY(AbstractMotorBase) public: - /// \param[in] robot Robot. /// \param[in] name Name of the motor. - AbstractMotorBase(const std::string & name); + explicit AbstractMotorBase(const std::string & name) noexcept; virtual ~AbstractMotorBase(); /// \brief Refresh the proxies. @@ -97,7 +96,7 @@ namespace jiminy virtual hresult_t resetAll(); /// \brief Configuration options of the motor. - GenericConfig getOptions() const; + GenericConfig getOptions() const noexcept; /// \brief Actual effort of the motor at the current time. double get() const; @@ -134,10 +133,10 @@ namespace jiminy JointModelType getJointType() const; /// \brief Index of the joint associated with the motor in configuration vector. - int32_t getJointPositionIdx() const; + Eigen::Index getJointPositionIdx() const; /// \brief Index of the joint associated with the motor in the velocity vector. - int32_t getJointVelocityIdx() const; + Eigen::Index getJointVelocityIdx() const; /// \brief Maximum effort of the motor. double getCommandLimit() const; @@ -199,34 +198,34 @@ namespace jiminy public: /// \brief Structure with the parameters of the motor. - std::unique_ptr baseMotorOptions_; + std::unique_ptr baseMotorOptions_{nullptr}; protected: /// \brief Dictionary with the parameters of the motor. - GenericConfig motorOptionsHolder_; + GenericConfig motorOptionsHolder_{}; /// \brief Flag to determine whether the controller has been initialized or not. - bool isInitialized_; + bool isInitialized_{false}; /// \brief Flag to determine whether the motor is attached to a robot. - bool isAttached_; + bool isAttached_{false}; /// \brief Robot for which the command and internal dynamics. - std::weak_ptr robot_; + std::weak_ptr robot_{}; /// \brief Notify the robot that the configuration of the sensors have changed. - std::function notifyRobot_; + std::function notifyRobot_{}; /// \brief Name of the motor. std::string name_; /// \brief Index of the motor in the measurement buffer. - std::size_t motorIdx_; - std::string jointName_; - pinocchio::JointIndex jointModelIdx_; - JointModelType jointType_; - int32_t jointPositionIdx_; - int32_t jointVelocityIdx_; - double commandLimit_; - double armature_; + std::size_t motorIdx_{0}; + std::string jointName_{}; + pinocchio::JointIndex jointModelIdx_{0}; + JointModelType jointType_{JointModelType::UNSUPPORTED}; + Eigen::Index jointPositionIdx_{0}; + Eigen::Index jointVelocityIdx_{0}; + double commandLimit_{0.0}; + double armature_{0.0}; private: /// \brief Shared data between every motors associated with the robot. - MotorSharedDataHolder_t * sharedHolder_; + MotorSharedDataHolder_t * sharedHolder_{nullptr}; }; } diff --git a/core/include/jiminy/core/hardware/abstract_sensor.h b/core/include/jiminy/core/hardware/abstract_sensor.h index b221b6167..ed1d599a6 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.h +++ b/core/include/jiminy/core/hardware/abstract_sensor.h @@ -3,7 +3,6 @@ #include "jiminy/core/fwd.h" #include "jiminy/core/utilities/helpers.h" -#include "jiminy/core/telemetry/telemetry_sender.h" #include #include @@ -18,94 +17,10 @@ namespace jiminy { class TelemetryData; + class TelemetrySender; class Robot; class AbstractSensorBase; - // Sensor data holder - namespace details - { - struct SensorDataItem - { - std::string name; - std::size_t idx; - Eigen::Ref value; - }; - } - - struct JIMINY_DLLAPI IndexByIndex - { - }; - struct JIMINY_DLLAPI IndexByName - { - }; - - struct SensorDataTypeMap : - public boost::multi_index::multi_index_container< - details::SensorDataItem, - boost::multi_index::indexed_by< - boost::multi_index::ordered_unique< - boost::multi_index::tag, - boost::multi_index:: - member, - std::less // Ordering by ascending order - >, - boost::multi_index::hashed_unique< - boost::multi_index::tag, - boost::multi_index:: - member>, - boost::multi_index::sequenced<>>> - { - public: - explicit SensorDataTypeMap(const Eigen::MatrixXd * sharedDataPtr = nullptr) : - multi_index_container(), - sharedDataPtr_(sharedDataPtr) - { - } - - /// @brief Returning data associated with all sensors at once. - /// - /// @warning It is up to the sure to make sure that the data are up-to-date. - inline const Eigen::MatrixXd & getAll() const - { - if (sharedDataPtr_) - { - assert((size() == static_cast(sharedDataPtr_->cols())) && - "Shared data inconsistent with sensors."); - return *sharedDataPtr_; - } - else - { - // Get sensors data size - Eigen::Index dataSize = 0; - if (size() > 0) - { - dataSize = this->cbegin()->value.size(); - } - - // Resize internal buffer if needed - data_.resize(Eigen::NoChange, dataSize); - - // Set internal buffer by copying sensor data sequentially - for (const auto & sensor : *this) - { - assert(sensor.value.size() == dataSize && - "Cannot get all data at once for heterogeneous sensors."); - data_.row(sensor.idx) = sensor.value; - } - - return data_; - } - } - - private: - const Eigen::MatrixXd * const sharedDataPtr_; - /* Internal buffer if no shared memory available. - It is useful if the sensors data is not contiguous in the first place, - which is likely to be the case when allocated from Python, or when - re-generating sensor data from log files. */ - mutable Eigen::MatrixXd data_{}; - }; - /// \brief Structure holding the data for every sensors of a given type. /// /// \details Every sensors of a given type must have the same 'behavior', e.g. the same delay @@ -192,8 +107,8 @@ namespace jiminy public: /// \param[in] name Name of the sensor - AbstractSensorBase(const std::string & name); - virtual ~AbstractSensorBase() = default; + explicit AbstractSensorBase(const std::string & name) noexcept; + virtual ~AbstractSensorBase(); /// \brief Reset the internal state of the sensors. /// @@ -252,7 +167,7 @@ namespace jiminy virtual hresult_t setOptionsAll(const GenericConfig & sensorOptions) = 0; /// \brief Configuration options of the sensor. - GenericConfig getOptions() const; + GenericConfig getOptions() const noexcept; template hresult_t set(const Eigen::MatrixBase & value); @@ -291,7 +206,7 @@ namespace jiminy virtual const std::string & getType() const = 0; /// \brief It is the size of the sensor's data vector. - virtual uint64_t getSize() const = 0; + virtual std::size_t getSize() const = 0; /// \brief Name of each element of the data measured by the sensor. virtual const std::vector & getFieldnames() const = 0; @@ -381,25 +296,25 @@ namespace jiminy public: /// \brief Structure with the parameters of the sensor - std::unique_ptr baseSensorOptions_; + std::unique_ptr baseSensorOptions_{nullptr}; protected: /// \brief Dictionary with the parameters of the sensor. - GenericConfig sensorOptionsHolder_; + GenericConfig sensorOptionsHolder_{}; /// \brief Flag to determine whether the sensor has been initialized. - bool isInitialized_; + bool isInitialized_{false}; /// \brief Flag to determine whether the sensor is attached to a robot. - bool isAttached_; + bool isAttached_{false}; /// \brief Flag to determine whether the telemetry of the sensor has been initialized. - bool isTelemetryConfigured_; + bool isTelemetryConfigured_{false}; /// \brief Robot for which the command and internal dynamics Name of the sensor. - std::weak_ptr robot_; + std::weak_ptr robot_{}; /// \brief Name of the sensor. std::string name_; private: /// \brief Telemetry sender of the sensor used to register and update telemetry variables. - TelemetrySender telemetrySender_; + std::unique_ptr telemetrySender_; }; template @@ -409,7 +324,7 @@ namespace jiminy DISABLE_COPY(AbstractSensorTpl) public: - AbstractSensorTpl(const std::string & name); + using AbstractSensorBase::AbstractSensorBase; virtual ~AbstractSensorTpl(); auto shared_from_this() { return shared_from(this); } @@ -422,7 +337,7 @@ namespace jiminy virtual std::size_t getIdx() const override final; virtual const std::string & getType() const override final; virtual const std::vector & getFieldnames() const final; - virtual uint64_t getSize() const override final; + virtual std::size_t getSize() const override final; virtual Eigen::Ref get() const override final; @@ -452,10 +367,10 @@ namespace jiminy static const bool areFieldnamesGrouped_; protected: - std::size_t sensorIdx_; + std::size_t sensorIdx_{0}; private: - SensorSharedDataHolder_t * sharedHolder_; + SensorSharedDataHolder_t * sharedHolder_{nullptr}; }; } diff --git a/core/include/jiminy/core/hardware/abstract_sensor.hxx b/core/include/jiminy/core/hardware/abstract_sensor.hxx index 27843cf20..6f114728c 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.hxx +++ b/core/include/jiminy/core/hardware/abstract_sensor.hxx @@ -34,14 +34,6 @@ namespace jiminy // ========================== AbstractSensorTpl =============================== - template - AbstractSensorTpl::AbstractSensorTpl(const std::string & name) : - AbstractSensorBase(name), - sensorIdx_(0), - sharedHolder_(nullptr) - { - } - template AbstractSensorTpl::~AbstractSensorTpl() { @@ -256,7 +248,7 @@ namespace jiminy } template - uint64_t AbstractSensorTpl::getSize() const + std::size_t AbstractSensorTpl::getSize() const { return fieldnames_.size(); } diff --git a/core/include/jiminy/core/hardware/basic_motors.h b/core/include/jiminy/core/hardware/basic_motors.h index 9d21eb661..88b244cf4 100644 --- a/core/include/jiminy/core/hardware/basic_motors.h +++ b/core/include/jiminy/core/hardware/basic_motors.h @@ -58,18 +58,18 @@ namespace jiminy motorOptions_t(const GenericConfig & options) : abstractMotorOptions_t(options), - enableFriction(boost::get(options.at("enableFriction"))), - frictionViscousPositive(boost::get(options.at("frictionViscousPositive"))), - frictionViscousNegative(boost::get(options.at("frictionViscousNegative"))), - frictionDryPositive(boost::get(options.at("frictionDryPositive"))), - frictionDryNegative(boost::get(options.at("frictionDryNegative"))), - frictionDrySlope(boost::get(options.at("frictionDrySlope"))) + enableFriction{boost::get(options.at("enableFriction"))}, + frictionViscousPositive{boost::get(options.at("frictionViscousPositive"))}, + frictionViscousNegative{boost::get(options.at("frictionViscousNegative"))}, + frictionDryPositive{boost::get(options.at("frictionDryPositive"))}, + frictionDryNegative{boost::get(options.at("frictionDryNegative"))}, + frictionDrySlope{boost::get(options.at("frictionDrySlope"))} { } }; public: - SimpleMotor(const std::string & name); + explicit SimpleMotor(const std::string & name) noexcept; virtual ~SimpleMotor() = default; auto shared_from_this() { return shared_from(this); } @@ -87,7 +87,7 @@ namespace jiminy double command) final override; private: - std::unique_ptr motorOptions_; + std::unique_ptr motorOptions_{nullptr}; }; } diff --git a/core/include/jiminy/core/hardware/basic_sensors.h b/core/include/jiminy/core/hardware/basic_sensors.h index d214f729d..0920893ab 100644 --- a/core/include/jiminy/core/hardware/basic_sensors.h +++ b/core/include/jiminy/core/hardware/basic_sensors.h @@ -13,7 +13,7 @@ namespace jiminy class JIMINY_DLLAPI ImuSensor : public AbstractSensorTpl { public: - ImuSensor(const std::string & name); + using AbstractSensorTpl::AbstractSensorTpl; virtual ~ImuSensor() = default; auto shared_from_this() { return shared_from(this); } @@ -36,10 +36,10 @@ namespace jiminy virtual void measureData() final override; private: - std::string frameName_; - pinocchio::FrameIndex frameIdx_; + std::string frameName_{}; + pinocchio::FrameIndex frameIdx_{0}; /// \brief Sensor inverse rotation bias. - Eigen::Matrix3d sensorRotationBiasInv_; + Eigen::Matrix3d sensorRotationBiasInv_{Eigen::Matrix3d::Identity()}; }; template<> @@ -52,7 +52,7 @@ namespace jiminy class JIMINY_DLLAPI ContactSensor : public AbstractSensorTpl { public: - ContactSensor(const std::string & name); + using AbstractSensorTpl::AbstractSensorTpl; virtual ~ContactSensor() = default; auto shared_from_this() { return shared_from(this); } @@ -73,8 +73,8 @@ namespace jiminy const ForceVector & fExternal) final override; private: - std::string frameName_; - pinocchio::FrameIndex frameIdx_; + std::string frameName_{}; + pinocchio::FrameIndex frameIdx_{0}; }; template<> @@ -87,7 +87,7 @@ namespace jiminy class JIMINY_DLLAPI ForceSensor : public AbstractSensorTpl { public: - ForceSensor(const std::string & name); + using AbstractSensorTpl::AbstractSensorTpl; virtual ~ForceSensor() = default; auto shared_from_this() { return shared_from(this); } @@ -98,7 +98,7 @@ namespace jiminy const std::string & getFrameName() const; pinocchio::FrameIndex getFrameIdx() const; - pinocchio::JointIndex getJointIdx() const; + pinocchio::JointIndex getJointModelIdx() const; private: virtual hresult_t set(double t, @@ -109,10 +109,10 @@ namespace jiminy const ForceVector & fExternal) final override; private: - std::string frameName_; - pinocchio::FrameIndex frameIdx_; - pinocchio::JointIndex parentJointIdx_; - pinocchio::Force f_; + std::string frameName_{}; + pinocchio::FrameIndex frameIdx_{0}; + pinocchio::JointIndex parentJointModelIdx_{0}; + pinocchio::Force f_{}; }; template<> @@ -125,7 +125,7 @@ namespace jiminy class JIMINY_DLLAPI EncoderSensor : public AbstractSensorTpl { public: - EncoderSensor(const std::string & name); + using AbstractSensorTpl::AbstractSensorTpl; virtual ~EncoderSensor() = default; auto shared_from_this() { return shared_from(this); } @@ -135,7 +135,7 @@ namespace jiminy virtual hresult_t refreshProxies() final override; const std::string & getJointName() const; - pinocchio::JointIndex getJointIdx() const; + pinocchio::JointIndex getJointModelIdx() const; JointModelType getJointType() const; private: @@ -147,9 +147,9 @@ namespace jiminy const ForceVector & fExternal) final override; private: - std::string jointName_; - pinocchio::JointIndex jointIdx_; - JointModelType jointType_; + std::string jointName_{}; + pinocchio::JointIndex jointModelIdx_{0}; + JointModelType jointType_{JointModelType::UNSUPPORTED}; }; template<> @@ -162,7 +162,7 @@ namespace jiminy class JIMINY_DLLAPI EffortSensor : public AbstractSensorTpl { public: - EffortSensor(const std::string & name); + using AbstractSensorTpl::AbstractSensorTpl; virtual ~EffortSensor() = default; auto shared_from_this() { return shared_from(this); } @@ -183,8 +183,8 @@ namespace jiminy const ForceVector & fExternal) final override; private: - std::string motorName_; - std::size_t motorIdx_; + std::string motorName_{}; + std::size_t motorIdx_{0}; }; template<> diff --git a/core/include/jiminy/core/hardware/fwd.h b/core/include/jiminy/core/hardware/fwd.h new file mode 100644 index 000000000..7c11588ac --- /dev/null +++ b/core/include/jiminy/core/hardware/fwd.h @@ -0,0 +1,104 @@ +#ifndef JIMINY_FORWARD_SENSOR_H +#define JIMINY_FORWARD_SENSOR_H + +#include "jiminy/core/fwd.h" + +#include // `boost::multi_index::hashed_unique` +#include // `boost::multi_index::member` +#include // `boost::multi_index::ordered_unique` +#include // `boost::multi_index::sequenced` +#include // `boost::multi_index::tag` +#include // `boost::multi_index::multi_index_container` + + +namespace jiminy +{ + // Sensor data holder + namespace details + { + struct SensorDataItem + { + std::string name; + std::size_t idx; + Eigen::Ref value; + }; + } + + struct JIMINY_DLLAPI IndexByIndex + { + }; + struct JIMINY_DLLAPI IndexByName + { + }; + + struct SensorDataTypeMap : + public boost::multi_index::multi_index_container< + details::SensorDataItem, + boost::multi_index::indexed_by< + boost::multi_index::ordered_unique< + boost::multi_index::tag, + boost::multi_index:: + member, + std::less // Ordering by ascending order + >, + boost::multi_index::hashed_unique< + boost::multi_index::tag, + boost::multi_index:: + member>, + boost::multi_index::sequenced<>>> + { + public: + explicit SensorDataTypeMap(const Eigen::MatrixXd * sharedDataPtr = nullptr) noexcept : + multi_index_container(), + sharedDataPtr_{sharedDataPtr} + { + } + + /// @brief Returning data associated with all sensors at once. + /// + /// @warning It is up to the sure to make sure that the data are up-to-date. + inline const Eigen::MatrixXd & getAll() const + { + if (sharedDataPtr_) + { + assert((size() == static_cast(sharedDataPtr_->cols())) && + "Shared data inconsistent with sensors."); + return *sharedDataPtr_; + } + else + { + // Get sensors data size + Eigen::Index dataSize = 0; + if (size() > 0) + { + dataSize = this->cbegin()->value.size(); + } + + // Resize internal buffer if needed + data_.resize(Eigen::NoChange, dataSize); + + // Set internal buffer by copying sensor data sequentially + for (const auto & sensor : *this) + { + assert(sensor.value.size() == dataSize && + "Cannot get all data at once for heterogeneous sensors."); + data_.row(sensor.idx) = sensor.value; + } + + return data_; + } + } + + private: + const Eigen::MatrixXd * const sharedDataPtr_; + /* Internal buffer if no shared memory available. + It is useful if the sensors data is not contiguous in the first place, + which is likely to be the case when allocated from Python, or when + re-generating sensor data from log files. */ + mutable Eigen::MatrixXd data_{}; + }; + + using SensorsDataMap = std::unordered_map; +} + +#endif // JIMINY_FORWARD_SENSOR_H diff --git a/core/include/jiminy/core/io/abstract_io_device.h b/core/include/jiminy/core/io/abstract_io_device.h index 71f734710..01afbab4b 100644 --- a/core/include/jiminy/core/io/abstract_io_device.h +++ b/core/include/jiminy/core/io/abstract_io_device.h @@ -34,10 +34,12 @@ namespace jiminy NEW_ONLY = 0x040, /// \brief Do not create the device if it does not exists. EXISTING_ONLY = 0x080, +#ifndef _WIN32 /// \brief Open the device in non blocking mode. NON_BLOCKING = 0x100, /// \brief Open the device in sync mode (ensure that write are finished at return). SYNC = 0x200, +#endif }; // Facility operators to avoid cast. @@ -52,7 +54,8 @@ namespace jiminy class JIMINY_DLLAPI AbstractIODevice { public: - AbstractIODevice(); + explicit AbstractIODevice(openMode_t supportedModes) noexcept; + explicit AbstractIODevice(AbstractIODevice &&) = default; virtual ~AbstractIODevice() = default; /// \brief Open the device. @@ -95,24 +98,24 @@ namespace jiminy /// /// \details For random-access devices, this function returns the size of the device. /// For sequential devices, bytesAvailable() is returned. - virtual int64_t size(); + virtual std::size_t size(); /// \brief Move the current position cursor to pos if possible. /// /// \param pos Desired new position of the cursor. /// /// \return hresult_t::SUCCESS if successful, another hresult_t value otherwise. - virtual hresult_t seek(int64_t pos); + virtual hresult_t seek(std::ptrdiff_t pos); /// \brief The current cursor position (0 if there is not concept of position cursor). - virtual int64_t pos(); + virtual std::ptrdiff_t pos(); /// \brief Resize the device to provided size. - virtual hresult_t resize(int64_t size); + virtual hresult_t resize(std::size_t size); /// \brief Returns the number of bytes that are available for reading. Commonly used with /// sequential device. - virtual int64_t bytesAvailable(); + virtual std::size_t bytesAvailable(); /// \brief Write data in the device. /// @@ -123,15 +126,48 @@ namespace jiminy /// /// \return hresult_t::SUCCESS if successful, another hresult_t value otherwise. template - hresult_t write(const T & valueIn); + std::enable_if_t && std::is_trivially_copyable_v, + hresult_t> + write(const T & value); - /// \brief Write data in the device. + template + std::enable_if_t, hresult_t> write(const T & value); + + template + std::enable_if_t && !std::is_trivially_copyable_v, + hresult_t> + write(const T & value) = delete; + + /// \brief Read data in the device. /// - /// \param data Buffer of data to write. - /// \param dataSize Number of bytes to write. + /// \details The default implementation manage only POD type. For specific type, the + /// template shall be extended with specific implementation. + /// + /// \param Value to store read data. /// /// \return hresult_t::SUCCESS if successful, another hresult_t value otherwise. - virtual hresult_t write(const void * data, int64_t dataSize); + template + std::enable_if_t> && + std::is_trivially_copyable_v>, + hresult_t> + read(T && value); + + template + std::enable_if_t>, hresult_t> read(T && value); + + template + std::enable_if_t> && + !std::is_trivially_copyable_v>, + hresult_t> + read(T && value) = delete; + + /// \brief Retrieve the latest error. Useful for calls that do not return an error code + /// directly. + hresult_t getLastError() const; + + protected: + virtual hresult_t doOpen(openMode_t mode) = 0; + virtual hresult_t doClose() = 0; /// \brief Write data in the device. /// @@ -139,18 +175,15 @@ namespace jiminy /// \param dataSize Number of bytes to write. /// /// \return the number of bytes written, -1 in case of error. - virtual int64_t writeData(const void * data, int64_t dataSize) = 0; + virtual std::ptrdiff_t writeData(const void * data, std::size_t dataSize) = 0; - /// \brief Read data in the device. - /// - /// \details The default implementation manage only POD type. For specific type, the - /// template shall be extended with specific implementation. + /// \brief Write data in the device. /// - /// \param Value to store read data. + /// \param data Buffer of data to write. + /// \param dataSize Number of bytes to write. /// /// \return hresult_t::SUCCESS if successful, another hresult_t value otherwise. - template - hresult_t read(T & valueIn); + virtual hresult_t write(const void * data, std::size_t dataSize); /// \brief Read data from the device. /// @@ -158,7 +191,7 @@ namespace jiminy /// \param dataSize Number of bytes to read. /// /// \return hresult_t::SUCCESS if successful, another hresult_t value otherwise. - virtual hresult_t read(void * data, int64_t dataSize); + virtual hresult_t read(void * data, std::size_t dataSize); /// \brief Read data in the device. /// @@ -166,38 +199,15 @@ namespace jiminy /// \param dataSize Number of bytes to read. /// /// \return the number of bytes read, -1 in case of error. - virtual int64_t readData(void * data, int64_t dataSize) = 0; - - /// \brief Retrieve the latest error. Useful for calls that do not return an error code - /// directly. - hresult_t getLastError() const; - - /// \brief Set the device blocking fashion. - /// - /// \return The latest generated error. - virtual hresult_t setBlockingMode(bool shouldBlock); - - /// \brief Set the device backend (reset the old one if any). - bool isBackendValid(); - - /// \brief Set the device backend (reset the old one if any). - virtual void setBackend(std::unique_ptr io); - - /// \brief Reset the device backend. - virtual void removeBackend(); + virtual std::ptrdiff_t readData(void * data, std::size_t dataSize) = 0; protected: - virtual hresult_t doOpen(openMode_t mode) = 0; - virtual hresult_t doClose() = 0; - - /// \brief Current opening mode. - openMode_t modes_; /// \brief Supported modes of the device. - openMode_t supportedModes_; + const openMode_t supportedModes_; + /// \brief Current opening mode. + openMode_t modes_{openMode_t::NOT_OPEN}; /// \brief Latest generated error. - hresult_t lastError_; - /// \brief Backend to use if any. - std::unique_ptr io_; + hresult_t lastError_{hresult_t::SUCCESS}; }; } diff --git a/core/include/jiminy/core/io/abstract_io_device.hxx b/core/include/jiminy/core/io/abstract_io_device.hxx index 09874387f..eb93d8d0f 100644 --- a/core/include/jiminy/core/io/abstract_io_device.hxx +++ b/core/include/jiminy/core/io/abstract_io_device.hxx @@ -7,40 +7,45 @@ namespace jiminy { - // Generic implementation - POD types + // POD types template - hresult_t AbstractIODevice::write(const T & valueIn) + std::enable_if_t && std::is_trivially_copyable_v, hresult_t> + AbstractIODevice::write(const T & value) { - return write(reinterpret_cast(&valueIn), sizeof(T)); + std::size_t toWrite = sizeof(T); + const void * const bufferPos = static_cast(&value); + return write(bufferPos, toWrite); } template - hresult_t AbstractIODevice::read(T & valueIn) + std::enable_if_t> && + std::is_trivially_copyable_v>, + hresult_t> + AbstractIODevice::read(T && value) { - int64_t toRead = sizeof(T); - uint8_t * bufferPos = reinterpret_cast(&valueIn); + std::size_t toRead = sizeof(T); + void * const bufferPos = static_cast(&value); return read(bufferPos, toRead); } - //--------------------------------- Specific implementations --------------------------------// - - // read - template<> - hresult_t AbstractIODevice::read>(std::vector & v); - template<> - hresult_t AbstractIODevice::read>(std::vector & v); - template<> - hresult_t AbstractIODevice::read(std::string & str) = delete; + // Contiguous container + template + std::enable_if_t, hresult_t> + AbstractIODevice::write(const T & value) + { + const std::size_t toWrite = value.size() * sizeof(typename T::value_type); + const void * const bufferPos = static_cast(value.data()); + return write(bufferPos, toWrite); + } - // write - template<> - hresult_t AbstractIODevice::write(const std::string_view & str); - template<> - hresult_t AbstractIODevice::write>(const std::vector & v); - template<> - hresult_t AbstractIODevice::write>(const std::vector & v); - template<> - hresult_t AbstractIODevice::write>(const std::vector & v); + template + std::enable_if_t>, hresult_t> + AbstractIODevice::read(T && value) + { + const std::size_t toRead = value.size() * sizeof(typename remove_cvref_t::value_type); + void * const bufferPos = static_cast(value.data()); + return read(bufferPos, toRead); + } } #endif // JIMINY_ABSTRACT_IO_DEVICE_HXX diff --git a/core/include/jiminy/core/io/file_device.h b/core/include/jiminy/core/io/file_device.h index 9a14813f3..d0ec1da8c 100644 --- a/core/include/jiminy/core/io/file_device.h +++ b/core/include/jiminy/core/io/file_device.h @@ -13,17 +13,14 @@ namespace jiminy class JIMINY_DLLAPI FileDevice : public AbstractIODevice { public: - FileDevice(const std::string & filename); + explicit FileDevice(const std::string & filename) noexcept; virtual ~FileDevice(); - int64_t size() override; - hresult_t resize(int64_t sizeIn) override; - hresult_t seek(int64_t pos) override; - int64_t pos() override; - int64_t bytesAvailable() override; - - int64_t readData(void * data, int64_t dataSize) override; - int64_t writeData(const void * data, int64_t dataSize) override; + std::size_t size() override; + hresult_t seek(std::ptrdiff_t pos) override; + std::ptrdiff_t pos() override; + hresult_t resize(std::size_t size) override; + std::size_t bytesAvailable() override; const std::string & name() const; @@ -31,9 +28,13 @@ namespace jiminy hresult_t doOpen(openMode_t mode) override; hresult_t doClose() override; + std::ptrdiff_t readData(void * data, std::size_t dataSize) override; + std::ptrdiff_t writeData(const void * data, std::size_t dataSize) override; + + protected: std::string filename_; /// \brief File descriptor. - int32_t fileDescriptor_; + int32_t fileDescriptor_{-1}; }; } diff --git a/core/include/jiminy/core/io/json_loader.h b/core/include/jiminy/core/io/json_loader.h index 6ff14edac..7a53a087b 100644 --- a/core/include/jiminy/core/io/json_loader.h +++ b/core/include/jiminy/core/io/json_loader.h @@ -20,8 +20,7 @@ namespace jiminy class JIMINY_DLLAPI JsonLoader { public: - explicit JsonLoader(std::shared_ptr device); - ~JsonLoader() = default; + explicit JsonLoader(const std::shared_ptr & device) noexcept; /// \brief Load json data from device and parse it to root json. hresult_t load(); @@ -33,8 +32,8 @@ namespace jiminy protected: /// \brief Hold the parsed document. - std::unique_ptr rootJson_; - std::vector payload_; + std::unique_ptr rootJson_{std::make_unique()}; + std::vector payload_{}; std::shared_ptr device_; }; } diff --git a/core/include/jiminy/core/io/json_writer.h b/core/include/jiminy/core/io/json_writer.h index 45aad552c..c99007baf 100644 --- a/core/include/jiminy/core/io/json_writer.h +++ b/core/include/jiminy/core/io/json_writer.h @@ -16,15 +16,11 @@ namespace jiminy class JIMINY_DLLAPI JsonWriter { public: - JsonWriter(std::shared_ptr device); - ~JsonWriter() = default; + explicit JsonWriter(const std::shared_ptr & device) noexcept; /// \brief Dump current content to device. hresult_t dump(const Json::Value & input); - /// \brief In case the constructor can't init the backend use set to init. - void setBackend(std::shared_ptr device); - private: std::shared_ptr device_; }; diff --git a/core/include/jiminy/core/io/memory_device.h b/core/include/jiminy/core/io/memory_device.h index be8019150..193baf790 100644 --- a/core/include/jiminy/core/io/memory_device.h +++ b/core/include/jiminy/core/io/memory_device.h @@ -11,41 +11,29 @@ namespace jiminy class JIMINY_DLLAPI MemoryDevice : public AbstractIODevice { public: - MemoryDevice(uint64_t size); - MemoryDevice(const MemoryDevice & other); + explicit MemoryDevice(std::size_t size) noexcept; MemoryDevice(MemoryDevice && other); - - MemoryDevice(std::vector && initBuffer); - + explicit MemoryDevice(std::vector && initBuffer) noexcept; virtual ~MemoryDevice(); - MemoryDevice & operator=(const MemoryDevice & other); - MemoryDevice & operator=(MemoryDevice && other); - - int64_t size() override { return static_cast(buffer_.size()); } + std::size_t size() override { return buffer_.size(); } + hresult_t seek(std::ptrdiff_t pos) override; + std::ptrdiff_t pos() override { return currentPos_; } + hresult_t resize(std::size_t size) override; + std::size_t bytesAvailable() override { return size() - currentPos_; } bool isSequential() const override { return false; } - int64_t pos() override { return currentPos_; } - - int64_t bytesAvailable() override { return size() - currentPos_; } - - hresult_t seek(int64_t pos) override; - - int64_t readData(void * data, int64_t dataSize) override; - int64_t writeData(const void * data, int64_t dataSize) override; - - hresult_t setBlockingMode(bool) override; - - hresult_t resize(int64_t size) override; - protected: hresult_t doOpen(openMode_t modes) override; hresult_t doClose() override; + std::ptrdiff_t readData(void * data, std::size_t dataSize) override; + std::ptrdiff_t writeData(const void * data, std::size_t dataSize) override; + private: std::vector buffer_; - int64_t currentPos_; + std::ptrdiff_t currentPos_{0}; }; } diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h index c640adb8e..ee9a77b8a 100644 --- a/core/include/jiminy/core/robot/model.h +++ b/core/include/jiminy/core/robot/model.h @@ -40,7 +40,7 @@ namespace jiminy struct JIMINY_DLLAPI constraintsHolder_t { public: - void clear(); + void clear() noexcept; std::pair find( const std::string & key, constraintsHolderType_t holderType); @@ -111,13 +111,13 @@ namespace jiminy public: /// \brief Store internal constraints related to joint bounds. - constraintsMap_t boundJoints; + constraintsMap_t boundJoints{}; /// \brief Store internal constraints related to contact frames. - constraintsMap_t contactFrames; + constraintsMap_t contactFrames{}; /// \brief Store internal constraints related to collision bounds. - std::vector collisionBodies; + std::vector collisionBodies{}; /// \brief Store internal constraints registered by user. - constraintsMap_t registered; + constraintsMap_t registered{}; }; class JIMINY_DLLAPI Model : public std::enable_shared_from_this @@ -245,7 +245,7 @@ namespace jiminy DISABLE_COPY(Model) public: - Model(); + explicit Model() noexcept; virtual ~Model() = default; hresult_t initialize(const pinocchio::Model & pncModel, @@ -317,7 +317,7 @@ namespace jiminy // Copy on purpose hresult_t setOptions(GenericConfig modelOptions); - GenericConfig getOptions() const; + GenericConfig getOptions() const noexcept; /// \remark This method are not intended to be called manually. The Engine is taking care /// of it. @@ -330,9 +330,9 @@ namespace jiminy const std::vector & getMeshPackageDirs() const; bool getHasFreeflyer() const; // Getters without 'get' prefix for consistency with pinocchio C++ API - int32_t nq() const; - int32_t nv() const; - int32_t nx() const; + Eigen::Index nq() const; + Eigen::Index nv() const; + Eigen::Index nx() const; const std::vector & getCollisionBodiesNames() const; const std::vector & getContactFramesNames() const; @@ -341,8 +341,8 @@ namespace jiminy const std::vector & getContactFramesIdx() const; const std::vector & getRigidJointsNames() const; const std::vector & getRigidJointsModelIdx() const; - const std::vector & getRigidJointsPositionIdx() const; - const std::vector & getRigidJointsVelocityIdx() const; + const std::vector & getRigidJointsPositionIdx() const; + const std::vector & getRigidJointsVelocityIdx() const; const std::vector & getFlexibleJointsNames() const; const std::vector & getFlexibleJointsModelIdx() const; @@ -391,84 +391,84 @@ namespace jiminy virtual hresult_t refreshProxies(); public: - pinocchio::Model pncModelOrig_; - pinocchio::Model pncModel_; - pinocchio::GeometryModel collisionModelOrig_; - pinocchio::GeometryModel collisionModel_; - pinocchio::GeometryModel visualModelOrig_; - pinocchio::GeometryModel visualModel_; - pinocchio::Data pncDataOrig_; - mutable pinocchio::Data pncData_; - mutable pinocchio::GeometryData collisionData_; - mutable pinocchio::GeometryData visualData_; - std::unique_ptr mdlOptions_; + pinocchio::Model pncModelOrig_{}; + pinocchio::Model pncModel_{}; + pinocchio::GeometryModel collisionModelOrig_{}; + pinocchio::GeometryModel collisionModel_{}; + pinocchio::GeometryModel visualModelOrig_{}; + pinocchio::GeometryModel visualModel_{}; + pinocchio::Data pncDataOrig_{}; + mutable pinocchio::Data pncData_{}; + mutable pinocchio::GeometryData collisionData_{}; + mutable pinocchio::GeometryData visualData_{}; + std::unique_ptr mdlOptions_{nullptr}; /// \brief Buffer storing the contact forces. - ForceVector contactForces_; + ForceVector contactForces_{}; protected: - bool isInitialized_; - std::string urdfPath_; - std::string urdfData_; - std::vector meshPackageDirs_; - bool hasFreeflyer_; - GenericConfig mdlOptionsHolder_; + bool isInitialized_{false}; + std::string urdfPath_{}; + std::string urdfData_{}; + std::vector meshPackageDirs_{}; + bool hasFreeflyer_{false}; + GenericConfig mdlOptionsHolder_{}; /// \brief Name of the collision bodies of the robot. - std::vector collisionBodiesNames_; + std::vector collisionBodiesNames_{}; /// \brief Name of the contact frames of the robot. - std::vector contactFramesNames_; + std::vector contactFramesNames_{}; /// \brief Indices of the collision bodies in the frame list of the robot. - std::vector collisionBodiesIdx_; + std::vector collisionBodiesIdx_{}; /// \brief Indices of the collision pairs associated with each collision body. - std::vector> collisionPairsIdx_; + std::vector> collisionPairsIdx_{}; /// \brief Indices of the contact frames in the frame list of the robot. - std::vector contactFramesIdx_; + std::vector contactFramesIdx_{}; /// \brief Name of the actual joints of the robot, not taking into account the freeflyer. - std::vector rigidJointsNames_; + std::vector rigidJointsNames_{}; /// \brief Index of the actual joints in the pinocchio robot. - std::vector rigidJointsModelIdx_; + std::vector rigidJointsModelIdx_{}; /// \brief All the indices of the actual joints in the configuration vector of the robot /// (ie including all the degrees of freedom). - std::vector rigidJointsPositionIdx_; + std::vector rigidJointsPositionIdx_{}; /// \brief All the indices of the actual joints in the velocity vector of the robot (ie /// including all the degrees of freedom). - std::vector rigidJointsVelocityIdx_; + std::vector rigidJointsVelocityIdx_{}; /// \brief Name of the flexibility joints of the robot regardless of whether the /// flexibilities are enabled. - std::vector flexibleJointsNames_; + std::vector flexibleJointsNames_{}; /// \brief Index of the flexibility joints in the pinocchio robot regardless of whether the /// flexibilities are enabled. - std::vector flexibleJointsModelIdx_; + std::vector flexibleJointsModelIdx_{}; - constraintsHolder_t constraintsHolder_; ///< Store constraints + constraintsHolder_t constraintsHolder_{}; ///< Store constraints /// \brief Upper position limit of the whole configuration vector (INF for non-physical /// joints, ie flexibility joints and freeflyer, if any). - Eigen::VectorXd positionLimitMin_; + Eigen::VectorXd positionLimitMin_{}; /// \brief Lower position limit of the whole configuration vector. - Eigen::VectorXd positionLimitMax_; + Eigen::VectorXd positionLimitMax_{}; /// \brief Maximum absolute velocity of the whole velocity vector. - Eigen::VectorXd velocityLimit_; + Eigen::VectorXd velocityLimit_{}; /// \brief Fieldnames of the elements in the configuration vector of the model. - std::vector logFieldnamesPosition_; + std::vector logFieldnamesPosition_{}; /// \brief Fieldnames of the elements in the velocity vector of the model. - std::vector logFieldnamesVelocity_; + std::vector logFieldnamesVelocity_{}; /// \brief Fieldnames of the elements in the acceleration vector of the model. - std::vector logFieldnamesAcceleration_; + std::vector logFieldnamesAcceleration_{}; /// \brief Concatenated fieldnames of the external force applied at each joint of the /// model, 'universe' excluded. - std::vector logFieldnamesForceExternal_; + std::vector logFieldnamesForceExternal_{}; private: - pinocchio::Model pncModelFlexibleOrig_; + pinocchio::Model pncModelFlexibleOrig_{}; /// \brief Vector of joints acceleration corresponding to a copy of data.a - temporary /// buffer for computing constraints. - MotionVector jointsAcceleration_; + MotionVector jointsAcceleration_{}; - int32_t nq_; - int32_t nv_; - int32_t nx_; + Eigen::Index nq_{0}; + Eigen::Index nv_{0}; + Eigen::Index nx_{0}; }; } diff --git a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h index 875793f79..005570845 100644 --- a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h +++ b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h @@ -29,7 +29,6 @@ #include "pinocchio/algorithm/cholesky.hpp" // `pinocchio::cholesky::` #include "jiminy/core/fwd.h" -#include "jiminy/core/traits.h" #include "jiminy/core/engine/engine_multi_robot.h" @@ -456,7 +455,7 @@ namespace jiminy::pinocchio_overload ConfigVectorType, TangentVectorType1> Pass1; - for (int32_t i = 1; i < model.njoints; ++i) + for (int i = 1; i < model.njoints; ++i) { Pass1::run(model.joints[i], data.joints[i], @@ -465,13 +464,13 @@ namespace jiminy::pinocchio_overload } typedef AbaBackwardStep Pass2; - for (int32_t i = model.njoints - 1; i > 0; --i) + for (int i = model.njoints - 1; i > 0; --i) { Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); } typedef pinocchio::AbaForwardStep2 Pass3; - for (int32_t i = 1; i < model.njoints; ++i) + for (int i = 1; i < model.njoints; ++i) { Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data)); } diff --git a/core/include/jiminy/core/robot/robot.h b/core/include/jiminy/core/robot/robot.h index 9f4b311fe..3ee65ed5f 100644 --- a/core/include/jiminy/core/robot/robot.h +++ b/core/include/jiminy/core/robot/robot.h @@ -2,6 +2,7 @@ #define JIMINY_ROBOT_H #include "jiminy/core/fwd.h" +#include "jiminy/core/hardware/fwd.h" #include "jiminy/core/utilities/helpers.h" #include "jiminy/core/robot/model.h" @@ -29,7 +30,7 @@ namespace jiminy DISABLE_COPY(Robot) public: - Robot(); + explicit Robot() noexcept; virtual ~Robot(); auto shared_from_this() { return shared_from(this); } @@ -81,7 +82,7 @@ namespace jiminy const std::string & sensorName) const; hresult_t setOptions(const GenericConfig & robotOptions); - GenericConfig getOptions() const; + GenericConfig getOptions() const noexcept; hresult_t setMotorOptions(const std::string & motorName, const GenericConfig & motorOptions); hresult_t setMotorsOptions(const GenericConfig & motorsOptions); @@ -118,8 +119,8 @@ namespace jiminy const std::vector & getMotorsNames() const; std::vector getMotorsModelIdx() const; - std::vector> getMotorsPositionIdx() const; - std::vector getMotorsVelocityIdx() const; + std::vector> getMotorsPositionIdx() const; + std::vector getMotorsVelocityIdx() const; const std::unordered_map> & getSensorsNames() const; const std::vector & getSensorsNames(const std::string & sensorType) const; @@ -140,26 +141,26 @@ namespace jiminy virtual hresult_t refreshProxies() override; protected: - bool isTelemetryConfigured_; - std::shared_ptr telemetryData_; - motorsHolder_t motorsHolder_; - sensorsGroupHolder_t sensorsGroupHolder_; - std::unordered_map sensorTelemetryOptions_; + bool isTelemetryConfigured_{false}; + std::shared_ptr telemetryData_{nullptr}; + motorsHolder_t motorsHolder_{}; + sensorsGroupHolder_t sensorsGroupHolder_{}; + std::unordered_map sensorTelemetryOptions_{}; /// \brief Name of the motors. - std::vector motorsNames_; + std::vector motorsNames_{}; /// \brief Name of the sensors. - std::unordered_map> sensorsNames_; + std::unordered_map> sensorsNames_{}; /// \brief Fieldnames of the command. - std::vector logFieldnamesCommand_; + std::vector logFieldnamesCommand_{}; /// \brief Fieldnames of the motors effort. - std::vector logFieldnamesMotorEffort_; + std::vector logFieldnamesMotorEffort_{}; /// \brief The number of motors. - uint64_t nmotors_; + uint64_t nmotors_{0U}; private: - std::unique_ptr mutexLocal_; + std::unique_ptr mutexLocal_{std::make_unique()}; std::shared_ptr motorsSharedHolder_; - sensorsSharedHolder_t sensorsSharedHolder_; + sensorsSharedHolder_t sensorsSharedHolder_{}; }; } diff --git a/core/include/jiminy/core/solver/constraint_solvers.h b/core/include/jiminy/core/solver/constraint_solvers.h index 533c02f2f..9faace665 100644 --- a/core/include/jiminy/core/solver/constraint_solvers.h +++ b/core/include/jiminy/core/solver/constraint_solvers.h @@ -11,11 +11,11 @@ namespace jiminy struct ConstraintBlock { - double lo; - double hi; - bool isZero; - Eigen::Index fIdx[3]; - std::uint_fast8_t fSize; + double lo{0.0}; + double hi{0.0}; + bool isZero{false}; + Eigen::Index fIdx[3]{-1, -1, -1}; + std::uint_fast8_t fSize{0}; }; struct ConstraintData @@ -24,22 +24,21 @@ namespace jiminy DISABLE_COPY(ConstraintData) public: - ConstraintData() = default; + explicit ConstraintData() = default; ConstraintData(ConstraintData && constraintData) = default; public: - AbstractConstraintBase * constraint; - Eigen::Index startIdx; - bool isInactive; - Eigen::Index dim; - ConstraintBlock blocks[3]; - std::uint_fast8_t nBlocks; + AbstractConstraintBase * constraint{nullptr}; + Eigen::Index startIdx{-1}; + bool isInactive{false}; + Eigen::Index dim{-1}; + ConstraintBlock blocks[3]{}; + std::uint_fast8_t nBlocks{0}; }; class JIMINY_DLLAPI AbstractConstraintSolver { public: - AbstractConstraintSolver() = default; virtual ~AbstractConstraintSolver() = default; /// \brief Compute the solution of the Nonlinear Complementary Problem: @@ -60,14 +59,14 @@ namespace jiminy DISABLE_COPY(PGSSolver) public: - PGSSolver(const pinocchio::Model * model, - pinocchio::Data * data, - constraintsHolder_t * constraintsHolder, - double friction, - double torsion, - double tolAbs, - double tolRel, - uint32_t maxIter); + explicit PGSSolver(const pinocchio::Model * model, + pinocchio::Data * data, + constraintsHolder_t * constraintsHolder, + double friction, + double torsion, + double tolAbs, + double tolRel, + uint32_t maxIter) noexcept; virtual ~PGSSolver() = default; virtual bool SolveBoxedForwardDynamics(double dampingInv, @@ -91,18 +90,18 @@ namespace jiminy double tolRel_; /// \brief Matrix holding the jacobian of the constraints. - Eigen::Matrix J_; + Eigen::Matrix J_{}; /// \brief Vector holding the drift of the constraints. - Eigen::VectorXd gamma_; + Eigen::VectorXd gamma_{}; /// \brief Vector holding the multipliers of the constraints. - Eigen::VectorXd lambda_; - std::vector constraintsData_; + Eigen::VectorXd lambda_{}; + std::vector constraintsData_{}; - Eigen::VectorXd b_; - Eigen::VectorXd y_; - Eigen::VectorXd yPrev_; + Eigen::VectorXd b_{}; + Eigen::VectorXd y_{}; + Eigen::VectorXd yPrev_{}; - bool isLcpFullyUpToDate_; + bool isLcpFullyUpToDate_{false}; }; } diff --git a/core/include/jiminy/core/stepper/abstract_runge_kutta_stepper.h b/core/include/jiminy/core/stepper/abstract_runge_kutta_stepper.h index 081366db2..1af8a3ece 100644 --- a/core/include/jiminy/core/stepper/abstract_runge_kutta_stepper.h +++ b/core/include/jiminy/core/stepper/abstract_runge_kutta_stepper.h @@ -15,12 +15,12 @@ namespace jiminy public: /// \param[in] f Dynamics function, with signature `a = f(t, q, v)`. /// \param[in] robots Robots whose dynamics the stepper will work on. - AbstractRungeKuttaStepper(const systemDynamics & f, - const std::vector & robots, - const Eigen::MatrixXd & RungeKuttaMatrix, - const Eigen::VectorXd & bWeights, - const Eigen::VectorXd & cNodes, - bool isFSAL); + explicit AbstractRungeKuttaStepper(const systemDynamics & f, + const std::vector & robots, + const Eigen::MatrixXd & RungeKuttaMatrix, + const Eigen::VectorXd & bWeights, + const Eigen::VectorXd & cNodes, + bool isFSAL) noexcept; virtual ~AbstractRungeKuttaStepper() = default; protected: diff --git a/core/include/jiminy/core/stepper/abstract_stepper.h b/core/include/jiminy/core/stepper/abstract_stepper.h index 82dd1063e..4b8882742 100644 --- a/core/include/jiminy/core/stepper/abstract_stepper.h +++ b/core/include/jiminy/core/stepper/abstract_stepper.h @@ -27,7 +27,8 @@ namespace jiminy public: /// \param[in] f Dynamics function, with signature `a = f(t, q, v)`. /// \param[in] robots Robots whose dynamics the stepper will work on. - AbstractStepper(const systemDynamics & f, const std::vector & robots); + explicit AbstractStepper(const systemDynamics & f, + const std::vector & robots) noexcept; virtual ~AbstractStepper() = default; /// \brief Attempt to integrate the system from time t to time t + dt. diff --git a/core/include/jiminy/core/stepper/euler_explicit_stepper.h b/core/include/jiminy/core/stepper/euler_explicit_stepper.h index 82c9adb96..4a7b7e2a6 100644 --- a/core/include/jiminy/core/stepper/euler_explicit_stepper.h +++ b/core/include/jiminy/core/stepper/euler_explicit_stepper.h @@ -11,9 +11,7 @@ namespace jiminy class JIMINY_DLLAPI EulerExplicitStepper : public AbstractStepper { public: - /// \param[in] f Dynamics function, with signature `a = f(t, q, v)`. - /// \param[in] robots Robots whose dynamics the stepper will work on. - EulerExplicitStepper(const systemDynamics & f, const std::vector & robots); + using AbstractStepper::AbstractStepper; protected: /// \brief Internal tryStep method wrapping the arguments as state_t and stateDerivative_t. diff --git a/core/include/jiminy/core/stepper/lie_group.h b/core/include/jiminy/core/stepper/lie_group.h index c98b60b3a..f0009028a 100644 --- a/core/include/jiminy/core/stepper/lie_group.h +++ b/core/include/jiminy/core/stepper/lie_group.h @@ -11,7 +11,6 @@ #include "pinocchio/algorithm/joint-configuration.hpp" // `pinocchio::integrate`, `pinocchio::difference` #include "jiminy/core/fwd.h" -#include "jiminy/core/traits.h" #include "jiminy/core/robot/robot.h" @@ -1034,11 +1033,26 @@ namespace Eigen { \ } \ \ - const Robot * const & robot() const { return robot_; } \ - Eigen::Ref & VAR1() { return EIGEN_CAT(VAR1, Ref_); } \ - const Eigen::Ref & VAR1() const { return EIGEN_CAT(VAR1, Ref_); } \ - Eigen::Ref & VAR2() { return EIGEN_CAT(VAR2, Ref_); } \ - const Eigen::Ref & VAR2() const { return EIGEN_CAT(VAR2, Ref_); } \ + const Robot * const & robot() const \ + { \ + return robot_; \ + } \ + Eigen::Ref & VAR1() \ + { \ + return EIGEN_CAT(VAR1, Ref_); \ + } \ + const Eigen::Ref & VAR1() const \ + { \ + return EIGEN_CAT(VAR1, Ref_); \ + } \ + Eigen::Ref & VAR2() \ + { \ + return EIGEN_CAT(VAR2, Ref_); \ + } \ + const Eigen::Ref & VAR2() const \ + { \ + return EIGEN_CAT(VAR2, Ref_); \ + } \ \ protected: \ const Robot * robot_; \ diff --git a/core/include/jiminy/core/stepper/runge_kutta4_stepper.h b/core/include/jiminy/core/stepper/runge_kutta4_stepper.h index 65d9adab6..353214397 100644 --- a/core/include/jiminy/core/stepper/runge_kutta4_stepper.h +++ b/core/include/jiminy/core/stepper/runge_kutta4_stepper.h @@ -30,7 +30,8 @@ namespace jiminy public: /// \param[in] f Dynamics function, with signature `a = f(t, q, v)`. /// \param[in] robots Robots whose dynamics the stepper will work on. - RungeKutta4Stepper(const systemDynamics & f, const std::vector & robots); + RungeKutta4Stepper(const systemDynamics & f, + const std::vector & robots) noexcept; }; } 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 4261b7eb6..edf98b077 100644 --- a/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h +++ b/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h @@ -49,10 +49,10 @@ namespace jiminy /// \param[in] robots Robots whose dynamics the stepper will work on. /// \param[in] tolRel Relative tolerance used to assess step success and timestep update. /// \param[in] tolAbs Absolute tolerance used to assess step success and timestep update. - RungeKuttaDOPRIStepper(const systemDynamics & f, - const std::vector & robots, - double tolRel, - double tolAbs); + explicit RungeKuttaDOPRIStepper(const systemDynamics & f, + const std::vector & robots, + double tolRel, + double tolAbs) noexcept; protected: /// \brief Determine if step has succeeded or failed, and adjust dt. diff --git a/core/include/jiminy/core/telemetry/fwd.h b/core/include/jiminy/core/telemetry/fwd.h new file mode 100644 index 000000000..8c591a1b8 --- /dev/null +++ b/core/include/jiminy/core/telemetry/fwd.h @@ -0,0 +1,36 @@ +#ifndef JIMINY_FORWARD_TELEMETRY_H +#define JIMINY_FORWARD_TELEMETRY_H + +#include "jiminy/core/fwd.h" + + +namespace jiminy +{ + /// \brief Version of the telemetry format. + inline constexpr int32_t TELEMETRY_VERSION{1}; + /// \brief Number of integers in the data section. + inline constexpr std::string_view NUM_INTS{"NumIntEntries"}; + /// \brief Number of floats in the data section. + inline constexpr std::string_view NUM_FLOATS{"NumFloatEntries"}; + /// \brief Marker of the beginning the constants section. + inline constexpr std::string_view START_CONSTANTS{"StartConstants"}; + /// \brief Marker of the beginning the columns section. + inline constexpr std::string_view START_COLUMNS{"StartColumns"}; + /// \brief Marker of the beginning of a line of data. + inline constexpr std::string_view START_LINE_TOKEN{"StartLine"}; + /// \brief Marker of the beginning of the data section. + inline constexpr std::string_view START_DATA{"StartData"}; + + struct JIMINY_DLLAPI LogData + { + int32_t version; + static_map_t constants; + double timeUnit; + VectorX times; + std::vector variableNames; + MatrixX integerValues; + MatrixX floatValues; + }; +} + +#endif // JIMINY_FORWARD_TELEMETRY_H diff --git a/core/include/jiminy/core/telemetry/telemetry_data.h b/core/include/jiminy/core/telemetry/telemetry_data.h index ff99af969..5fb4ef903 100644 --- a/core/include/jiminy/core/telemetry/telemetry_data.h +++ b/core/include/jiminy/core/telemetry/telemetry_data.h @@ -8,21 +8,6 @@ namespace jiminy { - /// \brief Version of the telemetry format. - inline constexpr int32_t TELEMETRY_VERSION{1}; - /// \brief Number of integers in the data section. - inline constexpr std::string_view NUM_INTS{"NumIntEntries"}; - /// \brief Number of floats in the data section. - inline constexpr std::string_view NUM_FLOATS{"NumFloatEntries"}; - /// \brief Marker of the beginning the constants section. - inline constexpr std::string_view START_CONSTANTS{"StartConstants"}; - /// \brief Marker of the beginning the columns section. - inline constexpr std::string_view START_COLUMNS{"StartColumns"}; - /// \brief Marker of the beginning of a line of data. - inline constexpr std::string_view START_LINE_TOKEN{"StartLine"}; - /// \brief Marker of the beginning of the data section. - inline constexpr std::string_view START_DATA{"StartData"}; - /// \brief This class manages the data structures of the telemetry. class JIMINY_DLLAPI TelemetryData { @@ -30,11 +15,10 @@ namespace jiminy DISABLE_COPY(TelemetryData) public: - TelemetryData(); - ~TelemetryData() = default; + explicit TelemetryData() = default; /// \brief Reset the telemetry before starting to use the telemetry. - void reset(); + void reset() noexcept; /// \brief Register a new variable in for telemetry. /// @@ -69,13 +53,13 @@ namespace jiminy // Must use dequeue to preserve pointer addresses after resize /// \brief Memory to handle constants. - std::deque> constantsRegistry_; + std::deque> constantsRegistry_{}; /// \brief Memory to handle integers. - std::deque> integersRegistry_; + std::deque> integersRegistry_{}; /// \brief Memory to handle floats. - std::deque> floatsRegistry_; + std::deque> floatsRegistry_{}; /// \brief Whether registering is available. - bool isRegisteringAvailable_; + bool isRegisteringAvailable_{true}; }; } diff --git a/core/include/jiminy/core/telemetry/telemetry_recorder.h b/core/include/jiminy/core/telemetry/telemetry_recorder.h index 938777bc5..7eb345f89 100644 --- a/core/include/jiminy/core/telemetry/telemetry_recorder.h +++ b/core/include/jiminy/core/telemetry/telemetry_recorder.h @@ -9,17 +9,7 @@ namespace jiminy { - struct JIMINY_DLLAPI LogData - { - int32_t version; - static_map_t constants; - double timeUnit; - VectorX times; - std::vector variableNames; - MatrixX integerValues; - MatrixX floatValues; - }; - + struct LogData; class TelemetryData; /// \class This class is responsible of writing recorded data to devices. @@ -29,7 +19,7 @@ namespace jiminy DISABLE_COPY(TelemetryRecorder) public: - TelemetryRecorder() = default; + explicit TelemetryRecorder() = default; ~TelemetryRecorder(); /// \brief Initialize the recorder. @@ -64,29 +54,29 @@ namespace jiminy hresult_t createNewChunk(); private: - std::deque flows_; + std::deque flows_{}; - bool isInitialized_; + bool isInitialized_{false}; - int64_t recordedBytesLimits_; - int64_t recordedBytesDataLine_; + std::size_t recordedBytesLimits_{0}; + std::size_t recordedBytesDataLine_{0}; /// \brief Bytes recorded in the file. - int64_t recordedBytes_; + std::size_t recordedBytes_{0}; /// \brief Size in byte of the header. - int64_t headerSize_; + std::size_t headerSize_{0}; /// \brief Pointer to the integer registry. - const std::deque> * integersRegistry_; + const std::deque> * integersRegistry_{nullptr}; /// \brief Size in bytes of the integer data section. - int64_t integerSectionSize_; + int64_t integerSectionSize_{-1}; /// \brief Pointer to the float registry. - const std::deque> * floatsRegistry_; + const std::deque> * floatsRegistry_{nullptr}; /// \brief Size in bytes of the float data section. - int64_t floatSectionSize_; + int64_t floatSectionSize_{-1}; /// \brief Precision to use when logging the time. - double timeUnitInv_; + double timeUnitInv_{NAN}; }; } -#endif // JIMINY_TELEMETRY_RECORDER_H \ No newline at end of file +#endif // JIMINY_TELEMETRY_RECORDER_H diff --git a/core/include/jiminy/core/telemetry/telemetry_sender.h b/core/include/jiminy/core/telemetry/telemetry_sender.h index 6a7c25adf..855c31d1a 100644 --- a/core/include/jiminy/core/telemetry/telemetry_sender.h +++ b/core/include/jiminy/core/telemetry/telemetry_sender.h @@ -13,8 +13,6 @@ namespace jiminy { class TelemetryData; - inline constexpr std::string_view DEFAULT_TELEMETRY_NAMESPACE{"Uninitialized Object"}; - /// \brief Class to inherit for sending telemetry data. class JIMINY_DLLAPI TelemetrySender { @@ -28,9 +26,8 @@ namespace jiminy template using telemetry_data_registry_t = std::vector...>>; - explicit TelemetrySender(); + explicit TelemetrySender() = default; explicit TelemetrySender(TelemetrySender &&) = default; - ~TelemetrySender() = default; /// \brief Configure the object. /// @@ -52,7 +49,7 @@ namespace jiminy /// \param[in] value Pointer to the newly recorded field. template std::enable_if_t, hresult_t> - registerVariable(const std::string & name, const Scalar * value); + registerVariable(const std::string & name, const Scalar * valuePtr); template hresult_t registerVariable(const std::vector & fieldnames, @@ -75,13 +72,13 @@ namespace jiminy protected: /// \brief Name of the logged object. - std::string objectName_; + std::string objectName_{"Uninitialized Object"}; private: - std::shared_ptr telemetryData_; + std::shared_ptr telemetryData_{nullptr}; /// \brief Associate each variable pointer provided by the user to their reserved position /// in the contiguous storage of telemetry data. - telemetry_data_registry_t bufferPosition_; + telemetry_data_registry_t bufferPosition_{}; }; } diff --git a/core/include/jiminy/core/telemetry/telemetry_sender.hxx b/core/include/jiminy/core/telemetry/telemetry_sender.hxx index 2e4910a58..acf5cf25c 100644 --- a/core/include/jiminy/core/telemetry/telemetry_sender.hxx +++ b/core/include/jiminy/core/telemetry/telemetry_sender.hxx @@ -9,17 +9,18 @@ namespace jiminy { template std::enable_if_t, hresult_t> - TelemetrySender::registerVariable(const std::string & name, const Scalar * value) + TelemetrySender::registerVariable(const std::string & name, const Scalar * valuePtr) { Scalar * positionInBuffer = nullptr; const std::string fullName = addCircumfix(name, objectName_, {}, TELEMETRY_FIELDNAME_DELIMITER); - hresult_t returnCode = telemetryData_->registerVariable(fullName, positionInBuffer); + if (returnCode == hresult_t::SUCCESS) { - bufferPosition_.emplace_back(telemetry_data_pair_t{value, positionInBuffer}); - *positionInBuffer = *value; + *positionInBuffer = *valuePtr; + bufferPosition_.emplace_back( + telemetry_data_pair_t{valuePtr, positionInBuffer}); } return returnCode; diff --git a/core/include/jiminy/core/traits.h b/core/include/jiminy/core/traits.h index 9660b3700..020b32311 100644 --- a/core/include/jiminy/core/traits.h +++ b/core/include/jiminy/core/traits.h @@ -26,6 +26,49 @@ namespace pinocchio namespace jiminy { + // ******************************** is_contiguous_container ******************************** // + + namespace details + { + template + struct voider + { + using type = void; + }; + + template + using void_t = typename voider::type; + + template class, class, class...> + struct canApplyImply : std::false_type + { + }; + + template class Z, class... Ts> + struct canApplyImply>, Ts...> : std::true_type + { + }; + + template class Z, class... Ts> + using canApply = canApplyImply; + + template + using sizeFunType = decltype(std::declval().size()); + + template + using hasSize = canApply; + + template + using rawIndexFunType = decltype(std::declval().data()[std::declval()]); + + template + using isContiguousIndexable = canApply; + } + + template + inline constexpr bool is_contiguous_container_v = + std::conjunction, details::isContiguousIndexable>::value; + // ************************************* remove_cvref ************************************** // // TODO: Remove it when moving to C++20 as it has been added to the standard library @@ -72,6 +115,21 @@ namespace jiminy template inline constexpr bool is_vector_v = is_vector>::value; + // **************************************** is_array *************************************** // + + template + struct is_array : std::false_type + { + }; + + template + struct is_array> : std::true_type + { + }; + + template + inline constexpr bool is_array_v = is_array>::value; + // **************************************** is_map ***************************************** // namespace details::is_map @@ -209,11 +267,11 @@ namespace jiminy }; template - inline constexpr bool is_eigen_ref_v = is_eigen_ref::value; + inline constexpr bool is_eigen_ref_v = is_eigen_ref>::value; // *************************************** is_eigen **************************************** // - namespace details::is_eigen + namespace details::is_eigen_plain { template std::true_type test(const Eigen::Matrix *); @@ -225,21 +283,24 @@ namespace jiminy struct Test : public decltype(test(std::declval>())) { }; - } // namespace details::is_eigen + } template - struct is_eigen : public std::false_type + struct is_eigen_plain : public std::false_type { }; template - struct is_eigen::value>> : + struct is_eigen_plain::value>> : std::true_type { }; template - inline constexpr bool is_eigen_v = is_eigen::value; + inline constexpr bool is_eigen_plain_v = is_eigen_plain>::value; + + template + inline constexpr bool is_eigen_v = std::disjunction, is_eigen_ref>::value; // ********************************** is_pinocchio_joint_ ********************************** // diff --git a/core/include/jiminy/core/utilities/helpers.h b/core/include/jiminy/core/utilities/helpers.h index 4610b4359..0a0de2f5f 100644 --- a/core/include/jiminy/core/utilities/helpers.h +++ b/core/include/jiminy/core/utilities/helpers.h @@ -24,15 +24,15 @@ namespace jiminy DISABLE_COPY(MutexLocal) public: - MutexLocal(); + explicit MutexLocal() = default; MutexLocal(MutexLocal && other) = default; ~MutexLocal(); - bool isLocked() const; + bool isLocked() const noexcept; private: - std::shared_ptr isLocked_; + std::shared_ptr isLocked_{std::make_shared(false)}; }; class JIMINY_DLLAPI LockGuardLocal @@ -41,7 +41,7 @@ namespace jiminy DISABLE_COPY(LockGuardLocal) public: - LockGuardLocal(MutexLocal & mutexLocal); + explicit LockGuardLocal(MutexLocal & mutexLocal) noexcept; LockGuardLocal(LockGuardLocal && other) = default; ~LockGuardLocal(); @@ -57,14 +57,15 @@ namespace jiminy using Time = std::chrono::high_resolution_clock; public: - Timer(); - void tic(); - void toc(); + explicit Timer() noexcept; + + void tic() noexcept; + void toc() noexcept; public: - std::chrono::time_point