diff --git a/.github/workflows/win.yml b/.github/workflows/win.yml index af733ad23..9bb480b23 100644 --- a/.github/workflows/win.yml +++ b/.github/workflows/win.yml @@ -68,7 +68,7 @@ jobs: -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE ` -DBoost_USE_STATIC_LIBS=OFF -DPYTHON_REQUIRED_VERSION="${{ matrix.python-version }}" ` -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON ` - -DCMAKE_CXX_FLAGS="/EHsc /bigobj -D_USE_MATH_DEFINES -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC -DURDFDOM_STATIC -DHPP_FCL_STATIC -DPINOCCHIO_STATIC" + -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus -D_USE_MATH_DEFINES -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC -DURDFDOM_STATIC -DHPP_FCL_STATIC -DPINOCCHIO_STATIC" cmake --build . --config "${Env:BUILD_TYPE}" --parallel 2 if (-not (Test-Path -PathType Container "$RootDir/build/pypi/jiminy_py/src/jiminy_py/core")) { diff --git a/build_tools/build_install_deps_linux.sh b/build_tools/build_install_deps_linux.sh index 1987fa64a..e96c58052 100755 --- a/build_tools/build_install_deps_linux.sh +++ b/build_tools/build_install_deps_linux.sh @@ -68,11 +68,13 @@ git clone -b "v5.0.1" https://github.com/assimp/assimp.git "$RootDir/assimp" cd "$RootDir/assimp" git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_linux/assimp.patch" -### Checkout hpp-fcl +### Checkout hpp-fcl, and update qhull to the latest release v8.0.2 git clone -b "v1.5.3" https://github.com/humanoid-path-planner/hpp-fcl.git "$RootDir/hpp-fcl" cd "$RootDir/hpp-fcl" git submodule --quiet update --init --recursive --jobs 8 git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_linux/hppfcl.patch" +cd "$RootDir/hpp-fcl/third-parties/qhull" +git checkout v8.0.2 ### Checkout pinocchio and its submodules git clone -b "v2.5.0" https://github.com/stack-of-tasks/pinocchio.git "$RootDir/pinocchio" @@ -151,7 +153,7 @@ cmake "$RootDir/console_bridge" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX=" -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 -############################## Build and install urdfdom_headers ###################################### +############################### Build and install urdfdom_headers ###################################### mkdir -p "$RootDir/urdfdom_headers/build" cd "$RootDir/urdfdom_headers/build" @@ -159,7 +161,7 @@ cmake "$RootDir/urdfdom_headers" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX= -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 -################################# Build and install urdfdom ########################################### +################################## Build and install urdfdom ########################################### mkdir -p "$RootDir/urdfdom/build" cd "$RootDir/urdfdom/build" @@ -168,29 +170,36 @@ cmake "$RootDir/urdfdom" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$Instal -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 -############################## Build and install assimp ###################################### +###################################### Build and install assimp ######################################## mkdir -p "$RootDir/assimp/build" cd "$RootDir/assimp/build" cmake "$RootDir/assimp" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \ -DASSIMP_BUILD_ASSIMP_TOOLS=OFF -DASSIMP_BUILD_ZLIB=ON -DASSIMP_BUILD_TESTS=OFF \ -DASSIMP_BUILD_SAMPLES=OFF -DBUILD_DOCS=OFF \ - -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-strict-overflow" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" + -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-strict-overflow -Wno-class-memaccess" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 -################################# Build and install hpp-fcl ########################################### +############################# Build and install qhull and hpp-fcl ###################################### + +mkdir -p "$RootDir/hpp-fcl/third-parties/qhull/build" +cd "$RootDir/hpp-fcl/third-parties/qhull/build" +cmake "$RootDir/hpp-fcl/third-parties/qhull" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \ + -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON -DCMAKE_CXX_FLAGS="-fPIC" -DCMAKE_C_FLAGS="-fPIC" \ + -DCMAKE_BUILD_TYPE="$BUILD_TYPE" +make install -j2 mkdir -p "$RootDir/hpp-fcl/build" cd "$RootDir/hpp-fcl/build" cmake "$RootDir/hpp-fcl" -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \ - -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" \ + -DCMAKE_PREFIX_PATH="$InstallDir" -DQhull_PREFIX="$InstallDir" -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" \ -DPYTHON_STANDARD_LAYOUT=ON -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \ -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" -DBoost_USE_STATIC_LIBS=OFF \ - -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=OFF -DINSTALL_DOCUMENTATION=OFF \ + -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=ON -DINSTALL_DOCUMENTATION=OFF \ -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="-fPIC -Wno-unused-parameter" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" make install -j2 -################################ Build and install Pinocchio ########################################## +################################# Build and install Pinocchio ########################################## ### Build and install pinocchio, finally ! mkdir -p "$RootDir/pinocchio/build" diff --git a/build_tools/build_install_deps_windows.ps1 b/build_tools/build_install_deps_windows.ps1 index d7333a00f..3f3fe4923 100644 --- a/build_tools/build_install_deps_windows.ps1 +++ b/build_tools/build_install_deps_windows.ps1 @@ -70,9 +70,11 @@ git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/ass ### Checkout hpp-fcl git clone -b "v1.5.3" https://github.com/humanoid-path-planner/hpp-fcl.git "$RootDir/hpp-fcl" -cd "$RootDir/hpp-fcl" +Set-Location -Path "$RootDir/hpp-fcl" git submodule --quiet update --init --recursive --jobs 8 git apply --reject --whitespace=fix "$RootDir/build_tools/patch_deps_windows/hppfcl.patch" +Set-Location -Path "$RootDir/hpp-fcl/third-parties/qhull" +git checkout v8.0.2 ### Checkout pinocchio and its submodules, then apply some patches (generated using `git diff --submodule=diff`) git clone -b "v2.5.0" https://github.com/stack-of-tasks/pinocchio.git "$RootDir/pinocchio" @@ -116,7 +118,7 @@ Set-Location -Path "$RootDir/eigen3/build" cmake "$RootDir/eigen3" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 ` -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" ` -DBUILD_TESTING=OFF -DEIGEN_BUILD_PKGCONFIG=OFF ` - -DCMAKE_CXX_FLAGS="/bigobj" + -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 ################################### Build and install eigenpy ########################################## @@ -132,7 +134,7 @@ cmake "$RootDir/eigenpy" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_ -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" ` -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE -DBoost_USE_STATIC_LIBS=OFF ` -DBUILD_TESTING=OFF -DINSTALL_DOCUMENTATION=OFF ` - -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj $( + -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus $( ) -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 @@ -148,7 +150,7 @@ if (-not (Test-Path -PathType Container "$RootDir/tinyxml/build")) { Set-Location -Path "$RootDir/tinyxml/build" cmake "$RootDir/tinyxml" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 ` -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" ` - -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj -DTIXML_USE_STL" + -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus -DTIXML_USE_STL" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 ############################## Build and install console_bridge ######################################## @@ -160,10 +162,10 @@ if (-not (Test-Path -PathType Container "$RootDir/console_bridge/build")) { Set-Location -Path "$RootDir/console_bridge/build" cmake "$RootDir/console_bridge" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 ` -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" ` - -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj" + -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 -############################## Build and install urdfdom_headers ###################################### +############################### Build and install urdfdom_headers ###################################### ### if (-not (Test-Path -PathType Container "$RootDir/urdfdom_headers/build")) { @@ -172,10 +174,10 @@ if (-not (Test-Path -PathType Container "$RootDir/urdfdom_headers/build")) { Set-Location -Path "$RootDir/urdfdom_headers/build" cmake -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 ` -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" ` - -DCMAKE_CXX_FLAGS="/EHsc /bigobj" "$RootDir/urdfdom_headers" + -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus" "$RootDir/urdfdom_headers" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 -################################# Build and install urdfdom ########################################### +################################## Build and install urdfdom ########################################### ### if (-not (Test-Path -PathType Container "$RootDir/urdfdom/build")) { @@ -185,10 +187,10 @@ Set-Location -Path "$RootDir/urdfdom/build" cmake "$RootDir/urdfdom" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 ` -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" ` -DBUILD_TESTING=OFF ` - -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj -D_USE_MATH_DEFINES -DURDFDOM_STATIC" + -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus -D_USE_MATH_DEFINES -DURDFDOM_STATIC" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 -############################## Build and install assimp ###################################### +###################################### Build and install assimp ######################################## ### if (-not (Test-Path -PathType Container "$RootDir/assimp/build")) { @@ -200,10 +202,20 @@ cmake -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 ` -DCMAKE_CXX_FLAGS="/EHsc /bigobj" "$RootDir/assimp" ` -DASSIMP_BUILD_ASSIMP_TOOLS=OFF -DASSIMP_BUILD_ZLIB=ON -DASSIMP_BUILD_TESTS=OFF ` -DASSIMP_BUILD_SAMPLES=OFF -DBUILD_DOCS=OFF -DASSIMP_INSTALL_PDB=OFF ` - -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj -D_USE_MATH_DEFINES" + -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus -D_USE_MATH_DEFINES" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 -################################# Build and install hpp-fcl ########################################### +############################# Build and install qhull and hpp-fcl ###################################### + +### Build qhull +if (-not (Test-Path -PathType Container "$RootDir/hpp-fcl/third-parties/qhull/build")) { + New-Item -ItemType "directory" -Force -Path "$RootDir/hpp-fcl/third-parties/qhull/build" +} +Set-Location -Path "$RootDir/hpp-fcl/third-parties/qhull/build" +cmake "$RootDir/hpp-fcl/third-parties/qhull" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_PLATFORM=x64 ` + -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" ` + -DBUILD_SHARED_LIBS=OFF -DBUILD_STATIC_LIBS=ON -DCMAKE_CXX_FLAGS="/EHsc /bigobj /Zc:__cplusplus" -DCMAKE_C_FLAGS="/EHsc /bigobj" +cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 ### Build hpp-fcl if (-not (Test-Path -PathType Container "$RootDir/hpp-fcl/build")) { @@ -215,7 +227,7 @@ cmake "$RootDir/hpp-fcl" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATOR_ -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" ` -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" ` -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE -DBoost_USE_STATIC_LIBS=OFF ` - -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=OFF -DINSTALL_DOCUMENTATION=OFF ` + -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=ON -DINSTALL_DOCUMENTATION=OFF ` -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /wd4068 /wd4267 /permissive- /Zc:__cplusplus $( ) -D_USE_MATH_DEFINES -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC -DHPP_FCL_STATIC" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 @@ -244,7 +256,7 @@ cmake "$RootDir/pinocchio" -G "Visual Studio 16 2019" -T "v142" -DCMAKE_GENERATO -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE -DBoost_USE_STATIC_LIBS=OFF ` -DBUILD_WITH_COLLISION_SUPPORT=ON -DBUILD_TESTING=OFF -DINSTALL_DOCUMENTATION=OFF ` -DBUILD_WITH_URDF_SUPPORT=ON -DBUILD_PYTHON_INTERFACE=ON ` - -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /wd4068 /wd4715 /wd4834 /permissive- $( + -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS="/EHsc /bigobj /wd4068 /wd4715 /wd4834 /permissive- /Zc:__cplusplus $( ) -D_USE_MATH_DEFINES -DNOMINMAX -DBOOST_ALL_NO_LIB -DBOOST_LIB_DIAGNOSTIC -DEIGENPY_STATIC -DURDFDOM_STATIC -DHPP_FCL_STATIC -DPINOCCHIO_STATIC" cmake --build . --target install --config "${Env:BUILD_TYPE}" --parallel 2 diff --git a/build_tools/easy_install_deps_ubuntu.sh b/build_tools/easy_install_deps_ubuntu.sh index 5031a9224..7ffebec04 100755 --- a/build_tools/easy_install_deps_ubuntu.sh +++ b/build_tools/easy_install_deps_ubuntu.sh @@ -60,17 +60,12 @@ if ! [-d "/opt/openrobots/lib/${PYTHON_BIN}/site-packages/" ] ; then curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | apt-key add - && \ apt update - if [ $DISTRIB_RELEASE == "18.04" ] ; then - apt install -y --allow-downgrades robotpkg-urdfdom=0.3.0r2 robotpkg-urdfdom-headers=0.3.0 robotpkg-hpp-fcl=1.4.2 \ - robotpkg-gepetto-viewer=4.4.0 robotpkg-py36-qt4-gepetto-viewer-corba=5.1.2 robotpkg-py36-omniorbpy \ - robotpkg-py36-eigenpy=2.3.0 robotpkg-py36-hpp-fcl=1.4.2 \ - robotpkg-pinocchio=2.3.1 robotpkg-py36-pinocchio=2.3.1 - else - apt install -y --allow-downgrades robotpkg-urdfdom=1.0.3 robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=1.4.5 \ - robotpkg-py38-qt5-gepetto-viewer=4.9.0r3 robotpkg-py38-qt5-gepetto-viewer-corba=5.4.0r2 robotpkg-py38-omniorbpy=4.2.4 \ - robotpkg-py38-eigenpy=2.5.0 robotpkg-py38-hpp-fcl=1.4.5 \ - robotpkg-pinocchio=2.4.7 robotpkg-py38-pinocchio=2.4.7 - fi + # apt-get must be used instead of apt to support wildcard in package name on Ubuntu 20 + apt-get install -y --allow-downgrades --allow-unauthenticated \ + robotpkg-urdfdom=1.0.3 robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=1.4.5 \ + robotpkg-py3*-qt5-gepetto-viewer=4.9.0r3 robotpkg-py3*-qt5-gepetto-viewer-corba=5.4.0r2 robotpkg-py3*-omniorbpy=4.2.4 \ + robotpkg-py3*-eigenpy=2.5.0 robotpkg-py3*-hpp-fcl=1.4.5 \ + robotpkg-pinocchio=2.4.7 robotpkg-py3*-pinocchio=2.4.7 sudo -H -u $(id -nu $SUDO_UID) bash -c " \ echo 'export LD_LIBRARY_PATH=\"/opt/openrobots/lib:\${LD_LIBRARY_PATH}\"' >> \$HOME/.bashrc && \ diff --git a/build_tools/patch_deps_linux/hppfcl.patch b/build_tools/patch_deps_linux/hppfcl.patch index 43b039106..d62d356bd 100644 --- a/build_tools/patch_deps_linux/hppfcl.patch +++ b/build_tools/patch_deps_linux/hppfcl.patch @@ -33,6 +33,15 @@ index d675c2c..c503cf1 100644 option(HPP_FCL_HAS_QHULL "use qhull library to compute convex hulls." FALSE) if(HPP_FCL_HAS_QHULL) +@@ -115,7 +108,7 @@ if(HPP_FCL_HAS_QHULL) + PATHS ${Qhull_PREFIX} + ) + find_library(Qhull_r_LIBRARY +- NAMES libqhull_r.so ++ NAMES libqhullstatic_r.a + PATHS ${Qhull_PREFIX} + ) + endif() @@ -172,12 +165,10 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/internal/traversal.h ) diff --git a/build_tools/patch_deps_windows/hppfcl.patch b/build_tools/patch_deps_windows/hppfcl.patch index 10e01368c..dadc7d2b3 100644 --- a/build_tools/patch_deps_windows/hppfcl.patch +++ b/build_tools/patch_deps_windows/hppfcl.patch @@ -33,6 +33,15 @@ index d675c2c..c503cf1 100644 option(HPP_FCL_HAS_QHULL "use qhull library to compute convex hulls." FALSE) if(HPP_FCL_HAS_QHULL) +@@ -115,7 +108,7 @@ if(HPP_FCL_HAS_QHULL) + PATHS ${Qhull_PREFIX} + ) + find_library(Qhull_r_LIBRARY +- NAMES libqhull_r.so ++ NAMES qhullstatic_r.lib + PATHS ${Qhull_PREFIX} + ) + endif() @@ -172,12 +165,10 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/internal/traversal.h ) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index c678a6721..4a3e951ec 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -24,7 +24,7 @@ if(NOT LEGACY_MODE) find_package(Eigen3 3.3.0 REQUIRED NO_MODULE) # It adds the target Eigen3::Eigen - find_package(pinocchio 2.4.0 NO_MODULE NO_CMAKE_SYSTEM_PATH) # It adds target pinocchio::pinocchio. Pinocchio was header-only before 2.4.0. + find_package(pinocchio 2.4.4 NO_MODULE NO_CMAKE_SYSTEM_PATH) # Pinocchio >= 2.4.4 is required to support convex mesh collision and display if(pinocchio_FOUND) message("-- Found pinocchio: version ${pinocchio_VERSION}") else() @@ -105,7 +105,7 @@ if(pinocchio_FOUND) elseif(hpp-fcl_FOUND) target_link_libraries_system(${PROJECT_NAME} hpp-fcl::hpp-fcl) elseif(LEGACY_MODE) - target_link_libraries(${PROJECT_NAME} hpp-fcl) + target_link_libraries(${PROJECT_NAME} pinocchio hpp-fcl) # It is necessary to link to Pinocchio shared lib since 2.4.0 endif() if(NOT LEGACY_MODE) if (NOT "${urdfdom_LIBRARIES}" MATCHES ".*tinyxml.*") diff --git a/core/include/jiminy/core/Utilities.h b/core/include/jiminy/core/Utilities.h index e97231e8a..f0831851d 100644 --- a/core/include/jiminy/core/Utilities.h +++ b/core/include/jiminy/core/Utilities.h @@ -100,7 +100,7 @@ namespace jiminy { return std::static_pointer_cast(shared_from_base(that)); } - + // ======================== is_vector =========================== template @@ -282,6 +282,13 @@ namespace jiminy hresult_t getJointTypeVelocitySuffixes(joint_t const & jointTypeIn, std::vector & jointTypeSuffixesOut); + hresult_t getBodyIdx(pinocchio::Model const & model, + std::string const & bodyName, + int32_t & bodyIdx); + hresult_t getBodiesIdx(pinocchio::Model const & model, + std::vector const & bodiesNames, + std::vector & bodiesIdx); + hresult_t getFrameIdx(pinocchio::Model const & model, std::string const & frameName, int32_t & frameIdx); diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h index 0cc701589..4c398c041 100644 --- a/core/include/jiminy/core/engine/EngineMultiRobot.h +++ b/core/include/jiminy/core/engine/EngineMultiRobot.h @@ -625,13 +625,27 @@ namespace jiminy vectorN_t const & v, vectorN_t const & a); + /// \brief Compute the force resulting from ground contact on a given body. + /// + /// \param[in] system System for which to perform computation. + /// \param[in] collisionPairIdx Id of the collision pair associated with the body + /// \return Contact force, at parent frame, in the global frame. + pinocchio::Force computeContactDynamicsAtBody(systemDataHolder_t const & system, + int32_t const & collisionPairIdx) const; + /// \brief Compute the force resulting from ground contact on a given frame. /// /// \param[in] system System for which to perform computation. - /// \param[in] header Id of the frame in contact. + /// \param[in] frameIdx Id of the frame in contact. /// \return Contact force, in the global frame. + pinocchio::Force computeContactDynamicsAtFrame(systemDataHolder_t const & system, + int32_t const & frameIdx) const; + + /// \brief Compute the force resulting from ground contact for a given normal direction and depth. pinocchio::Force computeContactDynamics(systemDataHolder_t const & system, - int32_t const & frameIdx) const; + vector3_t const & nGround, + float64_t const & depth, + vector3_t const & vFrameInWorld) const; void computeCommand(systemDataHolder_t & system, float64_t const & t, diff --git a/core/include/jiminy/core/robot/BasicSensors.h b/core/include/jiminy/core/robot/BasicSensors.h index acc4e3ac5..bb586228b 100644 --- a/core/include/jiminy/core/robot/BasicSensors.h +++ b/core/include/jiminy/core/robot/BasicSensors.h @@ -40,6 +40,33 @@ namespace jiminy quaternion_t sensorRotationBias_; ///< Sensor rotation bias. }; + class ContactSensor : public AbstractSensorTpl + { + public: + ContactSensor(std::string const & name); + ~ContactSensor(void) = default; + + auto shared_from_this() { return shared_from(this); } + + hresult_t initialize(std::string const & frameName); + + virtual hresult_t refreshProxies(void) final override; + + std::string const & getFrameName(void) const; + int32_t const & getFrameIdx(void) const; + + private: + virtual hresult_t set(float64_t const & t, + Eigen::Ref const & q, + Eigen::Ref const & v, + Eigen::Ref const & a, + vectorN_t const & uMotor) final override; + + private: + std::string frameName_; + int32_t frameIdx_; + }; + class ForceSensor : public AbstractSensorTpl { public: @@ -54,6 +81,8 @@ namespace jiminy std::string const & getFrameName(void) const; int32_t const & getFrameIdx(void) const; + std::string const & getBodyName(void) const; + int32_t getJointIdx(void) const; private: virtual hresult_t set(float64_t const & t, @@ -65,6 +94,7 @@ namespace jiminy private: std::string frameName_; int32_t frameIdx_; + int32_t parentBodyFrameIdx_; }; class EncoderSensor : public AbstractSensorTpl diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h index e19c284f6..033f6867b 100644 --- a/core/include/jiminy/core/robot/Model.h +++ b/core/include/jiminy/core/robot/Model.h @@ -41,11 +41,21 @@ namespace jiminy return config; }; + virtual configHolder_t getDefaultCollisionOptions() + { + // Add extra options or update default values + configHolder_t config; + config["maxContactPointsPerBody"] = 3U; // Max number of contact points per collision pairs + + return config; + }; + virtual configHolder_t getDefaultModelOptions() { configHolder_t config; config["dynamics"] = getDefaultDynamicsOptions(); config["joints"] = getDefaultJointOptions(); + config["collisions"] = getDefaultCollisionOptions(); return config; }; @@ -94,14 +104,27 @@ namespace jiminy } }; + struct collisionOptions_t + { + uint32_t const maxContactPointsPerBody; + + collisionOptions_t(configHolder_t const & options) : + maxContactPointsPerBody(boost::get(options.at("maxContactPointsPerBody"))) + { + // Empty. + } + }; + struct modelOptions_t { dynamicsOptions_t const dynamics; jointOptions_t const joints; + collisionOptions_t const collisions; modelOptions_t(configHolder_t const & options) : dynamics(boost::get(options.at("dynamics"))), - joints(boost::get(options.at("joints"))) + joints(boost::get(options.at("joints"))), + collisions(boost::get(options.at("collisions"))) { // Empty. } @@ -123,8 +146,8 @@ namespace jiminy std::string const & parentBodyName, pinocchio::SE3 const & framePlacement); hresult_t removeFrame(std::string const & frameName); - hresult_t addContactBodies(std::vector const & bodyNames); - hresult_t removeContactBodies(std::vector const & frameNames = {}); + hresult_t addCollisionBodies(std::vector const & bodyNames); + hresult_t removeCollisionBodies(std::vector const & frameNames = {}); hresult_t addContactPoints(std::vector const & frameNames); hresult_t removeContactPoints(std::vector const & frameNames = {}); @@ -142,7 +165,9 @@ namespace jiminy int32_t const & nv(void) const; int32_t const & nx(void) const; + std::vector const & getCollisionBodiesNames(void) const; std::vector const & getContactFramesNames(void) const; + std::vector const & getCollisionBodiesIdx(void) const; std::vector const & getContactFramesIdx(void) const; std::vector const & getRigidJointsNames(void) const; std::vector const & getRigidJointsModelIdx(void) const; @@ -169,6 +194,7 @@ namespace jiminy bool_t const & hasFreeflyer); hresult_t generateModelFlexible(void); hresult_t generateModelBiased(void); + hresult_t refreshCollisionsProxies(void); hresult_t refreshContactsProxies(void); virtual hresult_t refreshProxies(void); @@ -176,7 +202,7 @@ namespace jiminy pinocchio::Model pncModel_; mutable pinocchio::Data pncData_; pinocchio::GeometryModel pncGeometryModel_; - mutable std::unique_ptr pncGeometryData_; // Using ptr to avoid having to initialize it with an empty GeometryModel, which causes segfault for Pinocchio < 2.4.0. + mutable std::unique_ptr pncGeometryData_; // Using smart ptr to avoid having to initialize it with an empty GeometryModel, which causes segfault for Pinocchio < 2.4.0. pinocchio::Model pncModelRigidOrig_; pinocchio::Data pncDataRigidOrig_; std::unique_ptr mdlOptions_; @@ -188,8 +214,9 @@ namespace jiminy bool_t hasFreeflyer_; configHolder_t mdlOptionsHolder_; - std::vector contactBodiesNames_; ///< Name of the contact bodies of the robot + std::vector collisionBodiesNames_; ///< Name of the collision bodies of the robot std::vector contactFramesNames_; ///< Name of the contact frames of the robot + std::vector collisionBodiesIdx_; ///< Indices of the collision bodies in the frame list of the robot std::vector contactFramesIdx_; ///< Indices of the contact frames in the frame list of the robot std::vector rigidJointsNames_; ///< Name of the actual joints of the robot, not taking into account the freeflyer std::vector rigidJointsModelIdx_; ///< Index of the actual joints in the pinocchio robot diff --git a/core/src/Utilities.cc b/core/src/Utilities.cc index 0ecf8b39e..8626a4ee1 100644 --- a/core/src/Utilities.cc +++ b/core/src/Utilities.cc @@ -921,6 +921,41 @@ namespace jiminy return returnCode; } + hresult_t getBodyIdx(pinocchio::Model const & model, + std::string const & bodyName, + int32_t & bodyIdx) + { + if (!model.existBodyName(bodyName)) + { + std::cout << "Error - Utilities::getFrameIdx - Frame not found in urdf." << std::endl; + return hresult_t::ERROR_BAD_INPUT; + } + + bodyIdx = model.getBodyId(bodyName); + + return hresult_t::SUCCESS; + } + + hresult_t getBodiesIdx(pinocchio::Model const & model, + std::vector const & bodiesNames, + std::vector & bodiesIdx) + { + hresult_t returnCode = hresult_t::SUCCESS; + + bodiesIdx.resize(0); + for (std::string const & name : bodiesNames) + { + if (returnCode == hresult_t::SUCCESS) + { + int32_t idx; + returnCode = getFrameIdx(model, name, idx); + bodiesIdx.push_back(std::move(idx)); + } + } + + return returnCode; + } + hresult_t getJointPositionIdx(pinocchio::Model const & model, std::string const & jointName, std::vector & jointPositionIdx) diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 8736e94ce..65c73d417 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -679,20 +679,28 @@ namespace jiminy vectorN_t const & a = system.state.a; computeForwardKinematics(system, q, v, a); - // Make sure that the contact forces are bounded + // Make sure that the contact forces are bounded. + // TODO: One should rather use something like 10 * m * g instead of a fix threshold + float64_t forceMax = 0.0; auto const & contactFramesIdx = system.robot->getContactFramesIdx(); for (uint32_t i=0; i < contactFramesIdx.size(); i++) { - pinocchio::Force fextInFrame; - fextInFrame = computeContactDynamics(system, contactFramesIdx[i]); - //TODO: One should rather use something like 10 * m * g instead of a fix threshold - if (fextInFrame.linear().norm() > 1e5) - { - std::cout << "Error - EngineMultiRobot::start - The initial force exceeds 1e5 for at least one contact point, "\ - "which is forbidden for the sake of numerical stability. Please update the initial state." << std::endl; - returnCode = hresult_t::ERROR_BAD_INPUT; - } - break; + pinocchio::Force fext = computeContactDynamicsAtFrame(system, contactFramesIdx[i]); + forceMax = std::max(forceMax, fext.linear().norm()); + } + + auto const & collisionBodiesIdx = system.robot->getCollisionBodiesIdx(); + for (uint32_t i=0; i < collisionBodiesIdx.size(); i++) + { + pinocchio::Force fext = computeContactDynamicsAtBody(system, i); + forceMax = std::max(forceMax, fext.linear().norm()); + } + + if (forceMax > 1e5) + { + std::cout << "Error - EngineMultiRobot::start - The initial force exceeds 1e5 for at least one contact point, "\ + "which is forbidden for the sake of numerical stability. Please update the initial state." << std::endl; + returnCode = hresult_t::ERROR_BAD_INPUT; } // Activate every force impulse starting at t=0 @@ -1760,35 +1768,113 @@ namespace jiminy system.robot->pncGeometryModel_, *system.robot->pncGeometryData_); pinocchio::computeCollisions(system.robot->pncGeometryModel_, - *system.robot->pncGeometryData_); + *system.robot->pncGeometryData_, + false); // Update collision results + pinocchio::computeDistances(system.robot->pncGeometryModel_, + *system.robot->pncGeometryData_); // Update distance results. } - pinocchio::Force EngineMultiRobot::computeContactDynamics(systemDataHolder_t const & system, - int32_t const & frameIdx) const + pinocchio::Force EngineMultiRobot::computeContactDynamicsAtBody(systemDataHolder_t const & system, + int32_t const & collisionPairIdx) const + { + // TODO: It is assumed that the ground is flat. For now ground profile is not supported + // with body collision. Nevertheless it should not be to hard to generated a collision + // object simply by sampling points on the profile. + + // Get the frame Idx + uint32_t geometryIdx = system.robot->pncGeometryModel_.collisionPairs[collisionPairIdx].first; + uint32_t parentFrameIdx = system.robot->pncGeometryModel_.geometryObjects[geometryIdx].parentFrame; + + // Get the pose of the frame wrt the world + pinocchio::SE3 const & transformFrameInWorld = system.robot->pncData_.oMf[parentFrameIdx]; + matrix3_t const & rotFrameInWorld = system.robot->pncData_.oMf[parentFrameIdx].rotation(); + + // Extract collision and distance results + hpp::fcl::CollisionResult collisionResult = system.robot->pncGeometryData_->collisionResults[collisionPairIdx]; + hpp::fcl::DistanceResult distanceResult = system.robot->pncGeometryData_->distanceResults[collisionPairIdx]; + + if (collisionResult.isCollision()) + { + // Extract the contact information. + // Note that there is always a single contact point while computing the collision + // between two shape objects, as it is the case there (convex-box). + vector3_t nGround = vector3_t::Zero(); + float64_t depth = collisionResult.getContact(0).penetration_depth; + pinocchio::SE3 posContactInWorld = pinocchio::SE3::Identity(); + posContactInWorld.translation() = distanceResult.nearest_points[0]; + + // Get frame motion in the motion frame. + vector3_t const motionFrameInWorld = pinocchio::getFrameVelocity( + system.robot->pncModel_, system.robot->pncData_, parentFrameIdx).linear(); + vector3_t const vFrameInWorld = rotFrameInWorld * motionFrameInWorld; + + // Compute the ground reaction force at contact point in world frame + pinocchio::Force const fextAtContactInGlobal = computeContactDynamics(system, nGround, depth, vFrameInWorld); + + // Move the force at parent frame location + // TODO: 1. Compute the position of the contact point in local frame + // 2. Move the force at parent frame location + pinocchio::SE3 const posContactLocal = transformFrameInWorld.actInv(posContactInWorld); + pinocchio::Force fextAtParentFrameInGlobal = posContactLocal.actInv(fextAtContactInGlobal); + + return fextAtParentFrameInGlobal; + } + else + { + return pinocchio::Force::Zero(); + } + } + + pinocchio::Force EngineMultiRobot::computeContactDynamicsAtFrame(systemDataHolder_t const & system, + int32_t const & frameIdx) const { /* Returns the external force in the contact frame. It must then be converted into a force onto the parent joint. /!\ Note that the contact dynamics depends only on kinematics data. /!\ */ - contactOptions_t const & contactOptions_ = engineOptions_->contacts; - - matrix3_t const & tformFrameRot = system.robot->pncData_.oMf[frameIdx].rotation(); + // Get the pose of the frame wrt the world vector3_t const & posFrame = system.robot->pncData_.oMf[frameIdx].translation(); + matrix3_t const & rotFrame = system.robot->pncData_.oMf[frameIdx].rotation(); - // Initialize the contact force - vector3_t fextInWorld; + // Compute the ground normal and penetration depth at the contact point auto ground = engineOptions_->world.groundProfile(posFrame); float64_t const & zGround = std::get(ground); vector3_t & nGround = std::get(ground); nGround.normalize(); float64_t const depth = (posFrame(2) - zGround) * nGround(2); // First-order projection (exact assuming flat surface) + // Only compute the ground reaction force if the penetration depth is positive if (depth < 0.0) { // Get frame motion in the motion frame. vector3_t const motionFrame = pinocchio::getFrameVelocity( system.robot->pncModel_, system.robot->pncData_, frameIdx).linear(); - vector3_t const vFrameInWorld = tformFrameRot * motionFrame; + vector3_t const vFrameInWorld = rotFrame * motionFrame; + + // Compute the ground reaction force in world frame + return computeContactDynamics(system, nGround, depth, vFrameInWorld); + } + else + { + // Not in contact with the ground, thus no force applied + return pinocchio::Force::Zero(); + } + } + + pinocchio::Force EngineMultiRobot::computeContactDynamics(systemDataHolder_t const & system, + vector3_t const & nGround, + float64_t const & depth, + vector3_t const & vFrameInWorld) const + { + // Initialize the contact force + vector3_t fextInWorld; + + if (depth < 0.0) + { + // Extract some proxies + contactOptions_t const & contactOptions_ = engineOptions_->contacts; + + // Compute the penetration speed float64_t const vDepth = vFrameInWorld.dot(nGround); // Compute normal force @@ -1965,14 +2051,13 @@ namespace jiminy Eigen::Ref const & v, forceVector_t & fext) { - // Compute the contact forces + // Compute the forces at contact points std::vector const & contactFramesIdx = system.robot->getContactFramesIdx(); for (uint32_t i=0; i < contactFramesIdx.size(); i++) { // Compute force at the given contact frame. int32_t const & frameIdx = contactFramesIdx[i]; - pinocchio::Force & fextInGlobal = system.robot->contactForces_[i]; - fextInGlobal = computeContactDynamics(system, frameIdx); + pinocchio::Force const fextInGlobal = computeContactDynamicsAtFrame(system, frameIdx); // Apply the force at the origin of the parent joint frame pinocchio::Force const fextLocal = convertForceGlobalFrameToJoint( @@ -1981,10 +2066,25 @@ namespace jiminy fext[parentIdx] += fextLocal; // Convert contact force from the global frame to the local frame to store it in contactForces_. + // TODO: Why not to use fextLocal directly ? system.robot->contactForces_[i].linear() = system.robot->pncData_.oMf[frameIdx].rotation().transpose() * fextInGlobal.linear(); system.robot->contactForces_[i].angular() = system.robot->pncData_.oMf[frameIdx].rotation().transpose() * fextInGlobal.angular(); } + // Compute the force at collision bodies + std::vector const & collisionBodiesIdx = system.robot->getCollisionBodiesIdx(); + for (uint32_t i=0; i < collisionBodiesIdx.size(); i++) + { + // Compute force at the given contact frame. + int32_t const & bodyIdx = collisionBodiesIdx[i]; + pinocchio::Force const fextInGlobal = computeContactDynamicsAtBody(system, i); + + // Apply the force at the origin of the parent joint frame + pinocchio::Force const fextLocal = convertForceGlobalFrameToJoint( + system.robot->pncModel_, system.robot->pncData_, bodyIdx, fextInGlobal); + fext[bodyIdx] += fextLocal; + } + // Add the effect of user-defined external impulse forces auto forcesImpulseActiveIt = system.forcesImpulseActive.begin(); auto forcesImpulseIt = system.forcesImpulse.begin(); diff --git a/core/src/robot/BasicSensors.cc b/core/src/robot/BasicSensors.cc index 749f0bf98..c2d82d388 100644 --- a/core/src/robot/BasicSensors.cc +++ b/core/src/robot/BasicSensors.cc @@ -198,6 +198,104 @@ namespace jiminy } + // ===================== ContactSensor ========================= + + template<> + std::string const AbstractSensorTpl::type_("ContactSensor"); + template<> + bool_t const AbstractSensorTpl::areFieldnamesGrouped_(false); + template<> + std::vector const AbstractSensorTpl::fieldNames_({"FX", "FY", "FZ"}); + + ContactSensor::ContactSensor(std::string const & name) : + AbstractSensorTpl(name), + frameName_(), + frameIdx_(0) + { + // Empty. + } + + hresult_t ContactSensor::initialize(std::string const & frameName) + { + hresult_t returnCode = hresult_t::SUCCESS; + + if (!isAttached_) + { + std::cout << "Error - ContactSensor::initialize - Sensor not attached to any robot. Impossible to initialize it." << std::endl; + returnCode = hresult_t::ERROR_GENERIC; + } + + if (returnCode == hresult_t::SUCCESS) + { + frameName_ = frameName; + isInitialized_ = true; + returnCode = refreshProxies(); + } + + if (returnCode != hresult_t::SUCCESS) + { + isInitialized_ = false; + } + + return returnCode; + } + + hresult_t ContactSensor::refreshProxies(void) + { + hresult_t returnCode = hresult_t::SUCCESS; + + if (!robot_->getIsInitialized()) + { + std::cout << "Error - ContactSensor::refreshProxies - Robot not initialized. Impossible to refresh model-dependent proxies." << std::endl; + returnCode = hresult_t::ERROR_INIT_FAILED; + } + + if (returnCode == hresult_t::SUCCESS) + { + if (!isInitialized_) + { + std::cout << "Error - ContactSensor::refreshProxies - Sensor not initialized. Impossible to refresh model-dependent proxies." << std::endl; + returnCode = hresult_t::ERROR_INIT_FAILED; + } + } + + if (returnCode == hresult_t::SUCCESS) + { + returnCode = ::jiminy::getFrameIdx(robot_->pncModel_, frameName_, frameIdx_); + } + + return returnCode; + } + + std::string const & ContactSensor::getFrameName(void) const + { + return frameName_; + } + + int32_t const & ContactSensor::getFrameIdx(void) const + { + return frameIdx_; + } + + hresult_t ContactSensor::set(float64_t const & t, + Eigen::Ref const & q, + Eigen::Ref const & v, + Eigen::Ref const & a, + vectorN_t const & uMotor) + { + if (!isInitialized_) + { + std::cout << "Error - ContactSensor::set - Sensor not initialized. Impossible to set sensor data." << std::endl; + return hresult_t::ERROR_INIT_FAILED; + } + + std::vector const & contactFramesIdx = robot_->getContactFramesIdx(); + std::vector::const_iterator it = std::find(contactFramesIdx.begin(), contactFramesIdx.end(), frameIdx_); + data() = robot_->contactForces_[std::distance(contactFramesIdx.begin(), it)].linear(); + + return hresult_t::SUCCESS; + } + // ===================== ForceSensor ========================= template<> @@ -205,12 +303,13 @@ namespace jiminy template<> bool_t const AbstractSensorTpl::areFieldnamesGrouped_(false); template<> - std::vector const AbstractSensorTpl::fieldNames_({"FX", "FY", "FZ"}); + std::vector const AbstractSensorTpl::fieldNames_({"FX", "FY", "FZ", "MX", "MY", "MZ"}); ForceSensor::ForceSensor(std::string const & name) : AbstractSensorTpl(name), frameName_(), - frameIdx_(0) + frameIdx_(0), + parentBodyFrameIdx_(0) { // Empty. } @@ -264,6 +363,17 @@ namespace jiminy returnCode = ::jiminy::getFrameIdx(robot_->pncModel_, frameName_, frameIdx_); } + if (returnCode == hresult_t::SUCCESS) + { + parentBodyFrameIdx_ = robot_->pncModel_.frames[frameIdx_].parent; + pinocchio::Frame const & parentBodyFrame = robot_->pncModel_.frames[parentBodyFrameIdx_]; + if (parentBodyFrame.type != pinocchio::FrameType::BODY) + { + std::cout << "Error - ForceSensor::refreshProxies - The parent of the frame is not a body. Impossible to refresh model-dependent proxies." << std::endl; + returnCode = hresult_t::ERROR_INIT_FAILED; + } + } + return returnCode; } @@ -277,6 +387,16 @@ namespace jiminy return frameIdx_; } + std::string const & ForceSensor::getBodyName(void) const + { + return robot_->pncModel_.frames[parentBodyFrameIdx_].name; + } + + int32_t ForceSensor::getJointIdx(void) const + { + return robot_->pncModel_.frames[parentBodyFrameIdx_].parent; + } + hresult_t ForceSensor::set(float64_t const & t, Eigen::Ref const & q, Eigen::Ref const & v, @@ -289,9 +409,9 @@ namespace jiminy return hresult_t::ERROR_INIT_FAILED; } - std::vector const & contactFramesIdx = robot_->getContactFramesIdx(); - std::vector::const_iterator it = std::find(contactFramesIdx.begin(), contactFramesIdx.end(), frameIdx_); - data() = robot_->contactForces_[std::distance(contactFramesIdx.begin(), it)].linear(); + pinocchio::SE3 const & framePlacement = robot_->pncModel_.frames[frameIdx_].placement; + pinocchio::Force f = framePlacement.actInv(robot_->pncData_.f[getJointIdx()]); + data() = f.toVector(); return hresult_t::SUCCESS; } diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index bc219c0d5..0fedeb0b7 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -30,8 +30,9 @@ namespace jiminy urdfPath_(), hasFreeflyer_(false), mdlOptionsHolder_(), - contactBodiesNames_(), + collisionBodiesNames_(), contactFramesNames_(), + collisionBodiesIdx_(), contactFramesIdx_(), rigidJointsNames_(), rigidJointsModelIdx_(), @@ -202,106 +203,152 @@ namespace jiminy return returnCode; } - hresult_t Model::addContactBodies(std::vector const & bodyNames) + hresult_t Model::addCollisionBodies(std::vector const & bodyNames) { if (!isInitialized_) { - std::cout << "Error - Model::addContactBodies - Model not initialized." << std::endl; + std::cout << "Error - Model::addCollisionBodies - Model not initialized." << std::endl; return hresult_t::ERROR_INIT_FAILED; } // Make sure that the body list is not empty if (bodyNames.empty()) { - std::cout << "Error - Model::addContactBodies - The list of bodies must not be empty." << std::endl; + std::cout << "Error - Model::addCollisionBodies - The list of bodies must not be empty." << std::endl; return hresult_t::ERROR_BAD_INPUT; } // Make sure that no body are duplicates if (checkDuplicates(bodyNames)) { - std::cout << "Error - Model::addContactBodies - Some bodies are duplicates." << std::endl; + std::cout << "Error - Model::addCollisionBodies - Some bodies are duplicates." << std::endl; return hresult_t::ERROR_BAD_INPUT; } - // Make sure that there is no contact already associated with any of the bodies in the list - if (checkIntersection(contactBodiesNames_, bodyNames)) + // Make sure that there is no collision already associated with any of the bodies in the list + if (checkIntersection(collisionBodiesNames_, bodyNames)) { - std::cout << "Error - Model::addContactBodies - At least one of the body is already been associated with a contact." << std::endl; + std::cout << "Error - Model::addCollisionBodies - At least one of the body is already been associated with a collision." << std::endl; return hresult_t::ERROR_BAD_INPUT; } - // Make sure that all the frames exist - for (std::string const & body : bodyNames) + // Make sure that all the bodies exist + for (std::string const & name : bodyNames) { - if (!pncGeometryModel_.existGeometryName(body)) + if (!pncModel_.existBodyName(name)) { - std::cout << "Error - Model::addContactBodies - At least one of the body does not exist." << std::endl; + std::cout << "Error - Model::addCollisionBodies - At least one of the body does not exist." << std::endl; return hresult_t::ERROR_BAD_INPUT; } } - // Add the list of bodies to the set of contact bodies - contactBodiesNames_.insert(contactBodiesNames_.end(), bodyNames.begin(), bodyNames.end()); + // Make sure that one and only one geometry is associated with each body + for (std::string const & name : bodyNames) + { + int32_t nChildGeom = 0; + for (pinocchio::GeometryObject const & geom : pncGeometryModel_.geometryObjects) + { + if (pncModel_.frames[geom.parentFrame].name == name) + { + nChildGeom++; + } + } + if (nChildGeom != 1) + { + std::cout << "Error - Model::addCollisionBodies - Collision is only supported for bodies associated with one and only one geometry." << std::endl; + return hresult_t::ERROR_BAD_INPUT; + } + } + + // Add the list of bodies to the set of collision bodies + collisionBodiesNames_.insert(collisionBodiesNames_.end(), bodyNames.begin(), bodyNames.end()); // Create the collision pairs and add them to the geometry model of the robot pinocchio::GeomIndex const & groundId = pncGeometryModel_.getGeometryId("ground"); for (std::string const & name : bodyNames) { - pinocchio::GeomIndex const & bodyId = pncGeometryModel_.getGeometryId(name); + // Find the body id by looking at the first geometry having it for parent + pinocchio::GeomIndex bodyId; + for (uint32_t i=0; i const & bodyNames) + hresult_t Model::removeCollisionBodies(std::vector const & bodyNames) { if (!isInitialized_) { - std::cout << "Error - Model::removeContactBodies - Model not initialized." << std::endl; + std::cout << "Error - Model::removeCollisionBodies - Model not initialized." << std::endl; return hresult_t::ERROR_INIT_FAILED; } // Make sure that no body are duplicates if (checkDuplicates(bodyNames)) { - std::cout << "Error - Model::removeContactBodies - Some bodies are duplicates." << std::endl; + std::cout << "Error - Model::removeCollisionBodies - Some bodies are duplicates." << std::endl; return hresult_t::ERROR_BAD_INPUT; } - // Make sure that every body in the list is associated with a contact - if (!checkInclusion(contactBodiesNames_, bodyNames)) + // Make sure that every body in the list is associated with a collision + if (!checkInclusion(collisionBodiesNames_, bodyNames)) { - std::cout << "Error - Model::removeContactBodies - At least one of the body is not associated with any contact." << std::endl; + std::cout << "Error - Model::removeCollisionBodies - At least one of the body is not associated with any collision." << std::endl; return hresult_t::ERROR_BAD_INPUT; } - // Remove the list of bodies from the set of contact bodies + // Remove the list of bodies from the set of collision bodies if (!bodyNames.empty()) { - eraseVector(contactBodiesNames_, bodyNames); + eraseVector(collisionBodiesNames_, bodyNames); } else { - contactBodiesNames_.clear(); + collisionBodiesNames_.clear(); } // Get the indices of the corresponding collision pairs in the geometry model of the robot and remove them pinocchio::GeomIndex const & groundId = pncGeometryModel_.getGeometryId("ground"); for (std::string const & name : bodyNames) { - pinocchio::GeomIndex const & bodyId = pncGeometryModel_.getGeometryId(name); - pinocchio::CollisionPair const collisionPair(bodyId, groundId); + // Find the body id by looking at the first geometry having it for parent + pinocchio::GeomIndex bodyId; + for (uint32_t i=0; i(pncGeometryModel_); - pinocchio::updateGeometryPlacements(pncModel_, - pncData_, - pncGeometryModel_, - *pncGeometryData_); } if (returnCode == hresult_t::SUCCESS) @@ -697,6 +737,11 @@ namespace jiminy } } + if (returnCode == hresult_t::SUCCESS) + { + returnCode = refreshCollisionsProxies(); + } + if (returnCode == hresult_t::SUCCESS) { returnCode = refreshContactsProxies(); @@ -705,6 +750,43 @@ namespace jiminy return returnCode; } + hresult_t Model::refreshCollisionsProxies(void) + { + hresult_t returnCode = hresult_t::SUCCESS; + + if (!isInitialized_) + { + std::cout << "Error - Model::refreshCollisionsProxies - Model not initialized." << std::endl; + returnCode = hresult_t::ERROR_INIT_FAILED; + } + + if (returnCode == hresult_t::SUCCESS) + { + // A new geometry data object must be instantiate after changing the collision pairs + pncGeometryData_ = std::make_unique(pncGeometryModel_); + pinocchio::updateGeometryPlacements(pncModel_, + pncData_, + pncGeometryModel_, + *pncGeometryData_); + + // Set the max number of contact points per collision pairs + // Only a global collisionRequest is available for Pinocchio < 2.4.4, instead of one for each collision pair. + # if PINOCCHIO_MINOR_VERSION >= 4 || PINOCCHIO_PATCH_VERSION >= 4 + for (hpp::fcl::CollisionRequest & collisionRequest : pncGeometryData_->collisionRequests) + { + collisionRequest.num_max_contacts = mdlOptions_->collisions.maxContactPointsPerBody; + } + #else + pncGeometryData_->collisionRequest.num_max_contacts = mdlOptions_->collisions.maxContactPointsPerBody; + #endif + + // Extract the contact frames indices in the model + getFramesIdx(pncModel_, collisionBodiesNames_, collisionBodiesIdx_); + } + + return returnCode; + } + hresult_t Model::refreshContactsProxies(void) { hresult_t returnCode = hresult_t::SUCCESS; @@ -718,8 +800,7 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { // Reset the contact force internal buffer - uint64_t nContacts = contactFramesNames_.size() + contactBodiesNames_.size(); - contactForces_ = forceVector_t(nContacts, pinocchio::Force::Zero()); + contactForces_ = forceVector_t(contactFramesNames_.size(), pinocchio::Force::Zero()); // Extract the contact frames indices in the model getFramesIdx(pncModel_, contactFramesNames_, contactFramesIdx_); @@ -733,6 +814,7 @@ namespace jiminy bool_t internalBuffersMustBeUpdated = false; bool_t isFlexibleModelInvalid = false; bool_t isCurrentModelInvalid = false; + bool_t isCollisionDataInvalid = false; if (isInitialized_) { /* Check that the following user parameters has the right dimension, @@ -770,7 +852,7 @@ namespace jiminy internalBuffersMustBeUpdated |= (jointsVelocityLimitDiff.array().abs() >= EPS).all(); } - // Check if the flexible model and its associated proxies must be regenerated + // Check if the flexible model and its proxies must be regenerated configHolder_t & dynOptionsHolder = boost::get(modelOptions.at("dynamics")); bool_t const & enableFlexibleModel = boost::get(dynOptionsHolder.at("enableFlexibleModel")); @@ -789,6 +871,20 @@ namespace jiminy { isCurrentModelInvalid = true; } + + // Check that the collisions options are valid + configHolder_t & collisionOptionsHolder = + boost::get(modelOptions.at("collisions")); + uint32_t const & maxContactPointsPerBody = boost::get(collisionOptionsHolder.at("maxContactPointsPerBody")); + if (maxContactPointsPerBody < 1) + { + std::cout << "Error - Model::setOptions - The number of contact points by collision pair 'maxContactPointsPerBody' must be at least 1." << std::endl; + return hresult_t::ERROR_BAD_INPUT; + } + if (mdlOptions_ && maxContactPointsPerBody != mdlOptions_->collisions.maxContactPointsPerBody) + { + isCollisionDataInvalid = true; + } } // Update the internal options @@ -813,6 +909,11 @@ namespace jiminy // Update the info extracted from the model refreshProxies(); } + else if (isCollisionDataInvalid) + { + // Update the collision data + refreshCollisionsProxies(); + } return hresult_t::SUCCESS; } @@ -872,14 +973,28 @@ namespace jiminy // Build the robot geometry model pinocchio::urdf::buildGeom(pncModel_, urdfPath, pinocchio::COLLISION, pncGeometryModel_); - // Instantiate ground plane FCL geometry, wrapped as a pinocchio collision geometry - hpp::fcl::Vec3f const normal(0.0, 0.0, 1.0); - float64_t const offset = 0; - auto groudPlane = boost::shared_ptr(new hpp::fcl::Plane(normal, offset)); + // Replace the geometry object by their convex representation for efficiency + #if PINOCCHIO_MINOR_VERSION >= 4 || PINOCCHIO_PATCH_VERSION >= 4 + for (uint32_t i=0; i(pncGeometryModel_.geometryObjects[i].geometry); + bvh->buildConvexHull(true); + pncGeometryModel_.geometryObjects[i].geometry = bvh->convex; + } + #endif + + // Instantiate ground FCL box geometry, wrapped as a pinocchio collision geometry. + // Note that half-space cannot be used for Shape-Shape collision because it has no + // shape support. So a very large box is used instead. In the future, it could be + // a more complex topological object, even a mesh would be supported. + auto groudBox = boost::shared_ptr(new hpp::fcl::Box(1000.0, 1000.0, 2.0)); // Create a Pinocchio Geometry object associated with the ground plan. - // Its parent frame and parent joint are the universe, and it is centered and aligned with world frame. - pinocchio::GeometryObject groundPlane("ground", 0, 0, groudPlane, pinocchio::SE3::Identity()); // pinocchio::FrameIndex / pinocchio::JointIndex required ? + // Its parent frame and parent joint are the universe. It is aligned with world frame, + // and the top face is the actual ground surface. + pinocchio::SE3 groundPose = pinocchio::SE3::Identity(); + groundPose.translation() = (vector3_t() << 0.0, 0.0, -1.0).finished(); + pinocchio::GeometryObject groundPlane("ground", 0, 0, groudBox, groundPose); // Add the ground plane pinocchio to the robot model pncGeometryModel_.addGeometryObject(groundPlane, pncModel_); @@ -980,11 +1095,21 @@ namespace jiminy return hresult_t::SUCCESS; } + std::vector const & Model::getCollisionBodiesNames(void) const + { + return collisionBodiesNames_; + } + std::vector const & Model::getContactFramesNames(void) const { return contactFramesNames_; } + std::vector const & Model::getCollisionBodiesIdx(void) const + { + return collisionBodiesIdx_; + } + std::vector const & Model::getContactFramesIdx(void) const { return contactFramesIdx_; diff --git a/examples/cartpole_py/test_simulation.py b/examples/cartpole_py/test_simulation.py index 78e61e978..21995f440 100644 --- a/examples/cartpole_py/test_simulation.py +++ b/examples/cartpole_py/test_simulation.py @@ -8,8 +8,8 @@ script_dir = os.path.dirname(os.path.realpath(__file__)) -os.environ["JIMINY_MESH_PATH"] = os.path.join(script_dir, "../../data") -urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], "cartpole/cartpole.urdf") +os.environ["JIMINY_DATA_PATH"] = os.path.join(script_dir, "../../data") +urdf_path = os.path.join(os.environ["JIMINY_DATA_PATH"], "cartpole/cartpole.urdf") motor_joint_names = ("slider_to_cart",) encoder_sensors_def = {"slider": "slider_to_cart", "pole": "cart_to_pole"} diff --git a/examples/double_pendulum_py/test_simulation.py b/examples/double_pendulum_py/test_simulation.py index ff22f3187..4e75e2e3a 100644 --- a/examples/double_pendulum_py/test_simulation.py +++ b/examples/double_pendulum_py/test_simulation.py @@ -11,8 +11,8 @@ # ################################ User parameters ####################################### script_dir = os.path.dirname(os.path.realpath(__file__)) -os.environ["JIMINY_MESH_PATH"] = os.path.join(script_dir, "../../data") -urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], "double_pendulum/double_pendulum.urdf") +os.environ["JIMINY_DATA_PATH"] = os.path.join(script_dir, "../../data") +urdf_path = os.path.join(os.environ["JIMINY_DATA_PATH"], "double_pendulum/double_pendulum.urdf") # ########################### Initialize the simulation ################################# diff --git a/examples/simple_pendulum_py/test_simulation.py b/examples/simple_pendulum_py/test_simulation.py index c52810c8b..9de899df9 100644 --- a/examples/simple_pendulum_py/test_simulation.py +++ b/examples/simple_pendulum_py/test_simulation.py @@ -8,7 +8,7 @@ from jiminy_py import core as jiminy from jiminy_py.viewer import extract_viewer_data_from_log, play_trajectories -from jiminy_py.core import HeatMapFunctor, heatMapType_t, ForceSensor +from jiminy_py.core import HeatMapFunctor, heatMapType_t, ContactSensor import pinocchio as pin from interactive_plot_util import interactive_legend @@ -36,8 +36,8 @@ position_control = not acceleration_control script_dir = os.path.dirname(os.path.realpath(__file__)) -os.environ["JIMINY_MESH_PATH"] = os.path.join(script_dir, "../../data") -urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], "simple_pendulum/simple_pendulum.urdf") +os.environ["JIMINY_DATA_PATH"] = os.path.join(script_dir, "../../data") +urdf_path = os.path.join(os.environ["JIMINY_DATA_PATH"], "simple_pendulum/simple_pendulum.urdf") # ########################### Initialize the simulation ################################# @@ -56,7 +56,7 @@ robot.attach_motor(motor) motor.initialize(joint_name) for sensor_name, frame_name in force_sensor_def.items(): - force_sensor = jiminy.ForceSensor(sensor_name) + force_sensor = jiminy.ContactSensor(sensor_name) robot.attach_sensor(force_sensor) force_sensor.initialize(frame_name) robot.add_contact_points(contact_points) @@ -171,7 +171,7 @@ def updateState(robot, q, v, sensors_data): dcmOut = comOut + vcomOut / omega # Create zmp from forces - forces = np.asarray(sensors_data[ForceSensor.type]) + forces = np.asarray(sensors_data[ContactSensor.type]) newWrench = pin.Force.Zero() for i,name in enumerate(contact_points): update_frame(robot.pinocchio_model_th, robot.pinocchio_data_th, name) @@ -306,27 +306,27 @@ def internalDynamics(t, q, v, sensors_data, u): robot_options["telemetry"]["enableImuSensors"] = True robot_options["telemetry"]["enableForceSensors"] = True -robot_options["sensors"]['ForceSensor'] = {} -robot_options["sensors"]['ForceSensor']['F1'] = {} -robot_options["sensors"]['ForceSensor']['F1']["noiseStd"] = [] -robot_options["sensors"]['ForceSensor']['F1']["bias"] = [] -robot_options["sensors"]['ForceSensor']['F1']["delay"] = 0.0 -robot_options["sensors"]['ForceSensor']['F1']["delayInterpolationOrder"] = 0 -robot_options["sensors"]['ForceSensor']['F2'] = {} -robot_options["sensors"]['ForceSensor']['F2']["noiseStd"] = [] -robot_options["sensors"]['ForceSensor']['F2']["bias"] = [] -robot_options["sensors"]['ForceSensor']['F2']["delay"] = 0.0 -robot_options["sensors"]['ForceSensor']['F2']["delayInterpolationOrder"] = 0 -robot_options["sensors"]['ForceSensor']['F3'] = {} -robot_options["sensors"]['ForceSensor']['F3']["noiseStd"] = [] -robot_options["sensors"]['ForceSensor']['F3']["bias"] = [] -robot_options["sensors"]['ForceSensor']['F3']["delay"] = 0.0 -robot_options["sensors"]['ForceSensor']['F3']["delayInterpolationOrder"] = 0 -robot_options["sensors"]['ForceSensor']['F4'] = {} -robot_options["sensors"]['ForceSensor']['F4']["noiseStd"] = [] -robot_options["sensors"]['ForceSensor']['F4']["bias"] = [] -robot_options["sensors"]['ForceSensor']['F4']["delay"] = 0.0 -robot_options["sensors"]['ForceSensor']['F4']["delayInterpolationOrder"] = 0 +robot_options["sensors"]['ContactSensor'] = {} +robot_options["sensors"]['ContactSensor']['F1'] = {} +robot_options["sensors"]['ContactSensor']['F1']["noiseStd"] = [] +robot_options["sensors"]['ContactSensor']['F1']["bias"] = [] +robot_options["sensors"]['ContactSensor']['F1']["delay"] = 0.0 +robot_options["sensors"]['ContactSensor']['F1']["delayInterpolationOrder"] = 0 +robot_options["sensors"]['ContactSensor']['F2'] = {} +robot_options["sensors"]['ContactSensor']['F2']["noiseStd"] = [] +robot_options["sensors"]['ContactSensor']['F2']["bias"] = [] +robot_options["sensors"]['ContactSensor']['F2']["delay"] = 0.0 +robot_options["sensors"]['ContactSensor']['F2']["delayInterpolationOrder"] = 0 +robot_options["sensors"]['ContactSensor']['F3'] = {} +robot_options["sensors"]['ContactSensor']['F3']["noiseStd"] = [] +robot_options["sensors"]['ContactSensor']['F3']["bias"] = [] +robot_options["sensors"]['ContactSensor']['F3']["delay"] = 0.0 +robot_options["sensors"]['ContactSensor']['F3']["delayInterpolationOrder"] = 0 +robot_options["sensors"]['ContactSensor']['F4'] = {} +robot_options["sensors"]['ContactSensor']['F4']["noiseStd"] = [] +robot_options["sensors"]['ContactSensor']['F4']["bias"] = [] +robot_options["sensors"]['ContactSensor']['F4']["delay"] = 0.0 +robot_options["sensors"]['ContactSensor']['F4']["delayInterpolationOrder"] = 0 engine_options["telemetry"]["enableConfiguration"] = True engine_options["telemetry"]["enableVelocity"] = True diff --git a/gym_jiminy/gym_jiminy/common/env_bases.py b/gym_jiminy/gym_jiminy/common/env_bases.py index 35b1ba6fb..ed4c43f8c 100644 --- a/gym_jiminy/gym_jiminy/common/env_bases.py +++ b/gym_jiminy/gym_jiminy/common/env_bases.py @@ -17,6 +17,7 @@ from pinocchio import neutral from jiminy_py.core import (EncoderSensor as enc, EffortSensor as effort, + ContactSensor as contact, ForceSensor as force, ImuSensor as imu) from jiminy_py.dynamics import compute_freeflyer_state_from_fixed_body @@ -35,6 +36,7 @@ FLEX_VEL_ANG_UNIVERSAL_MAX = 10000.0 MOTOR_EFFORT_UNIVERSAL_MAX = 1000.0 SENSOR_FORCE_UNIVERSAL_MAX = 100000.0 +SENSOR_MOMENT_UNIVERSAL_MAX = 10000.0 SENSOR_GYRO_UNIVERSAL_MAX = 100.0 SENSOR_ACCEL_UNIVERSAL_MAX = 10000.0 T_UNIVERSAL_MAX = 10000.0 @@ -278,12 +280,23 @@ def _refresh_learning_spaces(self): sensor_space_raw[effort.type]['max'][0, sensor_idx] = \ action_high[motor_idx] + # Replace inf bounds by the appropriate universal bound for the Contact sensors + if contact.type in sensors_data.keys(): + sensor_space_raw[contact.type]['min'][:,:] = \ + -SENSOR_FORCE_UNIVERSAL_MAX + sensor_space_raw[contact.type]['max'][:,:] = \ + +SENSOR_FORCE_UNIVERSAL_MAX + # Replace inf bounds by the appropriate universal bound for the Force sensors if force.type in sensors_data.keys(): - sensor_space_raw[force.type]['min'][:,:] = \ + sensor_space_raw[force.type]['min'][:3,:] = \ -SENSOR_FORCE_UNIVERSAL_MAX - sensor_space_raw[force.type]['max'][:,:] = \ + sensor_space_raw[force.type]['max'][:3,:] = \ +SENSOR_FORCE_UNIVERSAL_MAX + sensor_space_raw[force.type]['min'][3:,:] = \ + -SENSOR_MOMENT_UNIVERSAL_MAX + sensor_space_raw[force.type]['max'][3:,:] = \ + +SENSOR_MOMENT_UNIVERSAL_MAX # Replace inf bounds by the appropriate universal bound for the IMU sensors if imu.type in sensors_data.keys(): diff --git a/gym_jiminy/gym_jiminy/common/wrappers.py b/gym_jiminy/gym_jiminy/common/wrappers.py index 6ddbcabb7..3c9711dc6 100644 --- a/gym_jiminy/gym_jiminy/common/wrappers.py +++ b/gym_jiminy/gym_jiminy/common/wrappers.py @@ -4,10 +4,11 @@ from gym import Wrapper, ObservationWrapper, spaces -from jiminy_py.core import EncoderSensor as enc, \ - EffortSensor as effort, \ - ForceSensor as force, \ - ImuSensor as imu +from jiminy_py.core import (EncoderSensor as enc, + EffortSensor as effort, + ContactSensor as contact, + ForceSensor as force, + ImuSensor as imu) # Define universal scale for the observation and action space @@ -164,19 +165,19 @@ def _refresh_learning_spaces_scale(self): dtype=sensors_data[imu.type].dtype) # Set the quaternion scale - quat_imu_idx = ['Quat' in field for field in imu.fieldnames] + quat_imu_idx = [field.startswith('Quat') for field in imu.fieldnames] imu_sensors_scale[quat_imu_idx] = np.full( (sum(quat_imu_idx), len(self.robot.sensors_names[imu.type])), 1.0) # Set the gyroscope scale - gyro_imu_idx = ['Gyro' in field for field in imu.fieldnames] + gyro_imu_idx = [field.startswith('Gyro') for field in imu.fieldnames] imu_sensors_scale[gyro_imu_idx] = np.full( (sum(gyro_imu_idx), len(self.robot.sensors_names[imu.type])), SENSOR_GYRO_SCALE) # Set the accelerometer scale - accel_imu_idx = ['Accel' in field for field in imu.fieldnames] + accel_imu_idx = [field.startswith('Accel') for field in imu.fieldnames] imu_sensors_scale[accel_imu_idx] = np.full( (sum(accel_imu_idx), len(self.robot.sensors_names[imu.type])), SENSOR_ACCEL_SCALE) @@ -184,15 +185,40 @@ def _refresh_learning_spaces_scale(self): # Set the scale self.observation_scale['sensors'][imu.type] = imu_sensors_scale + # Handling of Contact sensors data scale + if contact.type in sensors_space.spaces.keys(): + total_mass = self.robot.pinocchio_data_th.mass[0] + gravity = - self.robot.pinocchio_model_th.gravity.linear[2] + total_weight = total_mass * gravity + + contact_sensors_scale = np.full( + (len(contact.fieldnames), len(self.robot.sensors_names[contact.type])), + total_weight, dtype=sensors_data[contact.type].dtype) + + self.observation_scale['sensors'][contact.type] = contact_sensors_scale + # Handling of Force sensors data scale if force.type in sensors_space.spaces.keys(): + force_sensors_scale = np.zeros( + (len(force.fieldnames), len(self.robot.sensors_names[force.type])), + dtype=sensors_data[force.type].dtype) + total_mass = self.robot.pinocchio_data_th.mass[0] gravity = - self.robot.pinocchio_model_th.gravity.linear[2] total_weight = total_mass * gravity - force_sensors_scale = np.full( - (len(force.fieldnames), len(self.robot.sensors_names[force.type])), - total_weight, dtype=sensors_data[force.type].dtype) + # Set the linear force scale + lin_force_idx = [field.startswith('F') for field in force.fieldnames] + force_sensors_scale[lin_force_idx] = np.full( + (sum(lin_force_idx), len(self.robot.sensors_names[force.type])), + total_weight) + + # Set the moment scale + # TODO: Defining the moment scale using 'total_weight' does not really make sense. + moment_idx = [field.startswith('M') for field in force.fieldnames] + force_sensors_scale[moment_idx] = np.full( + (sum(moment_idx), len(self.robot.sensors_names[force.type])), + total_weight) self.observation_scale['sensors'][force.type] = force_sensors_scale diff --git a/gym_jiminy/gym_jiminy/envs/acrobot.py b/gym_jiminy/gym_jiminy/envs/acrobot.py index de15132d4..4e81805eb 100644 --- a/gym_jiminy/gym_jiminy/envs/acrobot.py +++ b/gym_jiminy/gym_jiminy/envs/acrobot.py @@ -76,9 +76,9 @@ def __init__(self, continuous=False): # ############################### Initialize Jiminy #################################### - os.environ["JIMINY_MESH_PATH"] = \ + os.environ["JIMINY_DATA_PATH"] = \ resource_filename('gym_jiminy.envs', 'data') - urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], + urdf_path = os.path.join(os.environ["JIMINY_DATA_PATH"], "double_pendulum/double_pendulum.urdf") robot = jiminy.Robot() diff --git a/gym_jiminy/gym_jiminy/envs/cartpole.py b/gym_jiminy/gym_jiminy/envs/cartpole.py index f51803a5b..42c2025fd 100644 --- a/gym_jiminy/gym_jiminy/envs/cartpole.py +++ b/gym_jiminy/gym_jiminy/envs/cartpole.py @@ -84,9 +84,9 @@ def __init__(self, continuous=False): # ############################### Initialize Jiminy #################################### - os.environ["JIMINY_MESH_PATH"] = \ + os.environ["JIMINY_DATA_PATH"] = \ resource_filename('gym_jiminy.envs', 'data') - urdf_path = os.path.join(os.environ["JIMINY_MESH_PATH"], + urdf_path = os.path.join(os.environ["JIMINY_DATA_PATH"], "cartpole/cartpole.urdf") robot = jiminy.Robot() diff --git a/python/jiminy_py/src/jiminy_py/_pinocchio_init.py b/python/jiminy_py/src/jiminy_py/_pinocchio_init.py index fea320f84..8e9949100 100644 --- a/python/jiminy_py/src/jiminy_py/_pinocchio_init.py +++ b/python/jiminy_py/src/jiminy_py/_pinocchio_init.py @@ -8,9 +8,11 @@ from math import atan2, pi, sqrt import pinocchio as pin +import hppfcl -warnings.filterwarnings("ignore", message=("This function signature is now " - "deprecated and will be removed in future releases of Pinocchio.")) +# Disable all deprecation warnings of Pinocchio because, for now, Jiminy +# supports many releases, for which some methods have different signatures. +warnings.filterwarnings("ignore", category=pin.DeprecatedWarning) from pinocchio.rpy import npToTTuple @@ -80,3 +82,21 @@ def display(self, q): self.viewer[self.getViewerNodeName(visual, pin.GeometryType.VISUAL)].set_transform(T) pin.visualize.meshcat_visualizer.MeshcatVisualizer.display = display + +loadPrimitive_orig = pin.visualize.gepetto_visualizer.GepettoVisualizer.loadPrimitive +def loadPrimitive(self, meshName, geometry_object): + # Do not load the geometry of the ground is is not an actually geometry + # but rather a calculus artifact. + geom = geometry_object.geometry + if geometry_object.name == "ground": + return False + elif isinstance(geom, hppfcl.Convex): + pts = [npToTuple(geom.points(geom.polygons(f)[i])) + for f in range(geom.num_polygons) for i in range(3)] + self.viewer.gui.addCurve(meshName, pts, npToTuple(geometry_object.meshColor)) + self.viewer.gui.setCurveMode(meshName, "TRIANGLES") + self.viewer.gui.setLightingMode(meshName, "ON") + return True + else: + return loadPrimitive_orig(self, meshName, geometry_object) +pin.visualize.gepetto_visualizer.GepettoVisualizer.loadPrimitive = loadPrimitive diff --git a/python/jiminy_py/src/jiminy_py/engine_asynchronous.py b/python/jiminy_py/src/jiminy_py/engine_asynchronous.py index d044b2d7c..c02ffc991 100644 --- a/python/jiminy_py/src/jiminy_py/engine_asynchronous.py +++ b/python/jiminy_py/src/jiminy_py/engine_asynchronous.py @@ -198,6 +198,10 @@ def step(self, action_next=None, dt_desired=-1): @return Final state of the simulation """ + if self._state is None: + raise RuntimeError("Simulation not initialized. " + "Please call 'reset' once before calling 'step'.") + if not self.engine.is_simulation_running: flag = self.engine.start(self._state, self.use_theoretical_model) if (flag != jiminy.hresult_t.SUCCESS): @@ -277,7 +281,7 @@ def close(self): """ @brief Close the connection with the renderer. """ - if self._viewer is not None: + if hasattr(self, '_viewer') and self._viewer is not None: self._viewer.close() self._viewer = None diff --git a/python/jiminy_py/src/jiminy_py/robot.py b/python/jiminy_py/src/jiminy_py/robot.py index 273b85854..e6a8acd3d 100644 --- a/python/jiminy_py/src/jiminy_py/robot.py +++ b/python/jiminy_py/src/jiminy_py/robot.py @@ -1,10 +1,12 @@ ## @file import os +import re import toml import copy -import pathlib import logging +import pathlib +import tempfile import numpy as np import xml.etree.ElementTree as ET from collections import OrderedDict @@ -15,6 +17,7 @@ from .core import (EncoderSensor as encoder, EffortSensor as effort, + ContactSensor as contact, ForceSensor as force, ImuSensor as imu) @@ -243,6 +246,50 @@ def generate_hardware_description_file( toml.dump(hardware_info, f) +def fix_urdf_mesh_path(urdf_path, mesh_root_path, output_root_path=None): + """ + @brief Generate an URDF with updated mesh paths. + + @param[in] urdf_path Full path of the URDF file. + @param[in] mesh_root_path Root path of the meshes. + @param[in] output_root_path Root directory of the fixed URDF file. + Optional: temporary directory by default. + + @return Full path of the fixed URDF file. + """ + # Extract all the mesh path that are not package path, continue if any + with open(urdf_path, 'r') as urdf_file: + urdf_contents = urdf_file.read() + pathlists = [ + filename + for filename in re.findall(' static enable_if_t::value - || std::is_same::value, void> + || std::is_same::value, void> visit(PyClass& cl) { visitAbstract(cl); @@ -828,6 +828,25 @@ namespace python ; } + template + static enable_if_t::value, void> + visit(PyClass& cl) + { + visitAbstract(cl); + visitBasicSensors(cl); + + cl + .add_property("frame_name", bp::make_function(&TSensor::getFrameName, + bp::return_value_policy())) + .add_property("frame_idx", bp::make_function(&TSensor::getFrameIdx, + bp::return_value_policy())) + .add_property("body_name", bp::make_function(&TSensor::getBodyName, + bp::return_value_policy())) + .add_property("joint_idx", bp::make_function(&TSensor::getJointIdx, + bp::return_value_policy())) + ; + } + template static enable_if_t::value, void> visit(PyClass& cl) @@ -896,6 +915,11 @@ namespace python boost::noncopyable>("ImuSensor", bp::init()) .def(PySensorVisitor()); + bp::class_, + std::shared_ptr, + boost::noncopyable>("ContactSensor", bp::init()) + .def(PySensorVisitor()); + bp::class_, std::shared_ptr, boost::noncopyable>("ForceSensor", bp::init()) @@ -930,6 +954,11 @@ namespace python (bp::arg("self"), "frame_name", "parent_body_name", "frame_placement")) .def("remove_frame", &Model::removeFrame, (bp::arg("self"), "frame_name")) + .def("add_collision_bodies", &PyModelVisitor::addCollisionBodies, + (bp::arg("self"), + bp::arg("link_names") = std::vector())) + .def("remove_collision_bodies", &PyModelVisitor::removeCollisionBodies, + (bp::arg("self"), "link_names")) .def("add_contact_points", &PyModelVisitor::addContactPoints, (bp::arg("self"), bp::arg("frame_names") = std::vector())) @@ -949,6 +978,10 @@ namespace python bp::return_internal_reference<>())) .add_property("pinocchio_data_th", bp::make_getter(&Model::pncDataRigidOrig_, bp::return_internal_reference<>())) + .add_property("collision_model", bp::make_getter(&Model::pncGeometryModel_, + bp::return_internal_reference<>())) + .add_property("collision_data", bp::make_function(&PyModelVisitor::getGeometryData, + bp::return_internal_reference<>())) .add_property("is_initialized", bp::make_function(&Model::getIsInitialized, bp::return_value_policy())) @@ -964,6 +997,8 @@ namespace python .add_property("nx", bp::make_function(&Model::nx, bp::return_value_policy())) + .add_property("collision_bodies_names", bp::make_function(&Model::getCollisionBodiesNames, + bp::return_value_policy())) .add_property("contact_frames_names", bp::make_function(&Model::getContactFramesNames, bp::return_value_policy())) .add_property("contact_frames_idx", bp::make_function(&Model::getContactFramesIdx, @@ -997,6 +1032,25 @@ namespace python ; } + static pinocchio::GeometryData & getGeometryData(Model & self) + { + return *(self.pncGeometryData_); + } + + static hresult_t addCollisionBodies(Model & self, + bp::list const & linkNamesPy) + { + auto linkNames = convertFromPython >(linkNamesPy); + return self.addCollisionBodies(linkNames); + } + + static hresult_t removeCollisionBodies(Model & self, + bp::list const & linkNamesPy) + { + auto linkNames = convertFromPython >(linkNamesPy); + return self.removeCollisionBodies(linkNames); + } + static hresult_t addContactPoints(Model & self, bp::list const & frameNamesPy) { diff --git a/unit_py/test_point_mass.py b/unit_py/test_point_mass.py index 57f6607b5..45fd175cd 100644 --- a/unit_py/test_point_mass.py +++ b/unit_py/test_point_mass.py @@ -30,7 +30,7 @@ def setUp(self): self.robot = jiminy.Robot() self.robot.initialize(urdf_path, has_freeflyer=True) self.robot.add_contact_points(['MassBody']) - force_sensor = jiminy.ForceSensor('MassBody') + force_sensor = jiminy.ContactSensor('MassBody') self.robot.attach_sensor(force_sensor) force_sensor.initialize('MassBody') @@ -118,7 +118,7 @@ def test_force_sensor(self): idx = self.robot.pinocchio_model.getFrameId("MassBody") def computeCommand(t, q, v, sensors_data, u): # Verify sensor data. - f = Force(sensors_data[jiminy.ForceSensor.type, "MassBody"], np.zeros(3)) + f = Force(sensors_data[jiminy.ContactSensor.type, "MassBody"], np.zeros(3)) f_joint_sensor = self.robot.pinocchio_model.frames[idx].placement * f f_jiminy = engine.system_state.f_external[self.robot.pinocchio_model.frames[idx].parent] self.assertTrue(np.allclose(f_joint_sensor.vector, f_jiminy.vector, atol=TOLERANCE))